Learning the Kalman Filter in Simulink Examples

Examples to run the Simulink model kalmanfilter. By Yi Cao at Cranfield University on 25 January 2008

Contents

Example 1, Automobile Voltimeter

Modified from Michael C. Kleder's "Learning the Kalman Filter"

Define the system as a constant of 12 volts:

But the Kalman filter uses different model.

This example shows the Kalman filter has certain robustness to model uncertainty.

A = 0; B = 1;   % this is original definition x(k+1) = 12 + w
A1 = 1; B1 = 0; % this is original definition for Kalman filter
% Define a process noise (stdev) of 2 volts as the car operates:
Q = 2^2; % variance, hence stdev^2
Q1 = Q;
% Define the voltimeter to measure the voltage itself:
% y(k+1) = x(k+1) + v, v ~ N(0,2^2)
C = 1; D = 0;
C1 = C; D1 = D;
% Define a measurement error (stdev) of 2 volts:
R = 2^2;
R1 = R;
% Define the system input (control) functions:
u = 12; % for the constant
% Initial state, 12 volts
x0 = 12;
x1 = x0;    %for Kalman filter
% Initial covariance as C*R*C'
P1 = 2^2;
% Simulation time span
tspan = [0 100];
[t,x,y] = sim('kalmanfilter',tspan,[],[0 u]);
figure
hold on
grid on
% plot measurement data:
hz=plot(t,y(:,2),'r.','Linewidth',2);
% plot a-posteriori state estimates:
hk=plot(t,y(:,3),'b-','Linewidth',2);
% plot true state
ht=plot(t,y(:,1),'g-','Linewidth',2);
legend([hz hk ht],'observations','Kalman output','true voltage',0)
title('Automobile Voltimeter Example')
hold off

Example 2, Predict the position and velocity of a moving train,

Modified from "An Intuitive Introduction to Kalman Filter" by Alex Blekhman.

The train is initially located at the point x = 0 and moves along the X axis with velocity varying around a constant speed 10m/sec. The motion of the train can be described by a set of differential equations:

where x_1 is the position and x_2 is the velocity, w the process noise due to road conditions, wind etc.

Problem: using the position measurement to estimate actual velocity.

Approach: We measure (sample) the position of the train every dt = 0.1 seconds. But, because of imperfect apparature, weather etc., our measurements are noisy, so the instantaneous velocity, derived from 2 consecutive position measurements (remember, we measure only position) is innacurate. We will use Kalman filter as we need an accurate and smooth estimate for the velocity in order to predict train's position in the future.

Model: The state space equation after discretization with sampling time dt

The measurment model is

dt = 0.1;
A = expm([0 1;0  0]*dt);
B = [0;0];
Q = diag([0;1]);
C = [1 0];
D = 0;
R = 1;
u = 0;
x0 = [0;10];

% For Kalman filter, we use the same model
A1 = A;
B1 = B;
C1 = C;
D1 = D;
Q1 = Q;
R1 = R;
x1 = [0;5];     %but with different initial state estimate
P1 = eye(2);

% Run the simulation for 100 smaples
tspan = [0 200];
[t,x,y1,y2,y3,y4] = sim('kalmanfilter',tspan,[],[0 u]);

Xtrue = y1(:,1);    %actual position
Vtrue = y1(:,2);    %actiual velocity
z = y2;             %measured position
X = y3;             %Kalman filter output
t = t*dt;           % actual time

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Position analysis %%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
subplot(211)
plot(t,Xtrue,'g',t,z,'c',t,X(:,1),'m','linewidth',2);
title('Position estimation results');
xlabel('Time (s)');
ylabel('Position (m)');
legend('True position','Measurements','Kalman estimated displacement','Location','NorthWest');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Velocity analysis %%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% The instantaneous velocity as derived from 2 consecutive position
% measurements
InstantV = [10;diff(z)/dt];

% The instantaneous velocity as derived from running average with a window
% of 5 samples from instantaneous velocity
WindowSize = 5;
InstantVAverage = filter(ones(1,WindowSize)/WindowSize,1,InstantV);

% figure;
subplot(212)
plot(t,InstantV,'g',t,InstantVAverage,'c',t,Vtrue,'m',t,X(:,2),'k','linewidth',2);
title('Velocity estimation results');
xlabel('Time (s)');
ylabel('Velocity (m/s)');
legend('Estimated velocity by raw consecutive samples','Estimated velocity by running average','True velocity','Estimated velocity by Kalman filter','Location','NorthWest');

set(gcf,'Position', [100 100 600 800]);

Example 3: A 2-input 2-output 4-state system

dt = 0.1;
A = [0.8110   -0.0348    0.0499    0.3313
     0.0038    0.9412    0.0184    0.0399
     0.1094    0.0094    0.6319    0.1080
    -0.3186   -0.0254   -0.1446    0.8391];
B = [-0.0130    0.0024
     -0.0011    0.0100
     -0.0781    0.0009
      0.0092    0.0138];
C = [0.1685   -0.9595   -0.0755   -0.3771
     0.6664    0.0835    0.6260    0.6609];
D = [0 0;0 0];
% process noise variance
Q=diag([0.5^2 0.2^2 0.3^2 0.5^2]);
% measurment noise variance
R=eye(2);
% random initial state
x0 = randn(4,1);
% Kalman filter set up
% The same model
A1 =A;
B1 = B;
C1 = C;
D1 = D;
Q1 = Q;
R1 = R;
% However, zeros initial state
x1 = zeros(4,1);
% Initial state covariance
P1 = 10*eye(4);
% Simulation set up
% time span 100 samples
tspan = [0 1000];
% random input change every 100 samples
u = [(0:100:1000)' randn(11,2)];
% simulation
[t,x,y1,y2,y3,y4]=sim('kalmanfilter',tspan,[],u);
% plot results
% Display results
t = t*dt;
figure
set(gcf,'Position',[100 100 600 800])
for k=1:4
    subplot(4,1,k)
    plot(t,y1(:,k),'b',t,y3(:,k),'r','linewidth',2);
    legend('Actual state','Estimated state','Location','best')
    title(sprintf('state %i',k))
end
xlabel('time, s')

Published with MATLAB® 7.5