Kalman Filtering :: Design Case Studies (Control System Toolbox™)

Learn more about Control System Toolbox

Kalman Filtering

On this page…
Overview of this Case Study Discrete Kalman Filter Steady-State Design Time-Varying Kalman Filter Time-Varying Design References

On this page…

Overview of this Case Study

Discrete Kalman Filter

Steady-State Design

Time-Varying Kalman Filter

Time-Varying Design

References

Overview of this Case Study

This final case study illustrates Kalman filter design and simulation. Both steady-state and time-varying Kalman filters are considered.

Consider the discrete plant

with additive Gaussian noise on the input and data

A = [1.1269   -0.4940    0.1129
     1.0000         0         0
          0    1.0000         0];

B = [-0.3832
      0.5919
      0.5191];

C = [1 0 0];

Our goal is to design a Kalman filter that estimates the output given the inputs and the noisy output measurements

where is some Gaussian white noise.

Discrete Kalman Filter

The equations of the steady-state Kalman filter for this problem are given as follows.

Measurement update

Time update

In these equations:

is the estimate of given past measurements up to
is the updated estimate based on the last measurement

Given the current estimate , the time update predicts the state value at the next sample (one-step-ahead predictor). The measurement update then adjusts this prediction based on the new measurement . The correction term is a function of the innovation, that is, the discrepancy.

between the measured and predicted values of . The innovation gain is chosen to minimize the steady-state covariance of the estimation error given the noise covariances

You can combine the time and measurement update equations into one state-space model (the Kalman filter).

This filter generates an optimal estimate of . Note that the filter state is .

Steady-State Design

You can design the steady-state Kalman filter described above with the function kalman. First specify the plant model with the process noise.

This is done by

% Note: set sample time to -1 to mark model as discrete
Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},...
                          'outputname','y');

Assuming that , you can now design the discrete Kalman filter by

Q = 1; R = 1;
[kalmf,L,P,M] = kalman(Plant,Q,R);

This returns a state-space model kalmf of the filter as well as the innovation gain

The inputs of kalmf are and , and its outputs are the plant output and state estimates and .

Because you are interested in the output estimate , keep only the first output of kalmf. Type

kalmf = kalmf(1,:);
kalmf
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
 
Input groups:                 
       Name        Channels   
    KnownInput        1       
    Measurement       2       
                              
Output groups:                
         Name         Channels
    OutputEstimate       1    
                              
Sampling time: unspecified
Discrete-time model.

To see how the filter works, generate some input data and random noise and compare the filtered response with the true response . You can either generate each response separately, or generate both together. To simulate each response separately, use lsim with the plant alone first, and then with the plant and filter hooked up together. The joint simulation alternative is detailed next.

The block diagram below shows how to generate both true and filtered outputs.

You can construct a state-space model of this block diagram with the functions parallel and feedback. First build a complete plant model with as inputs and (measurements) 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 use parallel to form the following parallel connection.

sys = parallel(P,kalmf,1,1,[],[])

Finally, close the sensor loop by connecting the plant output to the filter input with positive feedback.

% Close loop around input #4 and output #2
SimModel = feedback(sys,1,4,2,1)
% Delete yv from I/O list
SimModel = SimModel([1 3],[1 2 3])

The resulting simulation model has as inputs and as outputs.

SimModel.inputname

ans = 
    'w'
    'v'
    'u'

SimModel.outputname

ans = 
    'y'
    'y_e'

You are now ready to simulate the filter behavior. Generate a sinusoidal input and process and measurement noise vectors and .

t = [0:100]';
u = sin(t/5);

n = length(t)
randn('seed',0)
w = sqrt(Q)*randn(n,1);
v = sqrt(R)*randn(n,1);

Now simulate with lsim.

[out,x] = lsim(SimModel,[w,v,u]);

y = out(:,1);   % true response
ye = out(:,2);  % filtered response
yv = y + v;     % measured response

and compare the true and filtered responses graphically.

subplot(211), plot(t,y,'--',t,ye,'-'), 
xlabel('No. of samples'), ylabel('Output')
title('Kalman filter response')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-'),
xlabel('No. of samples'), ylabel('Error')

The first plot shows the true response (dashed line) and the filtered output (solid line). The second plot compares the measurement error (dash-dot) with the estimation error (solid). This plot shows that the noise level has been significantly reduced. This is confirmed by the following error covariance computations.

MeasErr = y-yv;
MeasErrCov = sum(MeasErr.*MeasErr)/length(MeasErr);
EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr);

The error covariance before filtering (measurement error) is

MeasErrCov
 
MeasErrCov =
    1.1138

while the error covariance after filtering (estimation error) is only

EstErrCov
 
EstErrCov =
    0.2722

Time-Varying Kalman Filter

The time-varying Kalman filter is a generalization of the steady-state filter for time-varying systems or LTI systems with nonstationary noise covariance. Given the plant state and measurement equations

the time-varying Kalman filter is given by the recursions

Measurement update

Time update

with and as defined in Discrete Kalman Filter, and in the following.

For simplicity, we have dropped the subscripts indicating the time dependence of the state-space matrices.

Given initial conditions and , you can iterate these equations to perform the filtering. Note that you must update both the state estimates and error covariance matrices at each time sample.

Time-Varying Design

Although the Control System Toolbox software does not offer specific commands to perform time-varying Kalman filtering, it is easy to implement the filter recursions in the MATLAB environment. This section shows how to do this for the stationary plant considered above.

First generate noisy output measurements

% Use process noise w and measurement noise v generated above
sys = ss(A,B,C,0,-1);
y = lsim(sys,u+w);      % w = process noise
yv = y + v;             % v = measurement noise

Given the initial conditions

you can implement the time-varying filter with the following for loop.

P = B*Q*B';         % Initial error covariance
x = zeros(3,1);     % Initial condition on the state
ye = zeros(length(t),1);
ycov = zeros(length(t),1); 

for i=1:length(t)
  % Measurement update
  Mn = P*C'/(C*P*C'+R);
  x = x + Mn*(yv(i)-C*x);   % x[n|n]
  P = (eye(3)-Mn*C)*P;      % P[n|n]

  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n]
end

You can now compare the true and estimated output graphically.

subplot(211), plot(t,y,'--',t,ye,'-')
title('Time-varying Kalman filter response')
xlabel('No. of samples'), ylabel('Output')
subplot(212), plot(t,y-yv,'-.',t,y-ye,'-')
xlabel('No. of samples'), ylabel('Output')

The first plot shows the true response (dashed line) and the filtered response (solid line). The second plot compares the measurement error (dash-dot) with the estimation error (solid).

The time-varying filter also estimates the covariance errcov of the estimation error at each sample. Plot it to see if your filter reached steady state (as you expect with stationary input noise).

subplot(211)
plot(t,errcov), ylabel('Error covar')

From this covariance plot, you can see that the output covariance did indeed reach a steady state in about five samples. From then on, your time-varying filter has the same performance as the steady-state version.

Compare with the estimation error covariance derived from the experimental data. Type

EstErr = y-ye;
EstErrCov = sum(EstErr.*EstErr)/length(EstErr)
EstErrCov =
    0.2718

This value is smaller than the theoretical value errcov and close to the value obtained for the steady-state design.

Finally, note that the final value and the steady-state value of the innovation gain matrix coincide.

Mn, M

Mn =
    0.3798
    0.0817
   -0.2570

M =
    0.3798
    0.0817
   -0.2570

References

[1] [Grimble, M.J., Robust Industrial Control: Optimal Design Approach for Polynomial Systems, Prentice Hall, 1994, p. 261 and pp. 443-456.

LQG Regulation: Rolling Mill Example

Recommended Products

Get MATLAB trial software

Includes the most popular MATLAB recorded presentations with Q&A sessions led by MATLAB experts.

Get the Interactive Kit