%
%   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
t = 0:T:(N-1)*T;                                    % [sec] istanti di tempo
lambda = 0.5;                                       % P[ gamma_k = 1 ] = lambda 

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



%% Caso 1

%Genera simulazione
randn('seed', sum(1e8*clock));
u = randn(6,N);
sys = ss(A,[sqrtm(Q) zeros(4,2)],C,[zeros(2,4) sqrtm(R)],T);
[y,t,x_] = lsim(sys,squeeze(u(:,:)),t,x0);
gamma = randbin(lambda,N);                           % variabile aleatoria che determina la perdita di pacchetti

%% Stimatore ottimo tempo variante (punto 2)
est_rec = kest_rec(N,A,C,Q,R,x0_,x0,P0,T,u,sys,t,gamma);

%% Stiamtore ottimo statico (punto 3)
est_asyn = kest_asyn(N,A,C,Q,R,x0_,x0,P0,T,u,sys,t,gamma,lambda);

%% stimatore che utilizza il valore di misura precedente (punto 4)
est_asyn_pto4 = kest_asyn_pto4(N,A,C,Q,R,x0_,x0,P0,T,u,sys,t,gamma);

%% stampa grafici
grafici(t,est_rec,est_asyn,est_asyn_pto4,0);
film(x_',est_rec.x_post,est_asyn.x_post,est_asyn_pto4.x_post,est_rec.P_post,est_asyn.P_post,6);

%% Punto 5
figure(5)
j = 0; clear S;
lambda_ = 0.05:0.05:1;
for lambda = lambda_
    j = j + 1;
    S(:,:,j) = equilibrioS(A,C,Q,R,lambda,1e-8);
end
plot(lambda_,[squeeze(S(1,1,:)) est_asyn_pto4.P_prior(1,1)*ones(size(lambda_'))])
title('Varianza asintotica errore di posizione a priori al variare di \lambda')
legend('S_{\infty}^{\lambda}','Guadagno costante senza perdita di pacchetti')
xlabel('\lambda')

