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

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;

%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

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');

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 |

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 |

Third example, only velocity and acceleration sensor:

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 |

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

## 2 comments:

Good Tutorial! Thanks!

Is there any tutorial about extended Kalman Filter?

Thanks for the compliments. However, I am sorry to say currently I don't have the Extended KF informations

Post a Comment