KALMDEMO demonstrates MATLAB's ability to do Kalman filtering.
Both a steady state filter and a time varying filter are designed
and simulated below.
Press any key to continue ...
% Given the following discrete plant
A =
1.1269 -0.4940 0.1129
1.0000 0 0
0 1.0000 0
B =
-0.3832
0.5919
0.5191
% Design a Kalman filter to estimate the output y based on the
% noisy measurements yv[n] = C x[n] + v[n]
pause % Press any key to continue ...
% Let's design a steady-state Kalman filter using the
% function KALMAN. This function determines the optimal
% steady-state filter gain M based on the process noise
% covariance Q and the sensor noise covariance R.
% First specify the plant + noise model.
% CAUTION: set the sample time to -1 to mark plant
% as discrete
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');
% Enter the process noise covariance:
Q = input('Enter a number greater than zero (e.g. 1): ');
Enter a number greater than zero (e.g. 1):
% Enter the process noise covariance:
Q = input('Enter a number greater than zero (e.g. 1): ');
Enter a number greater than zero (e.g. 1): % Let's design a steady-state Kalman filter using the
% function KALMAN. This function determines the optimal
% steady-state filter gain M based on the process noise
% covariance Q and the sensor noise covariance R.
% First specify the plant + noise model.
% CAUTION: set the sample time to -1 to mark plant
% as discrete
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');
% Enter the process noise covariance:
Q = input('Enter a number greater than zero (e.g. 1): ');
Enter a number greater than zero (e.g. 1):
??? Error: Expected a variable, function, or constant, found "end of line".
Enter a number greater than zero (e.g. 1):
nter a number greater than zero (e.g. 1):
echo off
Q = 1
% Enter the sensor noise covariance:
R = input('Enter a number greater than zero (e.g. 1): ');
Enter a number greater than zero (e.g. 1):
% Enter the sensor noise covariance:
R = input('Enter a number greater than zero (e.g. 1): ');
Enter a number greater than zero (e.g. 1):
echo off
R = 1
% Now design the steady-state Kalman filter with equations
%
% Time update: x[n+1|n] = Ax[n|n-1] + Bu[n]
%
% Mesurement update: x[n|n] = x[n|n-1] + M (yv[n] - Cx[n|n-1])
%
% where M = optimal innovation gain
[kalmf,L,P,M,Z] = kalman(Plant,Q,R);
pause % Press any key to continue ...
% The first output of the Kalman filter KALMF is the plant
% output estimate y_e = Cx[n|n], and the remaining outputs
% are the state estimates. Keep only the first output y_e
kalmf = kalmf(1,:)
a =
x1_e x2_e x3_e
x1_e 0.7683 -0.494 0.1129
x2_e 0.6202 0 0
x3_e -0.08173 1 0
b =
u y
x1_e -0.3832 0.3586
x2_e 0.5919 0.3798
x3_e 0.5191 0.08173
c =
x1_e x2_e x3_e
y_e 0.6202 0 0
d =
u y
y_e 0 0.3798
I/O groups:
Group name I/O Channel(s)
KnownInput I 1
Measurement I 2
OutputEstimate O 1
Sampling time: unspecified
Discrete-time model.
M, % innovation gain
M =
0.3798
0.0817
-0.2570
pause % Press any key to continue ...
% Let's see how this filter works by generating some data and
% comparing the filtered response with the true plant response.
%
% noise noise
% | |
% V V
% u -+->O-->[Plant]-->O--> yv --->[ ]
% | [filter]---> y_e
% +--------------------------->[ ]
%
% To simulate the system above, you can generate the response of
% each part separately or you can generate both together. To
% simulate each separately, you would use LSIM with the plant first
% and then the filter. Below we illustrate how to simulate both
% together.
% First, build a complete plant model with u,w,v as inputs and
% y and yv as outputs.
a = A;
b = [B B 0*B];
c = [C;C];
d = [0 0 0;0 0 1];
P = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});
% Then connect this plant model and the kalman filter in parallel
% by specifying u as a shared input:
sys = parallel(P,kalmf,1,1,[],[]);
pause % Press any key to continue ...
% Then connect this plant model and the kalman filter in parallel
% by specifying u as a shared input:
sys = parallel(P,kalmf,1,1,[],[]);
pause % Press any key to continue ...
% ... and connect the plant output yv to the filter input yv
% Note: yv is the 4th input of SYS and also its 2nd output
SimModel = feedback(sys,1,4,2,1);
SimModel = SimModel([1 3],[1 2 3]) % Delete yv form I/O
a =
? ? ? x1_e x2_e x3_e
? 1.127 -0.494 0.1129 0 0 0
? 1 0 0 0 0 0
? 0 1 0 0 0 0
x1_e 0.3586 0 0 0.7683 -0.494 0.1129
x2_e 0.3798 0 0 0.6202 0 0
x3_e 0.08173 0 0 -0.08173 1 0
b =
w v u
? -0.3832 0 -0.3832
? 0.5919 0 0.5919
? 0.5191 0 0.5191
x1_e 0 0.3586 -0.3832
x2_e 0 0.3798 0.5919
x3_e 0 0.08173 0.5191
c =
? ? ? x1_e x2_e x3_e
y 1 0 0 0 0 0
y_e 0.3798 0 0 0.6202 0 0
d =
w v u
y 0 0 0
y_e 0 0.3798 0
I/O groups:
Group name I/O Channel(s)
KnownInput I 3
OutputEstimate O 2
Sampling time: unspecified
Discrete-time model.
pause % Press any key to continue ...
pause % Press any key to continue ...
% The resulting simulation model has w,v,u as inputs and y,y_e as
% outputs:
SimModel.inputname
ans =
'w'
'v'
'u'
SimModel.outputname
ans =
'y'
'y_e'
pause % Press any key to continue ...
% You are now ready to simulate the filter behavior.
% Generate a sinusoidal input vector (known)
t = [0:100]';
u = sin(t/5);
% Generate process noise and sensor noise vectors
randn('seed',0)
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);
pause % Press any key to continue ...
% Now simulate the response using LSIM
[out,x] = lsim(SimModel,[w,v,u]);
y = out(:,1); % true response
ye = out(:,2); % filtered response
yv = y + v; % measured response
% Compare true response with filtered response
clf
subplot(211), plot(t,y,'b',t,ye,'r--'),
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'g',t,y-ye,'r--'),
xlabel('No. of samples'), ylabel('Error')
pause % Press any key after the plot ...
% The second plot indicates that the Kalman filter has reduced
% the error y-yv due to measurement noise, as confirmed by
% comparing the error covariances
MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);
% Covariance of error before filtering (measurement error)
MeasErrCov
MeasErrCov =
1.1138
% Covariance of error after filtering (estimation error)
EstErrCov
EstErrCo