
clear all; close all;
%% Simulazione

% A = [ .9 0.05; 0.05 .90]; 
% C = [ 1 0 ];
% Q = [ 0 0; 0 0.1];
% R = 0.1;
A = [ 0.9 0.1; 0 0.9]; 
C = [ 1 0 ];
Q = [ 0 0.1; 0.1 1];
R = 1;

lambda_c=1-1/max(eig(A))^2;
lambda = 0.5; % [ 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
    gamma(k)=randbin(lambda); %calcola ritardo aleatorio per ogni pacchetto in base alla funzione di probabilita' lamnda
end
[P,eigenvalues,L] = dare(A',C',Q,R); K = L';
for t=1:T
    P1 = zeros(2);
    P2 = zeros(2);
    P3 = zeros(2);
    Pl = zeros(2);
    P4 = zeros(2);
    P5 = zeros(2);
    for k=1:t
        P1 = A*P1*A'+Q-gamma(k)*A*P1*C'*inv(C*P1*C'+R)*C*P1*A';
        P2 = A*(diag([1,1])-gamma(k)*K*C)*P2*(diag([1,1])-gamma(k)*K*C)'*A'+gamma(k)*A*K*R*K'*A'+Q;
        Pl = A*Pl*A'+Q-A*Pl*C'*inv(C*Pl*C'+R)*C*Pl*A';
        P3 = gamma(k)*Pl+(1-gamma(k))*(A*P3*A'+Q);
        
        P4 = lambda*A*(diag([1,1])-K*C)*P4*(diag([1,1])-K*C)'*A'+lambda*A*K*R*K'*A'+Q+(1-lambda)*A*P4*A';
        P5 = lambda*Pl+(1-lambda)*(A*P5*A'+Q);
    end
    errore1(t) = P1(1,1);    % varianza di errore sulla posizione
    errore3(t) = P3(1,1);
    errore2(t) = P2(1,1);
    media1(t) = P4(1,1);
    media2(t) = P5(1,1);
end

plot(errore1,'r'); hold on
plot(errore2,'b');
%plot(errore3,'k');
plot(media1,'-.b');
%plot(media2,'-.k');
xlabel('Tempo (k)');
ylabel('');
legend('P_k','tilde{P}_k','E[tilde{P}_k]')
%legend('P_k','tilde{P}_k','P_k^s','E[tilde{P}_k]','E[P_k^s]')