Estimating Gyroscope Bias
MEMs gyroscopes never read zero when they are stationary. There is always a small measurement offset. This gyro bias is often related to temperature, so warming up the gyro with a heat gun will cause the bias to change – sometimes quite dramatically.
The most common way to remove gyro bias is to measure the gyroscopes shortly after power-up. This measurement is subtracted from all subsequent measurements. This generally works fine if the gyroscopes are stationary when the initial measurement is taken and when the temperature is stable. It also doesn’t account for the rotation of the earth, which would slightly affect the bias measurement. But it’s a very good start.
Magnetometers and accelerometers can be used to estimate and continuously update gyro bias. These sensors are used to create a quaternion that roughly represents orientation. Differentiating these quaternions produces a very rough estimation of angular velocity. Subtracting noisy accelerometer/magnetometer angular velocity from gyroscopes angular velocity gives a very noisy estimation of gyro bias. Fortunately, gyro bias doesn’t change very quickly, so this noisy gyro bias estimate can be heavily low pass filtered without too much worry.
//create acceleration quaternion
Vector<3> down = acceleration_dynamics(state, dt);
Vector<3> east = down.cross(mag_vec);
Vector<3> north = east.cross(down);
//find difference between accel_q and last accel_q
Quaternion q_diff = last_accel_q.conjugate() * accel_q;
last_accel_q = accel_q;
//turn q_diff into an angular velocity and subtract it from ang_vel (gyroscope reading)
//this creates a noisy gyro bias estimate
Vector<3> noisy_gyro_bias = ang_vel-q_diff.toAngularVelocity(dt);
//heavily low pass filter the noisy gyro bias
gyro_bias = gyro_bias - (0.000001 * (gyro_bias - noisy_gyro_bias));