echo on

% This file demonstrates MATLAB's ability to do Kalman filtering.  
% Both a steady state filter and a time varying filter are designed
% and simulated below.

echo off
%	Copyright (c) 1986-93 by the MathWorks, Inc.
echo on

pause % 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];
C=[ 1 0 0];
D=0;
printsys(A,B,C,D)

% Design a Kalman filter to filter out Gaussian noise added to the 
% input (i.e sensor noise).

pause % Press any key to continue ...

% Let's do a steady state filter design first using the function 
% DLQE.  The function DLQE determines the optimal steady state 
% filter gain based on the process noise covariance Q, and the 
% sensor noise covariance, R.

% For your information the dcgain of this plant is
ddcgain(A,B,C,D)

% and the covariance of the output due to process noise with 
% covariance of 1 is:
dcovar(A,B,C,D,1)

% Enter the process noise covariance:
Q = input('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): ')

% Now design the steady state filter gain
Lss = dlqe(A,B,C,Q,R)

pause % Press any key to continue ...

% The steady state Kalman filter we have just designed has the 
% following update equations:
%                      -         *
%  time update:        x[n+1] = Ax[n] + Bu[n]
%
%                      *      -                -
%  observation update: x[n] = x[n] + L(y[n] - Cx[n] - Du[n])

% Since this is a steady state filter we can combine these equations
% into one state model (the Kalman filter)
%  -                -
%  x[n+1] = (A-ALC) x[n] + AL y[n]
%
%  *                -           
%  y[n]   = (C-CLC) x[n] + CL y[n]
%  
% The function DESTIM produces this model (but also includes the 
% estimated states as outputs):
[af,bf,cf,df] = destim(A,B,C,D,Lss,[1],[1]);

% Remove est. state outputs
[af,bf,cf,df] = ssdelete(af,bf,cf,df,[],[2:4]);
printsys(af,bf,cf,df,'u y','y*','x1 x2 x3')

pause % Press any key to continue ...

% Let's see how it works by generating some data and comparing the 
% filtered response with the true response
%     noise         noise
%       |             |
%       V             V
% u -+->O-->[Plant]-->O--> y --O->[filter]-->y*
%    |                               |
%    +-------------------------------+
%

% To simulate the system above, we could generate the response of 
% each part separately or we could generate both together.  To 
% simulate each seperately we would use DLSIM with the plant first
% and then the filter.  Below we illustrate how to simulate both
% together.

% First, build one plant that contains the original plant and the 
% filter connected as shown in the diagram above.  Use PARALLEL and
% CLOOP.   Before connecting, add a process noise input to the plant
% that is the same as u and a sensor noise input.
a = A; b=[B,B,zeros(3,1)]; c = C; d=[D,D,1];

% Now connect the original plant input (1) with the known input (1)
% of the filter using PARALLEL.
[at,bt,ct,dt] = parallel(a,b,c,d,af,bf,cf,df,[1],[1],[],[]);

% Put the plant output (1) into the filter sensor input (4)
[at,bt,ct,dt] = cloop(at,bt,ct,dt,[1],[4]);

pause % Press any key to continue ...

% The complete system model now has 4 inputs 
%    (plant input,process noise,sensor noise,sensor input),
% 2 outputs
%    (plant output with noise,filtered output),
% and 6 states.

% Generate a sinusoidal input vector (known)
t = [0:100]';
u = sin(t/5);

% Generate process noise and sensor noise vectors
pnoise = sqrt(Q)*randn(length(t),1);
snoise = sqrt(R)*randn(length(t),1);

pause % Press any key to continue ...

% Now simulate using DLSIM
[y,x] = dlsim(at,bt,ct,dt,[u,pnoise,snoise,zeros(length(t),1)]);

% Generate "true" response
ytrue = y(:,1)-snoise;

% Plot comparison
clg
%subplot(211), plot(t,ytrue,t,y), xlabel('No. of samples'), ylabel('Output')
%subplot(212), plot(t,ytrue-y(:,2)), xlabel('No. of samples'), ylabel('Error')
%pause % Press any key to continue ...
echo off
subplot(211), plot(t,ytrue,t,y), xlabel('No. of samples'), ylabel('Output')
subplot(212), plot(t,ytrue-y(:,2)), xlabel('No. of samples'), ylabel('Error')
pause % Press any key after the plot ...
echo on

% Compute covariance of error
err1 = ytrue-y(:,1);
err1cov = sum(err1.*err1)/length(err1);
err2 = ytrue-y(:,2);
err2cov = sum(err2.*err2)/length(err2);

% Covariance of error before filtering
err1cov

% Covariance of error after filtering
err2cov

pause % Press any key to continue ...

% Now let's form a time varying Kalman filter to perform the same
% task.  A time varying Kalman filter is able to perform well even
% when the noise covariance is not stationary.  However for this 
% demonstration, we will use stationary covariance.

% The time varying Kalman filter has the following update equations
%                      -         *
%  time update:        x[n+1] = Ax[n] + Bu[n]
%                      -         * 
%                      P[n+1] = AP[n]A' + B*Q*B'
%
%                             -       -       -1
%  observation update: L[n] = P[n]C'(CP[n]C'+R)
%                      *      -                   -
%                      x[n] = x[n] + L[n](y[n] - Cx[n] - Du[n])
%                      *               -
%                      P[n] = (I-L[n]C)P[n]
%                      *       *
%                      y[n] = Cx[n] + Du[n]
%

pause % Press any key to continue ...

% Generate the noisy plant response
y = dlsim(A,B,C,D,u+pnoise) + snoise;

% Now filter. We can't use DLSIM here since the system is nonlinear,
% so just implement the above equations in a loop.  Use the same inputs
% as before.

P=B*Q*B';         % Guess for initial state covariance
x=zeros(3,1);     % Initial condition on the state
yest = zeros(length(t),1);
ycov = zeros(length(t),1); 
for i=1:length(t)
  yest(i) = C*x + D*u(i);
  ycov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);
  P = A*P*A' + B*Q*B';

  % Observation update
  L = P*C'/(C*P*C'+R);
  x = x + L*(y(i) - C*x - D*u(i));
  P = (eye(3)-L*C)*P;
end

% Compare true response with filtered response
clg
%subplot(211), plot(t,y-snoise,t,yest), ylabel('Output')
%subplot(212), plot(t,yest-y+snoise), ylabel('Error'), pause % Press any key
echo off
subplot(211), plot(t,y-snoise,t,yest), ylabel('Output')
subplot(212), plot(t,yest-y+snoise), ylabel('Error'), pause % Press any key
echo on

% The time varying filter also estimates the output covariance
% during the estimation.  Let's plot it to see if our filter reached
% steady state (as we would expect with stationary input noise).
subplot(111), plot(t,ycov), ylabel('Covar'), pause % Press any key after plot

% From the covariance plot we see that the output covariance did 
% indeed reach a steady state in about 5 samples.  From then on,
% our time varying filter has the same performance as the steady 
% state version.

% Compute covariance of error
err = y-snoise-yest;
errcov = sum(err.*err)/length(err)

pause % Press any key to continue ...

%Let's compare the final Kalman gain matrices
L,Lss

% So we see that they both obtain the same gain matrix in steady
% state.

echo off