## Documentation Center |

On this page… |
---|

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 *w*[*n*]
on the input *u*[*n*] 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 *y*[*n*]
given the inputs *u*[*n*] and the
noisy output measurements

where *v*[*n*] is some Gaussian
white noise.

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

*x*[*n*] given past measurements up to*y*[_{v}*n*− 1]is the updated estimate based on the last measurement

*y*[_{v}*n*]

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 *y _{v}*[

between the measured and predicted values of *y*[*n* +
1]. The innovation gain *M* 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 *y*[*n*].
Note that the filter state is
.

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 *Q* = *R* =
1, 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

M M = 0.3798 0.0817 -0.2570

The inputs of `kalmf` are *u* and *y _{v}*,
and its outputs are the plant output and state estimates
and
.

Because you are interested in the output estimate *y _{e}*,
keep only the first output of

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 *y _{e}* with
the true response

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 *u*, *w*, *v* as
inputs *y* and *y _{v}* (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 *y _{v}* to
the filter input

% 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 *w*, *v*, *u* as
inputs and *y*, *y _{e}* 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 *u* and process and measurement
noise vectors *w* and *v*.

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 *y* (dashed
line) and the filtered output *y _{e}* (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

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 *x*[*1*|*0*]
and *P*[*1*|*0*],
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.

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 *y* (dashed
line) and the filtered response *y _{e}* (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 *y* − *y _{e}* 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 *M*[*n*]
and the steady-state value *M* of the innovation
gain matrix coincide.

Mn, M Mn = 0.3798 0.0817 -0.2570 M = 0.3798 0.0817 -0.2570

Was this topic helpful?