# differential robot yaw estimation using kalman filter

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.

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.

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.

• Actually i am also new to kalman and this is my very first implementation of a kalman filter. Their are lots of articles and journals which describe the theory but when it comes down to the nitty gritty of implementation it is all very application specific. I guess that is where all the fun lies. Coming back to my topic, m sorry for having mislead you into thinking that i'll be fusing my gyro data directly with the encoder. I am supposed to first estimate the yaw angle and then compare it with my encoder. From the state equation (Xt = AX(t-1) + Bu(t) + w(t)). what would my W(t) be ? – Asusrog Jan 25 '15 at 7:36
• Welcome to Robotics @Asusrog . On stack exchange, comments are not intended for discussions, for that use Robotics Chat, when you have chat privileges. Comments are for helping to improve questions and answers, and are distracting, so we try to keep them to a minimum. If you have a related question, it can be asked as a new question (ideally referencing this one). Comments should be considered ephemeral, any comment which is too chatty or no longer actively helps to improve an answer may be deleted at any time to tidy up a post. – Mark Booth May 11 '17 at 9:26