% Exercise 2 KF Session 13

% Generation of oscillator data

Ac=[0 1; -1 0];
Bc=[0;1];
Cc=[1 0];
Dc=0;
T=0.5;
sysC=ss(Ac,Bc,Cc,Dc);
sysD=c2d(sysC,T);

% noiseless input to the oscillator
Un=1*[ones(1,10) zeros(1,10) ones(1,10) zeros(1,11)];
% add process noise 
sqW=0.1;
U=Un+sqW*randn(size(Un));

[Yn,Tsim,Xsim]=lsim(sysD,U);
% add measurement noise
sqV=0.2;
Ym=Yn+sqV*randn(size(Yn));


%% Plot of the output

figure(1);clf
%times=0:39;
PL=plot(Tsim,Ym);
NameArray = {'Color','LineStyle','LineWidth'};
ValueArray = {'blue','--',2};
set(PL,NameArray,ValueArray)

legend('measured outputs')
xlabel('{t}','FontSize',18)
ylabel('{y}','FontSize',18)
title('Oscillator data');
set(gca,'FontSize',13)


%% Kalman predictors

% Extract discrete-time matrices
A=sysD.A;
B=sysD.B;
C=sysD.C;
D=sysD.D;

% Define the noise covariances

% since the process noise acts only on the second state, we have
W=1*diag([0, sqW^2]);

V=1*sqV^2;
Ex0=[0;0];
sqx0=1e0;
Varx0=1*sqx0^2*eye(2);

%% (a) - Time-varying KF

% Placeholders for estimates and covariances computed through KF
XKK=[]; % stores \hat x k|k as rows. The first element is for the time k=0
SKK={}; % stores \Sigma k|k. The first element is for the time k=0
XKP1K=[]; % stores \hat x k+1|k. The first element is for the time k=0
SKP1K={}; % stores \Sigma k|k. The first element is for the time k=0

% LTV KF initialization
xkkm1=Ex0;
Skkm1=Varx0;

for k=1:length(Tsim)-1
   
    % extract the current masurements
    yk=Ym(k);
    uk=Un(k);
    % filtering step - LTV filter
    xkk=xkkm1+Skkm1*C'*inv(C*Skkm1*C'+V)*(yk-C*xkkm1);
    Skk=Skkm1-Skkm1*C'*inv(C*Skkm1*C'+V)*C*Skkm1;
        % store
        XKK=[XKK;xkk']; 
        SKK{k}=Skk;
    
    % prediction step - LTV filter
    xkp1k=A*xkk+B*uk;
    Skp1k=A*Skk*A'+W;
    % store
        XKP1K=[XKP1K;xkp1k']; 
        SKP1K{k}=Skp1k;
   
    % time increment
    xkkm1=xkp1k;
    Skkm1=Skp1k;% % SS KF initialization

end

%% (b) - Steady-State KF

% Placeholders for estimates and covariances computed through KF
ssXKP1K=[]; % stores \hat x k+1|k. The first element is for the time k=0


% Compute the Kalman predictor gain
barS=dare(A', C',W,V);
L=A*barS*C'*inv(C*barS*C'+V);
Al=A-L*C;

% verify stability of the predictor
abs(eig(Al))

% Steady-state KF initialization
ssxkkm1=Ex0;
for k=1:length(Tsim)-1
   
    % extract the current masurements
    yk=Ym(k);
    uk=Un(k);
 
    % Kalman Predictor update
    ssxkp1k=A*ssxkkm1+B*uk+L*(yk-C*ssxkkm1);
        % store
        ssXKP1K=[ssXKP1K;ssxkp1k'];    
        
    % time increment
    ssxkkm1=ssxkp1k;
end


%% Plot of the data and the output estimates \hat y k+1|k

% with the time-varying predictor
Ykp1k=XKP1K*C'; 
% with the steady-state predictor
ssYkp1k=ssXKP1K*C';

figure(1);clf

PL=plot(Tsim,Ym, Tsim(2:end),Ykp1k,Tsim(2:end),ssYkp1k);
NameArray = {'Color','LineStyle','LineWidth'};
ValueArray = {'blue','-',2; 'red' '--',2;'green' '-',2};
set(PL,NameArray,ValueArray)

legend('measured outputs', 'Time-varying predictor', 'Steady-state predictor')
xlabel('{t}','FontSize',18)
ylabel('{y}','FontSize',18)
title('Oscillator data');
set(gca,'FontSize',13)

%% Solutions to points 2 and 3

% The time-varying predictor converges quickly to the steady-state
% predictor - the only differences are noticeable in the transient


%% (c) - Changing the noise covariance 
% Using 
% V=0.001*sqV^2; gives much more oscillating estimates. This is because the 
% predictor thinks that there is "less noise" than in reality. As a 
% consequence it "trusts" more the output measurements and tends to track 
% them more closely

% The opposite is obtained using % V=1000*sqV^2; i.e. the predictor changes 
% more slowly because it trusts "less" the output measurements