%% Solution of Lab session on Nonlinear State estimation

clear; close all
load data1_labsession_nse;  % try also data2_labsession_nse

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
%

z0=[10 0 0]'; % z(0|-1)
P0=diag([1000 1000 (5/180*pi)^2]);  % P(0|-1)
% try different initial conditions!

zkp=z0;
Pp=P0;
for i=1:N,
    % correction
    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=zkp+K*(Y(i,:)'-[sqrt(zkp(1)^2+zkp(2)^2); atan2(zkp(2),zkp(1))]);
    % prediction
    zkp=zk+Ts*[ur(i)*cos(zk(3)); ur(i)*sin(zk(3)); ua(i)];  % xcap(i+1|i)
    F=[1 0 -Ts*ur(i)*sin(zk(3)); 0 1 Ts*ur(i)*cos(zk(3)); 0 0 1];
    Pp=F*P*F'+Q; % P(i+1|i)
    
    Xest(i,:)=zk';
    D(i,:)=diag(P)';
    diffth=min([mod(X(i,3)-Xest(i,3),2*pi) mod(Xest(i,3)-X(i,3),2*pi)]');   
    actMSE(i)=sqrt((X(i,1)-Xest(i,1))^2+(X(i,2)-Xest(i,2))^2+diffth^2);
    estMSE(i)=sqrt(trace(P));
end;

% comparisons
figure(3)
subplot(311),plot(t,X(:,1),'g',t,Xest(:,1),'r--')
title('x(t): true (green); estimate (red)') 
subplot(312),plot(t,X(:,2),'g',t,Xest(:,2),'r--')
title('y(t): true (green); estimate (red)') 
%subplot(313),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)-Xest(:,3))*180/pi,360) mod((Xest(:,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([z0(1); Xest(:,1)],[z0(2); Xest(:,2)],'r--')
plot(z0(1),z0(2),'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)-Xest(:,i),'r',t,3*sqrt(D(:,i)),'b--',t,-3*sqrt(D(:,i)),'b--')
 if i==1, title('Estimation error (red) and 3\sigma-confidence intervals (blu) for x(t)');
 else
     title('Estimation error (red) and 3\sigma-confidence intervals (blu) for y(t)');
 end
 bb=axis;
 bb(3)=-sqrt(D(end,i))*20; bb(4)=-bb(3);
 axis(bb)
end
figure, plot(t,180/pi*unwrap((X(:,3)-Xest(:,3))),'r',t,3*180/pi*sqrt(D(:,3)),'b--',t,-3*180/pi*sqrt(D(:,3)),'b--')
title('Estimation error (red) and 3\sigma-confidence intervals (blu) for \theta(t)');
bb=axis;
bb(3)=-sqrt(D(end,3))*20; bb(4)=-bb(3);
return
