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



%% Setup

clear all, close all, clc, warning off



%% Inizializzazione parametri      

T = 100;                                            % numero di misure effettuate
T_C = 0.1;                                          % [sec] tempo di campionamento
tempo = T_C*(0:T-1);                                % Indici temporali delle misure

Ac = zeros(4,4); Ac(1,2) = 1; Ac(3,4) = 1;          % matrice dinamica continua
A = expm(Ac*T_C);                                   % 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

lambda = [ 0 0.25 0.5 0.75];                        % distribuzione di probabilita:
                                                    % 1) \lambda_k=lambda(k+1)
                                                    % 2) lambda(k) = lambda(k+1) per k > 3
                                                    
[outs,states] = size(C);                            % calcola dimensione uscite e stato



%% Generazione delle misure

randn('seed', sum(1e3*rand));                       % inizializza generatore numeri casuali

if( exist('u') == 0 )                               % vettori rumori di ingresso 
u = randn(states+outs,T);                           %
end                                                 %

sys = ss(A,[sqrtm(Q) zeros(states,outs)],C,...      % definisci il processo
    [zeros(outs,states) sqrtm(R)],T_C);             %
[y,t,x_] = lsim(sys,u,tempo,x0); y = y'; x_ = x_';  % ottieni una realizzazione del processo

if( exist('tau') == 0 )                             % calcola ritardo aleatorio per ogni pacchetto 
for k=1:T                                           % in base alla funzione di probabilita' lambda:
    tau(k)=rand_delay(lambda);                      % tau \in {0,1,2,3} U {\infty}
end                                                 %
end                                                 %


%% Alloco le variabili

N = length(lambda);                                 % lunghezza massima buffer (dopo N passi non 
                                                    % arrivano sicuramente campioni
LB1 = N;                                            % lunghezza del buffer, caso tempo variante
LB2 = N;                                            % lunghezza del buffer, caso guadagno statico



%% Algoritmo con guadagno tempo variante

[ x_post1, P_prior1, NP_prior1 ] = stimTV(lambda, tau, LB1, A, C, Q, R, y, x0_, P0);



%% stimatore ottimo con guadagni costanti (punto 2)

[ x_post2, P_prior2, NP_prior2 ] = stimConst(lambda, tau, LB2, A, C, Q, R, y, x0_, P0);



%% Visualizzazione degli andamenti ottenuti

h=figure(1);
set(h,'Position',[0 0 1200 400]);
subplot(1,2,1), hold on
plot(tempo,[ NP_prior1; NP_prior2 ]);
title(sprintf('Andamento temporale varianza dell''errore di predizione, N = %d',N))
legend('Stima','Stima guad. statico');
xlabel('Campione');
subplot(1,2,2), hold on
plot(tempo,[x_post1(1,:);x_post2(1,:); x_(1,:)])
title(sprintf('Andamento temporale della stima della posizione x, N = %d',N))
legend('Stima','Stima guad. statico','Valore vero');
xlabel('Campione');



%% Punto 3
Nmax = 20;
for N = 1:Nmax
    [ K,S ] = solveGains( lambda, N, A, C, Q, R );
    V_0_N(:,:,N) = S(:,:,1);
    trV_0_N(N) = trace(S(:,:,1));
end
figure(2);
plot(1:Nmax,trV_0_N)
axis tight
title('Andamento prestazione dello stimatore al variare della lunghezza del buffer')
xlabel('lunghezza buffer')
ylabel('tr(S_\infty^0)')
% nota bene: dopo 4 è STAZIONARIO



%% Punto 4

film(x_post1,x_post2,x_,P_prior1,P_prior2,3);


