%%%
%%% Kalman Filter design example
%%%

%%%
%%% KALMAN FILTER DESIGN
%%%

close all

disp(sprintf('\n'))
disp('KALMAN FILTER DESIGN EXAMPLE...'), pause
disp(sprintf('\n'))
disp('... Press return key to continue...'), pause
disp(sprintf('\n'))      

%%%
%%% Consider the discrete plant
%%% with additive Gaussian noise on the input and output 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]; 

Tc = 1;

D = zeros(1,2);

%%%
%%% Goal:  design a Kalman filter that estimates the output given 
%%% the inputs and the noisy output measurements
%%%

%%%
%%% Design the Kalman filter described above with the 
%%% function "kalman". First specify the plant model with the process 
%%% noise.
%%%

%%% Note: set sample time to -1 to mark model as discrete

Plant = ss(A,[B B],C,0,-1,'inputname',{'u' 'w'},'outputname','y');

Q = 1; R = 1; %%% Assuming that Q = R = 1;

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

%%%
%%% Because we are interested in the output estimate, keep only the first 
%%% output of kalmf.
%%%

kalmf = kalmf(1,:);

Akf = kalmf.a;
Bkf = kalmf.b;
Ckf = kalmf.c;
Dkf = kalmf.d;

%%%
%%% Confronto con l'osservatore dinamico di stato.
%%%

Ko = place(A',C',[0.2 0.21 0.22])';
Ao = A - Ko*C;
Bo = [B Ko];
Co = C;
Do = zeros(1,2);

%%%
%%% Construction of the state-space model of the block diagram with the 
%%% functions parallel and feedback. First build a complete plant model 
%%% with u, w and v as inputs and y and yv (measurements) as outputs.

a     = A;
b     = [B B 0*B];
c     = [C;C];
d     = [0 0 0;0 0 1];
Plant = ss(a,b,c,d,-1,'inputname',{'u' 'w' 'v'},'outputname',{'y' 'yv'});

sysp = parallel(Plant,kalmf,1,1,[],[]);

%%%
%%% Both the paralleled blocks take u as input (...,1,1,...).
%%% The outputs of the paralleled blocks are not added (...,[],[]))
%%%

%%%
%%% The inputs for the paralleled system are w, v, u and yv; the
%%% outputs are y, yv and ye.
%%%

%%%
%%% Close loop (1) around input #4 (yv) and output #2
%%% (yv) with sign +1
%%%

SimModel = feedback(sysp,1,4,2,1);

%%%
%%% Delete yv from I/O list
%%%

SimModel = SimModel([1 3],[1 2 3]);

%%%
%%% Simulation of the system behaviour
%%%

%%%
%%% 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);

figure, subplot(221), plot(t,u,'-'), xlabel('Samples'), ylabel('u(t)'), title('Plant noise-free input')
        subplot(222), plot(t,w,'-'), xlabel('Samples'), ylabel('w(t)'), title('Process noise')
        subplot(223), plot(t,v,'-'), xlabel('Samples'), ylabel('v(t)'), title('Sensor noise')
        subplot(224), plot(t,u+w,'-'), xlabel('Samples'), title('Noisy input')

disp(sprintf('\n'))        
disp('... Press return key to continue...'), pause
disp(sprintf('\n'))

%%%
%%% 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

figure, subplot(311), plot(t,y,'-'),  xlabel('Samples'), ylabel('y(t)'),   title('True response')
        subplot(312), plot(t,ye,'-'), xlabel('Samples'), ylabel('y_e(t)'), title('Filtered response')
        subplot(313), plot(t,yv,'-'), xlabel('Samples'), ylabel('y_v(t)'), title('Measured response')
        

disp(sprintf('\n'))        
disp('... Press return key to continue...'), pause
disp(sprintf('\n'))      

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

%%%
%%% Performance indices
%%%

disp(sprintf('\n'))       
pause, disp('... Press return key to continue...')
disp(sprintf('\n'))       

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

disp(sprintf('The error covariance before filtering (measurement error) is %0.4f',MeasErrCov))

disp(sprintf('\n'))       
disp('... Press return key to continue...'), pause
disp(sprintf('\n'))       

disp(sprintf('... while the error covariance after filtering (estimation error) is %0.4f',EstErrCov))

disp(sprintf('\n'))       
disp('... Press return key to continue...'), pause
disp(sprintf('\n'))

return