image thumbnail

Learning the Kalman-Bucy Filter in Simulink

by

 

27 Jan 2008 (Updated )

A Simulink model to learn the continuous-time Kalman-Bucy Filter

Learning the Kalman-Bucy Filter in Simulink

Learning the Kalman-Bucy Filter in Simulink

Examples to run the Simulink model kalmanbucy in the command window. By Yi Cao at Cranfield University on 28 January 2008

Contents

Example 1: Ship position esitimate

x0 = [0;2];
A = [0 1;0 0];
B = [0;1];
C = [1 0];
D = 0;
Q = eye(2);
R = 1;
% Kalman-Bucy filter with initial estimation error
x1 = [0;1];
A1 = A;
B1 = B;
C1 = C;
D1 = D;
Q1 = Q;
R1 = R;
P1 = 10*eye(2);
% simulation configuration
tspan = 0:0.1:100;
u = sin(tspan'/2+randn(1001,1));
% u = randn(1001,1);
[t,x,y1,y2,y3,y4]=sim('kalmanbucy',tspan,[],[tspan' u]);

Xtrue = y1(:,1);    %actual position
Vtrue = y1(:,2);    %actiual velocity
Z = y2;             %measured position
X = y3;   %Kalman filter output

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%% Position analysis %%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
figure;
set(gcf,'Position', [100 100 600 800]);
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','best');

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

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

% 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','best');

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

A = [-1.4576   -0.3369    1.0503    3.7815
      0.0979   -0.5998    0.2727    0.4077
      1.7212    0.1711   -4.5537    1.1045
     -3.5418   -0.3277   -1.7419   -0.9578];
B = [-0.1072         0
           0    0.1000
     -0.9640         0
           0    0.1500];
C = [0.1685   -0.9595   -0.0755   -0.3771];
D = [0 0];
% process noise variance
Q=diag([0.5^2 0.2^2 0.3^2 0.5^2]);
% measurment noise variance
R=1;
% initial state
x0 = [-1.7073
       0.2279
       0.6856
      -0.6368];
% Kalman-Bucy filter setting
% The same model
A1 =A;
B1 = B;
C1 = C;
D1 = D;
Q1 = Q;
R1 = R;
% zeros initial state estimate
x1 = zeros(4,1);
% initial covariance estimate
P1 = 10*eye(4);
% time span
tspan=0:0.1:100;
% input
u = randn(11,2);
% run the simulation
[t,x,y1,y2,y3,y4]=sim('kalmanbucy',tspan,[],[(0:10:100)' u]);
% Display results
figure
set(gcf,'Position',[100 100 500 700])
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')

Contact us