% Exercise 3 KF Session 13

% Generation of data measured by the sensors

A=1;
B=0;
C=[1 1]';
D=0;


x0=5;

Ts=1;
sysD=ss(A,B,C,D,Ts);
% input
times=0:199;
U=zeros(1,length(times));

[Yn,Tsim,Xsim]=lsim(sysD,U, times, x0);

% add measurement noise
V=[1 0.1; 0.1 1];
sqV=sqrtm(V);
mnoise=sqV*randn(2,length(Yn));
Ym=Yn+mnoise';


%% Plot of the output

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

legend('Sensor 1', 'Sensor 2')
xlabel('{t}','FontSize',18)
ylabel('{y}','FontSize',18)
title('Measurements by the two sensors');
set(gca,'FontSize',13)



%% Kalman predictors


% Define the noise covariances

W=0;

Ex0=[1];
Varx0=1;

%% 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=U(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


%% Plot of the constant and the  estimates \hat x k+1|k

figure(2);clf
PL=plot(Tsim,x0*ones(size(Tsim)), Tsim(2:end),XKP1K);
NameArray = {'Color','LineStyle','LineWidth'};
ValueArray = {'blue','-',2; 'red' '--',2};
set(PL,NameArray,ValueArray)

legend('Constant parameter', 'Time-varying predictor')
xlabel('{t}','FontSize',18)
ylabel('{y}','FontSize',18)
title('Estimation of the unknown parameter');
set(gca,'FontSize',13)