%% Solution of Lab session #8

clear; close all
load data1_labsession8;

N=length(t);

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

% data plot
figure
subplot(211),plot(t,Y(:,1))
title('mesurements of d(t)')
subplot(212),plot(t,Y(:,2)*180/pi)
title('measurements of \alpha(t)')

%
% Extended Kalman filter
%
zk=zeros(3,N);

zk(:,1)=[0 0 0];     % z(0|0)
P=diag([1000 1000 (5/180*pi)^2]);               % P(0|0)
% try different initial conditions!

actMSE(1)=norm(X(1,:)'-zk(:,1));    % true error
estMSE(1)=sqrt(trace(P));            % error predicted by EKF
D(:,1)=diag(P);
for i=1:N-1,
    zkp=zk(:,i)+Ts*[ur(i)*cos(zk(3,i)); ur(i)*sin(zk(3,i)); ua(i)];  % xcap(i+1|i)
    F=[1 0 -Ts*ur(i)*sin(zk(3,i)); 0 1 Ts*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))]);
    D(:,i+1)=diag(P);
 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;

% comparisons
figure(3)
subplot(311),plot(t,X(:,1),'g',t,zk(1,:)','r--')
title('x(t): true (green); estimate (red)') 
subplot(312),plot(t,X(:,2),'g',t,zk(2,:)','r--')
title('y(t): true (green); estimate (red)') 
%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('estimation error on \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('Trajectories: true (blue), EKF (red), radar (light blue)')


% Comparison between true error and error predicted by EKF
figure
plot(t,actMSE,'g',t,estMSE,'r')
title('Comparison between true error (green) and EKF error estimate (red)') 

% Check confidence intervals 
for i=1:2,
 figure, plot(t,X(:,i)-zk(i,:)','r',t,3*sqrt(D(i,:)),'b--',t,-3*sqrt(D(i,:)),'b--')
 bb=axis;
 bb(3)=-sqrt(D(i,end))*20; bb(4)=-bb(3);
 axis(bb)
end
figure, plot(t,180/pi*unwrap((X(:,3)-zk(3,:)')),'r',t,3*180/pi*sqrt(D(3,:)),'b--',t,-3*180/pi*sqrt(D(3,:)),'b--')
bb=axis;
bb(3)=-sqrt(D(3,end))*20; bb(4)=-bb(3);
return
