differential robot yaw estimation using kalman filter

Question

Hello i am building a differential drive robot which is equipped with quadrature encoders on both of the motors. My aim is to be able to predict the heading (yaw angle) of the robot using a kalman filter. I am using an MPU 9150 imu. As of now m just interested in yaw angle and not the roll/pitch. As i understand, i will be needing the z-axis of gyro to be fused with the magnetometer data in order to properly estimate the heading angle. My problem is how do i implement the bias and covariance required for the kalman filter to work. Gyroscope would be my process and magnetometer data would be my update step yeah?. From the datasheet i have found the Angular random walk of my gyroscope to be 0.3 degrees/second for 10 Hz motion bandwidth and a constant bias of 20 degrees/second at room temperature. If i am not mistaken i should include the bias in my state prediction equation?. Also how do i get the covariance matrix Q.

Answer 1

There are indeed many ways to implement an (Extended) Kalman Filter for IMU data. You might or not include the bias, depending if you want also to calibrate the sensor within the KF framework itself. w(t) is the noise and you do not make it, it is just for demonstration there and you might use it to implement fake sensor data to test your algorithm. Just an general advice, if you wrap the angles between [pi, -pi] or other way, remember to compute always the minimum difference between the two angles (when you are computing the innovation vector) or you will get some nasty surprises.

Answer 2

I think the bias gets removed before Kalman, ie the measured Heading Change, which is the result of magnetometer/accelerometer combination minus bias.

As such both are part of the update step. The only variance would be the random walk, and you can simplify Kalman significantly by not using matrix, if you are only interested in 2D.

gyroMeasured = meanGyroZValue(5,4) - GyroOffset;

// predict
tachoPredicted = tachoUpdated + dt * gyroMeasured;
varianceFilterPredicted = varianceFilterUpdated + varianceGyro;

// heading must be between 0 and 2*PI
tachoPredicted = NormalizeAngle(tachoPredicted);

// Kalman gain
kalmanGain = varianceFilterPredicted /
(varianceFilterPredicted + varianceTacho);

// update
tachoUpdated = tachoPredicted + kalmanGain * (tachoMeasured - tachoPredicted);
varianceFilterUpdated = varianceFilterPredicted +
kalmanGain * (varianceTacho - varianceFilterPredicted);

RobotH = NormalizeAngle(tachoUpdated);

I think this is close to correct, but I am new at Kalman.