% Identificazione e Analisi dei Dati II
% Esercitazione #1 : Filtro di Kalman 

clear; close all
load dati_eser1

A=[ [a1 a2 a3; b1 0 0; 0 b2 b3] zeros(3); [c1 c2 c3; 0 0 0; 0 0 0] [0 0 0; d1 0 0; 0 d2 d3] ];
B=eye(6);
C=[1 1 1 0 0 0 ; 0 0 0 1 1 1];
D=zeros(2,6);

T=100;
N=length(t);

Q=sw2*eye(6);
R=sv2*eye(2);

%%%%
% A
%%%%
disp('A')

yn=yA;
xn=xveroA;
u=uA;

%
% filtro di kalman ricorsivo
%
zk=zeros(6,N);
% inizializzazione
%zk0=[yn(1,1)/3 yn(1,1)/3 yn(1,1)/3 yn(1,2)/3 yn(1,2)/3 yn(1,2)/3 ]';   % z(0|-1)
zk0=[0 0 0 0 0 0]';
P=100^2*eye(6);               % P(0|-1)
K=P*C'*inv(C*P*C'+R);
zk(:,1)=zk0+K*(yn(1,:)'-C*zk0);  % z(0|0)

predMSE(1,:)=sqrt(trace(P));
actMSE(1,:)=(xn(1,:)'-zk(:,1))';
for i=2:N,
% predittore
% zk(:,i)=A*zk(:,i-1)+A*K*(yn(i-1,:)'-C*zk(:,i-1))+B*u(i-1,:)';
% P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
% K=P*C'*inv(C*P*C'+R);

% filtro
 P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
 K=P*C'*inv(C*P*C'+R);
 zk(:,i)=A*zk(:,i-1)+K*(yn(i,:)'-C*A*zk(:,i-1)-C*B*u(i-1,:)')+B*u(i-1,:)';

 actMSE(i,:)=(xn(i,:)'-zk(:,i))';
predMSE(i,:)=sqrt(trace(P));
end;

% stima variabili di stato
figure
subplot(321), plot(t,xn(:,1),'g',t,zk(1,:)','r--');title('A: xvero (ver); xKF (ros)');ylabel('x1')
subplot(322), plot(t,xn(:,2),'g',t,zk(2,:)','r--');ylabel('x2')
subplot(323), plot(t,xn(:,3),'g',t,zk(3,:)','r--');ylabel('x3')
subplot(324), plot(t,xn(:,4),'g',t,zk(4,:)','r--');ylabel('x4')
subplot(325), plot(t,xn(:,5),'g',t,zk(5,:)','r--');ylabel('x5')
subplot(326), plot(t,xn(:,6),'g',t,zk(6,:)','r--');ylabel('x6')

avevars=mean(actMSE)
MSE=sum((actMSE.^2)');
aveMSE=sqrt(sum(MSE)/N)
figure, plot(t,sqrt(MSE),'g',t,predMSE,'r'), title('A: andamento di EQ - vero (ver); stimato (ros)')

%%%%
% B
%%%%
disp('B')

%
% filtro di kalman asintotico
%
[Kas,Pinf,Pinf2,KFeig]=dlqe(A,eye(6),C,Q,R);
%zas=dlsim(A-A*Kas*C,[A*Kas B],eye(6),zeros(6,8),[yn u],zk0);  % predittore
zk0as=zk0;
zas=dlsim(A-A*Kas*C,[A*Kas B],eye(6)-Kas*C,[Kas zeros(6)],[yn u],zk0as);   % filtro

figure
subplot(321), plot(t,xn(:,1),'g',t,zk(1,:)','r--',t,zas(:,1)','b-');title('B: xvero (ver); xKF (ros); xKFas (blu)');ylabel('x1')
subplot(322), plot(t,xn(:,2),'g',t,zk(2,:)','r--',t,zas(:,2)','b-');ylabel('x2')
subplot(323), plot(t,xn(:,3),'g',t,zk(3,:)','r--',t,zas(:,3)','b-');ylabel('x3')
subplot(324), plot(t,xn(:,4),'g',t,zk(4,:)','r--',t,zas(:,4)','b-');ylabel('x4')
subplot(325), plot(t,xn(:,5),'g',t,zk(5,:)','r--',t,zas(:,5)','b-');ylabel('x5')
subplot(326), plot(t,xn(:,6),'g',t,zk(6,:)','r--',t,zas(:,6)','b-');ylabel('x6')


%%%%
% C
%%%%
disp('C')

yn=yC;

%
% filtro di kalman con R fissa
%
R=300^2*eye(2);
zk=zeros(6,N);
% inizializzazione
%zk0=[0 0 0 0 0 0]';         % z(0|-1)
zk0=[yn(1,1)/3 yn(1,1)/3 yn(1,1)/3 yn(1,2)/3 yn(1,2)/3 yn(1,2)/3 ]';   % z(0|-1)
P=100*eye(6);               % P(0|-1)
K=P*C'*inv(C*P*C'+R);
zk(:,1)=zk0+K*(yn(1,:)'-C*zk0);  % z(0|0)
predMSE(1,:)=sqrt(trace(P));
actMSE(1,:)=(xn(1,:)'-zk(:,1))';
for i=2:N,
% predittore
% zk(:,i)=A*zk(:,i-1)+A*K*(yn(i-1,:)'-C*zk(:,i-1))+B*u(i-1,:)';
% P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
% K=P*C'*inv(C*P*C'+R);

% filtro
 P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
 K=P*C'*inv(C*P*C'+R);
 zk(:,i)=A*zk(:,i-1)+K*(yn(i,:)'-C*A*zk(:,i-1)-C*B*u(i-1,:)')+B*u(i-1,:)';

 actMSE(i,:)=(xn(i,:)'-zk(:,i))';
 predMSE(i,:)=sqrt(trace(P));
end;

zkf=zk;
MSE=sum((actMSE.^2)');
aveMSE=sqrt(sum(MSE)/N)

%
% filtro di kalman con R variabile (vera)
%
zk=zeros(6,N);
% inizializzazione
%zk0=[0 0 0 0 0 0]';         % z(0|-1)
zk0=[yn(1,1)/3 yn(1,1)/3 yn(1,1)/3 yn(1,2)/3 yn(1,2)/3 yn(1,2)/3 ]';   % z(0|-1)
P=100*eye(6);               % P(0|-1)
sv(1)=300;
R=sv(1)^2*eye(2);
K=P*C'*inv(C*P*C'+R);
zk(:,1)=zk0+K*(yn(1,:)'-C*zk0);  % z(0|0)

predMSEv(1,:)=sqrt(trace(P));
actMSE(1,:)=(xn(1,:)'-zk(:,1))';
for i=2:N,
sv(i)=300*0.95^(i-1);
R=sv(i)^2*eye(2);
% predittore
% zk(:,i)=A*zk(:,i-1)+A*K*(yn(i-1,:)'-C*zk(:,i-1))+B*u(i-1,:)';
% P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
% K=P*C'*inv(C*P*C'+R);

% filtro
 P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
 K=P*C'*inv(C*P*C'+R);
 zk(:,i)=A*zk(:,i-1)+K*(yn(i,:)'-C*A*zk(:,i-1)-C*B*u(i-1,:)')+B*u(i-1,:)';

 actMSE(i,:)=(xn(i,:)'-zk(:,i))';
 predMSEv(i,:)=sqrt(trace(P));
end;

% stima variabili di stato
figure
subplot(321), plot(t,xn(:,1),'g',t,zk(1,:)','r--',t,zkf(1,:)','b--'); title('C: xvera (ver); xKF R vera (ros); xKF R fissa (blu)'); ylabel('x1')
subplot(322), plot(t,xn(:,2),'g',t,zk(2,:)','r--',t,zkf(2,:)','b--'); ylabel('x2')
subplot(323), plot(t,xn(:,3),'g',t,zk(3,:)','r--',t,zkf(3,:)','b--'); ylabel('x3')
subplot(324), plot(t,xn(:,4),'g',t,zk(4,:)','r--',t,zkf(4,:)','b--'); ylabel('x4')
subplot(325), plot(t,xn(:,5),'g',t,zk(5,:)','r--',t,zkf(5,:)','b--'); ylabel('x5')
subplot(326), plot(t,xn(:,6),'g',t,zk(6,:)','r--',t,zkf(6,:)','b--'); ylabel('x6')

MSE2=sum((actMSE.^2)');
aveMSE2=sqrt(sum(MSE2)/N)
figure, plot(t,sqrt(MSE),'b',t,sqrt(MSE2),'g',t,predMSE,'b:',t,predMSEv,'g:')
title('C: Confronto degli EQ (blu: R fissa; ver: R vera)')

%%%%
% D
%%%%
disp('D')

yn=yD;
xn=xveroD;
u=uD;

%
% filtro di kalman con matrice A costante (stessa del punto A)
%
zk=zeros(6,N);
% inizializzazione
%zk0=[yn(1,1)/3 yn(1,1)/3 yn(1,1)/3 yn(1,2)/3 yn(1,2)/3 yn(1,2)/3 ]';   % z(0|-1)
zk0=[0 0 0 0 0 0]';
P=100^2*eye(6);               % P(0|-1)
K=P*C'*inv(C*P*C'+R);
zk(:,1)=zk0+K*(yn(1,:)'-C*zk0);  % z(0|0)

predMSE(1,:)=sqrt(trace(P));
actMSE(1,:)=(xn(1,:)'-zk(:,1))';
for i=2:N,
% predittore
% zk(:,i)=A*zk(:,i-1)+A*K*(yn(i-1,:)'-C*zk(:,i-1))+B*u(i-1,:)';
% P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
% K=P*C'*inv(C*P*C'+R);

% filtro
 P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
 K=P*C'*inv(C*P*C'+R);
 zk(:,i)=A*zk(:,i-1)+K*(yn(i,:)'-C*A*zk(:,i-1)-C*B*u(i-1,:)')+B*u(i-1,:)';

 actMSE(i,:)=(xn(i,:)'-zk(:,i))';
 predMSE(i,:)=sqrt(trace(P));
end;

zkf=zk;
predMSEf=predMSE;
MSE=sum((actMSE.^2)');

%
% filtro di kalman con matrice A variabile (vera)
%
zk=zeros(6,N);
% inizializzazione
%zk0=[yn(1,1)/3 yn(1,1)/3 yn(1,1)/3 yn(1,2)/3 yn(1,2)/3 yn(1,2)/3 ]';   % z(0|-1)
zk0=[0 0 0 0 0 0]';
P=100^2*eye(6);               % P(0|-1)
K=P*C'*inv(C*P*C'+R);
zk(:,1)=zk0+K*(yn(1,:)'-C*zk0);  % z(0|0)

predMSE(1,:)=sqrt(trace(P));
actMSE(1,:)=(xn(1,:)'-zk(:,1))';
for i=2:N,
% predittore
% zk(:,i)=A*zk(:,i-1)+A*K*(yn(i-1,:)'-C*zk(:,i-1))+B*u(i-1,:)';
% P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
% K=P*C'*inv(C*P*C'+R);

A(1,2)=0.45+0.3*sin(2*pi/20*(i-1));

% filtro
 P=A*P*A'+Q-A*P*C'*inv(C*P*C'+R)*C*P*A';
 K=P*C'*inv(C*P*C'+R);
 zk(:,i)=A*zk(:,i-1)+K*(yn(i,:)'-C*A*zk(:,i-1)-C*B*u(i-1,:)')+B*u(i-1,:)';

 actMSE(i,:)=(xn(i,:)'-zk(:,i))';
 predMSE(i,:)=sqrt(trace(P));
end;

% stima variabili di stato
figure
subplot(321), plot(t,xn(:,1),'g',t,zk(1,:)','r--',t,zkf(1,:)','b--'); title('D: xvera (ver); xKF A vera (ros); xKF A fissa (blu)'); ylabel('x1')
subplot(322), plot(t,xn(:,2),'g',t,zk(2,:)','r--',t,zkf(2,:)','b--'); ylabel('x2')
subplot(323), plot(t,xn(:,3),'g',t,zk(3,:)','r--',t,zkf(3,:)','b--'); ylabel('x3')
subplot(324), plot(t,xn(:,4),'g',t,zk(4,:)','r--',t,zkf(4,:)','b--'); ylabel('x4')
subplot(325), plot(t,xn(:,5),'g',t,zk(5,:)','r--',t,zkf(5,:)','b--'); ylabel('x5')
subplot(326), plot(t,xn(:,6),'g',t,zk(6,:)','r--',t,zkf(6,:)','b--'); ylabel('x6')

MSE2=sum((actMSE.^2)');
figure, plot(t,sqrt(MSE),'b',t,sqrt(MSE2),'g',t,predMSEf,'b:',t,predMSE,'g:')
title('D: Confronto degli EQ (blu: A fissa; ver: A vera)')
