% Identificazione e analisi dei dati 2: Esercitazione #3
% Filtro di Kalman Esteso per la localizzazione di un robot mobile

clear; close all
load dati1;

N=length(t);
T=t(end);

ur=U(:,1);
ua=U(:,2);

% plot dati
figure
subplot(211),plot(t,Y(:,1))
title('misure di d(t)')
subplot(212),plot(t,Y(:,2)*180/pi)
title('misure di \alpha(t)')

%
% filtro di kalman ricorsivo
%
zk=zeros(3,N);

zk(:,1)=[0 0 0];     % z(0|0)
P=diag([10 10 (5/180*pi)^2]);               % P(0|0)
% provare diverse condizioni iniziali!

actMSE(1)=norm(X(1,:)'-zk(:,1));    % MSE vero
estMSE(1)=sqrt(trace(P));            % MSE predetto dal filtro

for i=1:N-1,
    zkp=zk(:,i)+Tc*[ur(i)*cos(zk(3,i)); ur(i)*sin(zk(3,i)); ua(i)];  % xcap(i+1|i)
    F=[1 0 -Tc*ur(i)*sin(zk(3,i)); 0 1 Tc*ur(i)*cos(zk(3,i)); 0 0 1];
    Pp=F*P*F'+Q; % P(i+1|i)
    Hden=sqrt(zkp(1)^2+zkp(2)^2);
    H=[zkp(1)/Hden zkp(2)/Hden 0; -zkp(2)/Hden^2 zkp(1)/Hden^2 0];
    K=Pp*H'*inv(H*Pp*H'+R);
    P=Pp-K*H*Pp;
    zk(:,i+1)=zkp+K*(Y(i+1,:)'-[sqrt(zkp(1)^2+zkp(2)^2); atan2(zkp(2),zkp(1))]);
 
 diffth=min([mod((X(i,3)-(zk(3,i)')),2*pi) mod(((zk(3,i)'-X(i,3))),2*pi)]');   
 actMSE(i+1)=sqrt((X(i,1)-zk(1,i))^2+(X(i,2)-zk(2,i))^2+diffth^2);
 estMSE(i+1)=sqrt(trace(P));
end;

% confronti
figure(3)
subplot(311),plot(t,X(:,1),'g',t,zk(1,:)','r--')
title('x(t): vera (ver) e stimata (ros)') 
subplot(312),plot(t,X(:,2),'g',t,zk(2,:)','r--')
title('y(t): vera (ver) e stimata (ros)') 
%subplot(223),plot(t,mod(X(:,3)*180/pi,360),'g',t,mod((zk(3,:)')*180/pi,360),'r--')
%title('\theta(t): vera (ver) e stimata (ros)') 
subplot(313),plot(t,min([mod((X(:,3)-(zk(3,:)'))*180/pi,360) mod(((zk(3,:)'-X(:,3)))*180/pi,360)]'),'r')
title('errore di stima su \theta(t)') 
% traiettoria
figure,
plot([Y(:,1).*cos(Y(:,2))],[Y(:,1).*sin(Y(:,2))],'c:') % radar!
hold on
plot(X(:,1),X(:,2),'b') 
plot(X(1,1),X(1,2),'bo')
plot(zk(1,:),zk(2,:),'r--')
plot(zk(1,1),zk(2,1),'ro')
title('Traiettorie: vera (blu), EKF (ros), radar (cel)')


% confronto tra MSE vero e predetto dal filtro
figure
plot(t,actMSE,'g',t,estMSE,'r')
title('Confronto tra MSE vero (ver) e predetto (ros)') 

return
