clear all; close all;

A = [ 1 0.1; 0 1]; 
C = [ 1 0 ];
Q = [ 0 0.1; 0.1 1];
R = 1;


%%%%%%%

lambda = [ 0 0.25 0.5 0.75];   %% distribuzione di probabilita, lambda_0=lambda(1), ... , lambda_3=lambda(4), lamnda_k=lambda(4) per k>3

T = 100; %numero passi simulazione

for k=1:T
    tau(k)=rand_delay(lambda);   %calcola ritardo aleatorio per ogni pacchetto in base alla funzione di probabilita' lamnda
end

%%%%%%%%%%% questa parte calcola la varianza d'errore dello stimatore ottimo
%%%%%%%%%%% partendo da k=1 fino a k=t 

for t=1:T
    P = zeros(2);
    for k=1:t
        P = A*P*A'+Q-gamma_tk(t,k,tau(k))*A*P*C'*inv(C*P*C'+R)*C*P*A';
    end
    errore(t) = P(1,1); % varianza di errore sulla posizione
end

subplot(2,1,1); 
plot(errore,'r'); hold on

%%%%%%%% questa parte inplementa lo stimatore ottimo tramite un buffer con
%%%%%%%% lunghezza finita poiche' data la distribuzione di probabilita' i
%%%%%%%% pachetti che arrivano hanno ritardo massimo finito, in questo caso dato da
%%%%%%%% length(lambda)-1=3 (attenzione lambda(k)=\lambda_{k-1})

N = length(lambda);
P = zeros(2);
for t=N:T
    P = A*P*A'+Q-gamma_tk(t,t-N+1,tau(t-N+1))*A*P*C'*inv(C*P*C'+R)*C*P*A';    % aggiornamento di P^t_{k-N+1|k-N}
    P1 = P;
    for k=N-1:-1:1
        P1 = A*P1*A'+Q-gamma_tk(t,t-k+1,tau(t-k+1))*A*P1*C'*inv(C*P1*C'+R)*C*P1*A'; % calcolo di P^t_{t+1|t} tramite iterazione sul dati del buffer
    end
    errore2(t) = P1(1,1);
end

subplot(2,1,2);
plot(errore2,'b');  % si notino come i grafici siano effettivamente identici

        
