%
%   Corso Progettazione Sistemi di Controllo  -  a.a. 2006/07
%   Guido Albertin    (546986)
%   Elena Toffoli     (546946)
%   Giancarlo Baldan  (547324)
%

% Il vettore di stato e' X = [ x, dx/dt, y,  dy/dt ]'

%% Setup

clear all, close all, clc, warning off

%% Inizializzazione parametri

T = 0.1;                                            % [sec] tempo di campionamento
N = 200;                                            % iterazioni minime
M = 100;                                             % numero di simulazioni
t = 0:T:(N-1)*T;                                    % [sec] istanti di tempo

Ac = zeros(4,4); Ac(1,2) = 1; Ac(3,4) = 1;          % matrice dinamica continua
A = expm(Ac*T);                                     % matrice dinamica discreta

Q_(1:2,1:2) = 0.01*[0.1 0.01; 0.01 1];              % struttura covarianza rumore di modello
Q_(3:4,3:4) = Q_(1:2,1:2);                          %
R_ = diag([0.01 0.01]);                             % struttura covarianza rumore di misura

C = zeros(2,4); C(1,1) = 1; C(2,3) = 1;             % matrice uscite

x0 = [1 1 2 1]';                                    % stato iniziale (noto solo per la simulazione)
x0_ = [0 0 0 0]';                                   % stato iniziale (stima)
P0 = diag([1 1 1 1]);                               % covarianza iniziale

q = [ 1 1 0 ];                                      % casi da analizzare
r = [ 1 0 1 ];                                      %
          
%% Caso 1

Q = q(1)*Q_;
R = r(1)*R_;

%Genera simulazione
fprintf(strcat(strcat(strcat(strcat('\nq=',num2str(q(1))),' r='),num2str(r(1))),'\n\n'));
randn('state', sum(1e8*clock));
u = randn(M,6,N);
sys = ss(A,[sqrtm(Q) zeros(4,2)],C,[zeros(2,4) sqrtm(R)],T);

est_rec = kest_rec(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);
est_asyn = kest_asyn(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);

grafici(t,est_rec,est_asyn,0,q(1),r(1),M);

film(est_rec.x_post,est_rec.P_post,6,q(1),r(1));

%% Caso 2
fprintf('\nPress a key to analyze next case...\n')
pause
close all, clc;

Q = q(2)*Q_;
R = r(2)*R_;
 
%Genera simulazione
fprintf(strcat(strcat(strcat(strcat('\nq=',num2str(q(2))),' r='),num2str(r(2))),'\n\n'));
randn('state', sum(1e8*clock));
u = randn(M,6,N);
sys = ss(A,[sqrtm(Q) zeros(4,2)],C,[zeros(2,4) sqrtm(R)],T);

est_rec = kest_rec(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);
est_asyn = kest_asyn(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);

grafici(t,est_rec,est_asyn,20,q(2),r(2),M);

film(est_rec.x_post,est_rec.P_post,1,q(2),r(2));


%% Caso 3
fprintf('\nPress a key to analyze next case...\n')
pause
close all, clc;

Q = q(3)*Q_;
R = r(3)*R_;
 
%Genera simulazione
fprintf(strcat(strcat(strcat(strcat('\nq=',num2str(q(3))),' r='),num2str(r(3))),'\n\n'));
randn('state', sum(1e8*clock));
u = randn(M,6,N);
sys = ss(A,[sqrtm(Q) zeros(4,2)],C,[zeros(2,4) sqrtm(R)],T);

est_rec = kest_rec(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);
est_asyn = kest_asyn(N,M,A,C,Q,R,x0_,x0,P0,T,u,sys,t);

grafici(t,est_rec,est_asyn,20,q(3),r(3),M);

film(est_rec.x_post,est_rec.P_post,6,q(3),r(3));
