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
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.
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.
