%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% ESERCITAZIONE 1
%
% Alberton Riccardo
% Ausserer Markus
% Barazzuol Andrea
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

close all;
clear all;
clc



%%%%%%%%
% Dati
%%%%%%%%

%Lunghezza traiettoria
T=100;

%Matrici del sistema A, C
A=[1 0.1 0 0;
   0 1 0 0;
   0 0 1 0.1;
   0 0 0 1];

C=[1 0 0 0;
   0 0 1 0];


q=1;
r=1;
% Matrice di varianza di w
Q=q*0.01*[0.01 0.1 0 0;
          0.1 1 0 0;
          0 0 0.01 0.1;
          0 0 0.1 1];

% Matrice di varianza di v
R=r*[0.01 0;
     0 0.01];

 %Condizioni iniziali per filtri di Kalman
P0=eye(4);
x0=[0 0 0 0];

%%%%%%%%%%%%%%%%%%%%%%%%
%Generazione Traiettoria
%%%%%%%%%%%%%%%%%%%%%%%%

% Stato iniziale reale
x_0=[1 1 2 1];

%rumori
v=sqrt(R)*randn(2,T);
w=sqrt(Q)*randn(4,T);

[x1,x2,x3,x4,y1,y2]=Esercitazione1Ex1c(A,C, x_0, T, v, w);


%%%%%%%%%%%%%%%%%%%%%%%%%%
% 1) Filtro di Kalman (_k)
%%%%%%%%%%%%%%%%%%%%%%%%%%

% Simulazione filtro di Kalman
[x1_hat_k,x2_hat_k,x3_hat_k,x4_hat_k]=Esercitazione1Ex1b(A,C,Q,R,x0,P0,y1,y2,T);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 2) Filtro di Kalman a regime (_reg)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Calcolo dell'ARE
P_reg = dare(A', C', Q, R);

Lambda_reg = C*P_reg*C' + R; 
K_reg = P_reg*C'*inv(Lambda_reg); 

% Simulazione filtro a regime
[x1_hat_reg,x2_hat_reg,x3_hat_reg,x4_hat_reg]=Esercitazione1Ex1d(A,C,K_reg,y1,y2,x0,T);


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 3) Filtro di Kalman smoother (_smo)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Simulazione filtro di Kalman smoother
[x1_hat_smo,x2_hat_smo,x3_hat_smo,x4_hat_smo]=Esercitazione1Ex1e(A,C,Q,R,x0,P0,y1,y2,T);



%%%%%%%%%%%%%%%%%%%%%%
%Prestazione - Grafici
%%%%%%%%%%%%%%%%%%%%%%

figure;
plot([0:100],x1,'r');
hold on
plot([0:100],x1_hat_k,'b');
plot([0:100],x1_hat_reg,'g');
plot([0:100],x1_hat_smo,'k');
xlabel('t [sec]'); ylabel('x');
legend('p_x(t)','kalman di p_x(t)','kalman a regime di p_x(t)','kalman smoother di p_x(t)');
grid;

figure;
plot([0:100],x3,'r');
hold on
plot([0:100],x3_hat_k,'b');
plot([0:100],x3_hat_reg,'g');
plot([0:100],x3_hat_smo,'k');
xlabel('t [sec]'); ylabel('y');
legend('p_y(t)','kalman di p_y(t)','kalman a regime di p_y(t)','kalman smoother di p_y(t)');
grid;

figure;
plot([0:100],x2,'r');
hold on
plot([0:100],x2_hat_k,'b');
plot([0:100],x2_hat_reg,'g');
plot([0:100],x2_hat_smo,'k');
xlabel('t [sec]'); ylabel('v_x');
legend('v_x(t)','kalman di v_x(t)','kalman a regime di v_x(t)','kalman smoother di v_x(t)');
grid;

figure;
plot([0:100],x4,'r');
hold on
plot([0:100],x4_hat_k,'b');
plot([0:100],x4_hat_reg,'g');
plot([0:100],x4_hat_smo,'k');
xlabel('t [sec]'); ylabel('v_y');
legend('v_y(t)','kalman di v_y(t)','kalman a regime di v_y(t)','kalman smoother di v_y(t)');
grid;


e1_k = zeros(1,T+1);
e2_k = zeros(1,T+1);
e1_reg = zeros(1,T+1);
e2_reg = zeros(1,T+1);
e1_smo = zeros(1,T+1);
e2_smo = zeros(1,T+1);

%calcolo errori di stima
for t = 1:T+1
    e1_k(t) = y1(t) - C(1,:)*[x1_hat_k(t);x2_hat_k(t);x3_hat_k(t);x4_hat_k(t)];
    e2_k(t) = y2(t) - C(2,:)*[x1_hat_k(t);x2_hat_k(t);x3_hat_k(t);x4_hat_k(t)];
    e1_reg(t) = y1(t) - C(1,:)*[x1_hat_reg(t);x2_hat_reg(t);x3_hat_reg(t);x4_hat_reg(t)];
    e2_reg(t) = y2(t) - C(2,:)*[x1_hat_reg(t);x2_hat_reg(t);x3_hat_reg(t);x4_hat_reg(t)];
    e1_smo(t) = y1(t) - C(1,:)*[x1_hat_smo(t);x2_hat_smo(t);x3_hat_smo(t);x4_hat_smo(t)];
    e2_smo(t) = y2(t) - C(2,:)*[x1_hat_smo(t);x2_hat_smo(t);x3_hat_smo(t);x4_hat_smo(t)];
end

%Calcolo varianze campionarie, non considerando transitorio
disp('varianza dell`errore di posizione sull`asse x per filtro di kalman');
var(e1_k(10:T-9))
disp('varianza dell`errore di posizione sull`asse x per filtro di kalman a regime');
var(e1_reg(10:T-9))
disp('varianza dell`errore di posizione sull`asse x per smoother di kalman');
var(e1_smo(10:T-9))
disp('varianza dell`errore di posizione sull`asse x per filtro di kalman');
var(e2_k(10:T-9))
disp('varianza dell`errore di posizione sull`asse x per filtro di kalman a regime');
var(e2_reg(10:T-9))
disp('varianza dell`errore di posizione sull`asse x per smoother di kalman');
var(e2_smo(10:T-9))


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 4) Plot posizione car reale - virtuale (usando Kalman smoother)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

N = 100;                            % numero di punti
t = linspace(0,10,N);               % tempo

% Visualizzo il piano x-y
figure;
hold on; 
axis equal; 
axis([0 15 0 15]); 
grid on;
title('Veicolo reale blu - Veicolo virtuale rosso');
xlabel('P_x');
ylabel('P_y');

% Inizializzo la varianza dell'errore di stima
Pt = P0;    

for n=1:N
    Pt=A*(Pt-Pt*C'*inv(C*Pt*C'+ R)*C*Pt)*A'+Q;
    P_pos=[Pt(1,1) Pt(1,3); Pt(3,1) Pt(3,3)];
    
    % Traiettoria veicolo reale
    carR = plot_car(x1(n),x3(n),atan2((x3(n+1)-x3(n)),( x1(n+1)- x1(n))),0.3,'b');    
    carR_bar = plot_bar(x1(n),x3(n)); 

    % Traiettoria veicolo virtuale
    carV = plot_car(x1_hat_smo(1,n),x3_hat_smo(1,n),atan2((x4_hat_smo(1,n)),(x2_hat_smo(1,n))),0.3,'r');
    carV_bar = plot_bar(x1_hat_smo(1,n),x3_hat_smo(1,n));

    % Sovrappongo al veicolo virtuale il grafico dell'elisse di confidenza
    % utilizzando la funzione data: plot_ellipse.m
    h = plot_ellipse(x1_hat_smo(n),x3_hat_smo(n),9*P_pos);
    
    pause(0.05); 
    delete(h); delete(carR); delete(carV); delete(carR_bar); delete(carV_bar);
end

for n=1:N
    
    Pt=A*(Pt-Pt*C'*inv(C*Pt*C'+ R)*C*Pt)*A'+Q;
    P_pos=[Pt(1,1) Pt(1,3); Pt(3,1) Pt(3,3)];
    
    %lascio disegnate le distanze tra i due baricentri
    if n>1 && n<(N-1)
        abs_dist=sqrt((x1(n)-x1_hat_smo(1,n))^2+(x3(n)-x3_hat_smo(1,n))^2);
        dist=plot_dist(x1_hat_smo(1,n),x3_hat_smo(1,n),x1(n),x3(n));
        
        dirR=plot_dir(x1(n),x3(n),atan2((x3(n+1)-x3(n)),( x1(n+1)- x1(n))),abs_dist/2,'b');
        dirV=plot_dir(x1(n),x3(n),atan2((x4_hat_smo(1,n)),(x2_hat_smo(1,n))),abs_dist/2,'r');
        plot(x1(n),x3(n),'b.');
        plot(x1_hat_smo(1,n),x3_hat_smo(1,n),'r.');
    end
    
end



































