# Need help regarding development of Extended Kalman Filter for sensor-data fusion of odometry and IMU data

I'm trying to develop an Extended Kalman Filter (EKF) for the positioning of a wheeled vehicle. I have a 'Baron' robot frame with 4 static wheels, all driven by a motor. On the 2 rear wheels I have an encoder. I want to fuse this odometry data with data from an 'MPU9150' 9 DOF IMU.

This is my mathlab code for the what I call 'medium-size' EKF. This uses data from encoders, accelerometer in x and y axis and gyroscope z-axis.

# Medium-size EKF

Inputs: x: "a priori" state estimate vector (8x1)
t: sampling time [s]
P: "a priori" estimated state covariance vector (8x8)
z: current measurement vector (5x1) (encoder left; encoder right; x-acceleration, y-acceleration, z-axis gyroscope)
Output: x: "a posteriori" state estimate vector (8x1)
P: "a posteriori" state covariance vector (8x8)

State vector x: a 8x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame \\ \ddot x \rightarrow Acceleration In X-direction Global Frame \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\ \ddot y \rightarrow Acceleration In Y-direction Global Frame \\ \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z:
a 5x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \\ \dot \theta_z \rightarrow GyroscopeMeasurementInZ-axisVehicleFrame \\ a_x \rightarrow AccelerometerMeasurementX-axisVehicleFrame \\ a_y \rightarrow AccelerometerMeasurementY-axisVehicleFrame \end {bmatrix}$

function [x,P] = moodieEKFmedium(x,t,P,z,sigma_ax,sigma_ay,sigma_atau,sigma_odo,sigma_acc,sigma_gyro)

% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 8 && columns ~= 1)
error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 5 && columns ~= 1)
error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(8,6);
Q(3,3) = sigma_ax;
Q(6,6) = sigma_ay;
Q(8,8) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(8);
[dfdx(1,2),dfdx(2,3),dfdx(4,5),dfdx(5,6),dfdx(7,8)] = deal(t);
[dfdx(1,3),dfdx(4,6)] = deal((t^2)/2);

dfda = zeros(6,6);
[dfda(3,3),dfda(6,6),dfda(8,8)] = deal(1);

dhdn = eye(5,5);

R = zeros(5,5);
[R(1,1),R(2,2)] = deal(sigma_odo);
R(3,3) = sigma_gyro;
[R(4,4),R(5,5)] = deal(sigma_acc);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(8,1);
xtemp(1) = x(1) + t*x(2)+((t^2)/2)*x(3);
xtemp(2) = x(2) + t*x(3);
u1 = normrnd(0,sigma_ax);
xtemp(3) = x(3) + u1;

xtemp(4) = x(4) + t*x(5)+((t^2)/2)*x(6);
xtemp(5) = x(5) + t*x(6);
u2 = normrnd(0,sigma_ay);
xtemp(6) = x(6) + u2;

xtemp(7) = x(7) + t*x(8);
u3 = normrnd(0,sigma_atau);
xtemp(8) = x(8) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(5,8);
if(x(2) == 0 && x(5) == 0)
[dhdx(1,2),dhdx(2,2)] = deal(0);
[dhdx(1,4),dhdx(2,4)] = deal(0);
else
[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(5)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(5)/sqrt(x(2)^2+x(5)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

dhdx(4,3) = sin(x(7));
dhdx(4,6) = -cos(x(7));
dhdx(4,7) = (x(3)*cos(x(7)))+(x(6)*sin(x(7)));

dhdx(5,3) = cos(x(7));
dhdx(5,6) = sin(x(7));
dhdx(5,7) = (-x(3)*sin(x(7)))+(x(6)*cos(x(7)));

Kk = P * transpose(dhdx) * (dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1)

% Update state
H = zeros(5,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;
n3 = normrnd(0,sigma_gyro);
H(3)= x(8) + n3;
n4 = normrnd(0,sigma_acc);
H(4)=(x(3)*sin(x(7))-(x(6)*cos(x(7))))+n4;
n5 = normrnd(0,sigma_acc);
H(5)=(x(3)*cos(x(7))+(x(6)*sin(x(7))))+n5;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(8)-Kk*dhdx)*P;


end

This is the filter in schematic :

These are the state transition equations I use : $$\ x_{t+1} = x_{t} + T \cdot \dot x_{t} + \frac{T^{2}}{2} \cdot \ddot x_{t}$$ $$\ \dot x_{t+1} = \dot x_{t} + T \cdot \ddot x_{t}$$ $$\ \ddot x_{t+1} = \ddot x_{t} + u_{1}$$ $$\ y_{t+1} = y_{t} + T \cdot \dot y_{t} + \frac{T^{2}}{2} \cdot \ddot y_{t}$$ $$\ \dot y_{t+1} = \dot y_{t} + T \cdot \ddot y_{t}$$ $$\ \ddot y_{t+1} = \ddot y_{t} + u_{2}$$ $$\ \dot \theta_{t+1} = \dot \theta_{t} + T \cdot \ddot \theta_{t}$$ $$\ \ddot \theta_{t+1} = \ddot \theta_{t} + u_{3}$$

These are the observation equations I use :

$$\ \eta_{left} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} + \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{1}$$ $$\ \eta_{right} = \frac{T \cdot n_{0}}{2 \cdot \pi \cdot r} \cdot \sqrt{\dot x^{2} + \dot y^{2}} - \frac{T \cdot n_{0} \cdot b}{2 \cdot \pi \cdot r} \cdot \dot \theta + n_{2}$$ $$\ \dot \theta_{z} = \dot \theta + n_{3}$$ $$\ a_{x} = \ddot x \sin \theta - \ddot y \cos \theta + n_{4}$$ $$\ a_{y} = \ddot x \cos \theta + \ddot y \sin \theta + n_{5}$$

# Small-size EKF

I wanted to test my filter, therefore I started with a smaller one, in which I only give the odometry measurements as input. This because I know that if I always receive the same amount of pulses on the left and right encoder, than my vehicle should be driving a straight line.

Inputs: x: "a priori" state estimate vector (6x1)
t: sampling time [s]
P: "a priori" estimated state covariance vector (6x6)
z: current measurement vector (2x1) (encoder left; encoder right)
Output: x: "a posteriori" state estimate vector (6x1)
P: "a posteriori" state covariance vector (6x6)

State vector x: a 6x1 vector $\begin{bmatrix} x \rightarrow X-Position In Global Frame \\ \dot x \rightarrow Speed In X-direction Global Frame \\ y \rightarrow Y-Position In Global Frame \\ \dot y \rightarrow Speed In Y-direction Global Frame \\ \theta \rightarrow Vehicle Angle In Global Frame \\ \dot \theta \rightarrow Angular Speed Of The Vehicle \end {bmatrix}$

Measurement vector z: a 2x1 vector $\begin{bmatrix} \eta_{left} \rightarrow Wheelspeed Pulses On Left Wheel \\ \eta_{right} \rightarrow Wheelspeed Pulses On Right Wheel \end {bmatrix}$

% Check if input matrixes are of correct size
[rows columns] = size(x);
if (rows ~= 6 && columns ~= 1)
error('Input vector size incorrect')
end
[rows columns] = size(z);
if (rows ~= 2 && columns ~= 1)
error('Input data vector size incorrect')
end

% Constants
n0 = 16;
r = 30;
b = 50;

Q = zeros(6,6);
Q(2,2) = sigma_ax;
Q(4,4) = sigma_ay;
Q(6,6) = sigma_atau;
%[Q(1,8),Q(3,6),Q(6,3)] = deal(small);

dfdx = eye(6);
[dfdx(1,2),dfdx(3,4),dfdx(5,6)] = deal(t);

dfda = zeros(6,6);
[dfda(2,2),dfda(4,4),dfda(6,6)] = deal(1);

dhdn = eye(2,2);

R = zeros(2,2);
[R(1,1),R(2,2)] = deal(sigma_odo);
%[R(2,1),R(1,2)] = deal(small);

% Predict next state
% xk = f(xk-1)
xtemp = zeros(6,1);
xtemp(1) = x(1) + t*x(2);
u1 = normrnd(0,sigma_ax);
xtemp(2) = x(2) + u1;
xtemp(3) = x(3) + t*x(4);
u2 = normrnd(0,sigma_ay);
xtemp(4) = x(4) + u2;
xtemp(5) = x(5) + t*x(6);
u3 = normrnd(0,sigma_atau);
xtemp(6) = x(6) + u3;

x = xtemp

% Predict next state covariance
% Pk = dfdx * Pk-1 * transpose(dfdx) + dfda * Q * transpose(dfda)
P = dfdx * P * transpose(dfdx) + dfda * Q * transpose(dfda);

% Calculate Kalman gain
% Kk = P * transpose(dhdx) [dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn)]^-1
dhdx = zeros(2,6);
if((x(2) < 10^(-6)) && (x(4)< 10^(-6)))
[dhdx(1,2),dhdx(2,2)] = deal((t*n0)/(2*pi*r));
[dhdx(1,4),dhdx(2,4)] = deal((t*n0)/(2*pi*r));
else
[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
end
%[dhdx(1,2),dhdx(2,2)] = deal(((t*n0)/(2*pi*r))*(x(2)/sqrt(x(2)^2+x(4)^2)));
%[dhdx(1,4),dhdx(2,4)] = deal(((t*n0)/(2*pi*r))*(x(4)/sqrt(x(2)^2+x(4)^2)));
dhdx(1,6) = (t*n0*b)/(2*pi*r);
dhdx(2,6) = -(t*n0*b)/(2*pi*r);

Kk = P * transpose(dhdx) * ((dhdx * P * transpose(dhdx) + dhdn * R * transpose(dhdn))^(-1))

% Update state
H = zeros(2,1);
n1 = normrnd(0,sigma_odo);
H(1) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))+(((t*n0*b)/(2*pi*r))*x(6)) + n1;
n2 = normrnd(0,sigma_odo);
H(2) = (((t*n0)/(2*pi*r))*sqrt(x(2)^2+x(4)^2))-(((t*n0*b)/(2*pi*r))*x(6)) + n2;

x = x + Kk*(z-H)

% Update state covariance
P = (eye(6)-Kk*dhdx)*P;


end

## Odometry observation equations

If you would wonder how I come to the observation equations for the odometry data: $\ V_{vl} = V{c} + \dot \theta \cdot b \rightarrow V_{vl} = \sqrt{ \dot x^{2} + \dot y^{2}} + \dot \theta \cdot b$

# Problem

If I try the small-size EKF, using a Matlab user interface, it does seem to drive a straight line, but not under a heading of 0° like I would expect. Eventhough I start with a state vector of $\ x= \begin{bmatrix}0\\0\\0\\0\\0\\0\end{bmatrix}$ meaning starting at position [0,0] in the global coordinate frame, with speed and acceleration of zero and under an angle of 0°.

In the top right corner you can see the measurement data which I give as input, which is 5 wheelspeed counts on every wheel, every sampling period. (Simulating straight driving vehicle) In the top left corner you see a plot of the X and Y coordinate (from state vector) at the end of one predict+update cycle of the filter, labeled with the timecycle. Bottom left corner is a plot of the angle in the state vector. You see that after 12 cycles the angle is still almost 0° like I would expect.

Could anyone please provide some insights in to what could be wrong here?

# Solutions I've been thinking on

1. I could use the 'odometry motion model' like explained in this question. The difference is that the odometry data is inserted in the predict step of the filter. But if I would do this, I see 2 problems: 1) I don't see how to make a small-size version of this for testing purposes, because I don't know which measurements to add in the update-step and 2) for the medium-size version I don't know how to make the observation equations as the state vector doesn't imply velocity and acceleration.

2. I could use the 'odometry motion model' and in the update step use the Euler-angle, which can be linked to $\ \theta$. This Euler-angle I can obtain from the Digital Motion Processor (DMP), implemented in the IMU. Then it is no problem that angular velocity is not in the state matrix. But than I still have a problem with the acceleration observation equations.

• The assumption behind the odometery solution you linked is that odometery pulses are not a measurement of state, but of control inputs. It's a design choice. I'm amazed you coded up the whole filter before testing it. – Josh Vander Hook Aug 3 '16 at 14:49

Adding to the above, my favorite way to debug a misbehaving filter is to isolate each step.

1. Make sure your prediction step works before correcting it. Your bot should drive straight right with 0,0,0 as a starting state and constant vel. Otherwise, your measurements are correcting your model, not sensor noise

2. Remove the measurement step and feed residuals by hand. A zero vector residual should produce the same state you started with (also great for verifying your state transitions, which may be wrong as above). If a zero residual produces a non zero change, your K matrix is usually wrong, and usually because your state transition Jacobian is wonky.

3. If prediction and residuals make sense, then check your measurements separately as well. Feed states and Odom measurements into it to make sure the resulting residuals match what you expect. E.g. 1,1 goes straight, 2,1 right curve, 1,2 left, 1,-1 adjusts only heading, etc.

4. If all that works, and your result still doesn't make sense when you put it together or display it, then the plumbing or display is the problem. I've wasted hours redefining all the equations because of a display bug.

In your case I'm not clear about the odometery equations. You seem to be controlling vel in x,y,theta, but use odometery to update that velocity. Is it not the case that you are actually issuing wheel velocity commands?

You should first validate your filter is working before second-guessing your modelling choices. But I agree both those filters look OK (although I did not double check all the maths) and both of your suggested changes should also work.

There's a process I used to find errors in KF's, which is in no way comprehensive:

1. Plot the measurement residuals at every update.
• They should start large and decrease asymptotically to the noise floor.
• Your residuals should fall within the gates for the usual Gaussian tolerance intervals (68, 95 and 99.7)% of residuals fall within ($1\sigma$, $2\sigma$, $3\sigma$) respectively.
• If this is not the case then your filter is not working properly which is usually due to errors in the actual KF implementation, or a measurement model.
• If it is converging but the quality is poorer than expected, it could simply be a covariance tuning problem
2. Validate your measurement models and update step by disabling your predict stage
• For each predict step, set $x_{k} = x_{k-1}$ but continue to grow $P$
• If the residuals aren't decreasing, your measurement models are probably wrong (since your models make the angle and position observable)
3. Enable your Predict stage again and now validate your process model in the same way

For a comprehensive set of lecture notes, see ACFR's KF Tutorial, specifically after slide 121.