## Friday, February 24, 2012

### Kalman Filtering for Dummies - Part IV

Here goes the coding in Matlab for the position-velocity-acceleration system:

First Example, full observability (all sensors are available).

clear all
clc

%timeline and sampling period, Ts
Ts = 0.01;
t = 0:Ts:10;

%true parameters position, velocity and acceleration
x = cos(t);
dx = -sin(t);
ddx = -cos(t);

%add noise to the "measurements"
for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

%Kalman filter initialization
%initialize the states, here, the initial value is incorrect
q = [0; 0; 1];
%initialize the estimate covariance with nonzero values.
P = eye(3);

%F and H are the model parameters where
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];
H = [1 0 0; 0 1 0; 0 0 1];

%Q and R both the process and measurement noise covariances
Q = [ 0 0 0; 0 0 0; 0 0 1];
R = [1 0 0; 0 1 0; 0 0 1];

%Kalman iterate
for i = 1:length(t)-1
%predict stage
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%this is where the measurement is corrupted with noise
z=[x_noise(i);dx_noise(i);ddx_noise(i)];

%update stage
y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

%Plotting the data
subplot(311)
my_plot = plot(t,x,t,q(1,:),t,x_noise,':');
set(my_plot, 'linewidth', 1);
legend('Position','Estimate Position','Position with Noise');
subplot(312)
my_plot = plot(t,dx,t,q(2,:),t, dx_noise,':');
set(my_plot, 'linewidth', 1);
legend('Velocity','Estimate Velocity','Velocity with Noise');
subplot(313)
my_plot = plot(t,ddx,t,q(3,:),t,ddx_noise,':');
set(my_plot, 'linewidth', 1);
legend('Acceleration','Estimate Acceleration','Acceleration with noise');
 Full observer
The picture above is plotted from the Matlab file. As seen, the position and velocity reading is quite clean (green line) despite the corrupted measurement (red line) as compared to the true value (blue line). As for the acceleration, the data is not really clean because the model used does not fuse well for acceleration.

Second example, only position and velocity sensor:

Ts = 0.01;
t = 0:Ts:10;

x = cos(t);
dx = -sin(t);
ddx = -cos(t);

for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

Ts = 0.01;
q = [1; 0; -1];
P=zeros(3);

%As seen here, the H matrix is different from first example.
H = [1 0 0; 0 1 0];
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];

%process covariance
Q = [0.0001 0 0; 0 0.0001 0; 0 0 0.1];
%measurement covariance

R = [1 0 ; 0 1];

%Kalman filter interation
for i = 1:length(t)-1
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%The measurement only have two sensors
z=[x_noise(i);dx_noise(i)];

y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

 Position and velocity observer
As seen in the plot above, only the position have significantly clean data. There is a reduction in noise in the velocity measurement but the acceleration is totally noisy and unpredictable.

Third example, only velocity and acceleration sensor:

Ts = 0.01;
t = 0:Ts:10;

x = cos(t);
dx = -sin(t);
ddx = -cos(t);

for i=1:length(t)
x_noise(i) = x(i) + (randn());
dx_noise(i) = dx(i) + (randn());
ddx_noise(i) = ddx(i) + (randn());
end

q = [0; 0; 1];
P=eye(3);

%The H matrix is different from first two examples
H = [0 1 0; 0 0 1];
F = [1 Ts 0.5*Ts*Ts; 0 1 Ts; 0 0 1];

%As seen from all example, the Q can take on any value and still works fine.
Q = [0.25*power(Ts,4) 0.5*power(Ts,3) 0.5*Ts;
0.5*power(Ts,3) power(Ts,2) Ts;
0.5*power(Ts,2) Ts 1];
R = [1 0 ; 0 1 ];

for i = 1:length(t)-1
q(:,i+1) = F * q(:,i);
P=F*P*transpose(F) + Q;

%Only velocity and acceleration measurement
z=[dx_noise(i);ddx_noise(i)];

y=z-H*q(:,i+1);
S=H*P*transpose(H)+R;
K=P*transpose(H)/S;
q(:,i+1) = q(:,i+1) + K*y;
P=(eye(3)-K*H)*P;
end

 Velocity and acceleration sensors
From picture above, only velocity gives clean data while the position sensor is off. This is due to the lack of position sensor, and this can be avoided if the position initial value is exactly known.

In a nutshell:
From these three examples, all the F matrix is the same because the internal state variables are interconnected similarly. The H is different because of the usage of different sensor sets.

In part V, I will show other examples where Kalman filter can be used, including an example to obtain the pitch from accelerometer and gyroscope and an example for sensors in navigation.

Part I
Part II
Part III
Part IV
Part V
Part VI