                        %% PLOTs for Dynamic User scenario %%

%Commentare la tecnica che NON si sta analizzando
% A='LS';
A='KF';

    %3D POSITION ERROR
figure
plot(sol,'bo')
title('3D Error')
ylabel('\epsilon [meters]')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter measurement activation')
    annotation('textbox','String',{'50 Prc alt: ' num2str(palt50)},'position',[.5 .8 .1 .1],'FitBoxToText','on')
    annotation('textbox','String',{'95 Prc alt: ' num2str(palt95)},'position',[.5 .7 .1 .1],'FitBoxToText','on')
    annotation('textbox','String',{'99 Prc alt: ' num2str(palt99)},'position',[.5 .6 .1 .1],'FitBoxToText','on')
end
% ylim([0 100])
% xlim([5940 6060])
annotation('textbox','String',{'50 Percentile: ' num2str(prc50)},'position',[.8 .8 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'95 Percentile: ' num2str(prc95)},'position',[.8 .7 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'99 Percentile: ' num2str(prc99)},'position',[.8 .6 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'50 Prc no tr: ' num2str(pnotr50)},'position',[.8 .4 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'95 Prc no tr: ' num2str(pnotr95)},'position',[.8 .3 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'99 Prc no tr: ' num2str(pnotr99)},'position',[.8 .2 .1 .1],'FitBoxToText','on')

    %3D VELOCITY ERROR
figure
if strcmp(A,'LS')
    plot(sqrt((vx_fin-Mu(1:end-1,4)).^2+(vy_fin-Mu(1:end-1,5)).^2+(vz_fin-Mu(1:end-1,6)).^2),'bo')
else
    plot(sqrt((errvx).^2+(errvy).^2+(errvz).^2),'bo')
end
title('3D Velocity Error')
ylabel('Velocity [m/s]')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter measurement activation')
end
% ylim([0 70])
% xlim([5940 6060])

    % COMPONENT POSITION ERROR
figure
sgtitle('\bf Position components error')
subplot(3,1,1)
plot(errx,'o')
ylabel('x error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 70])
% xlim([5940 6060])
subplot(3,1,2)
plot(erry,'o')
ylabel('y error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 70])
% xlim([5940 6060])
subplot(3,1,3)
plot(errz,'o')
ylabel('z error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 70])
% xlim([5940 6060])

    % COMPONENT VELOCITY ERROR
figure
sgtitle('\bf Velocity components error')
subplot(3,1,1)
if strcmp(A,'LS')
    plot(vx_fin-Mu(1:end-1,4),'o')
else
    plot(errvx,'o')
end
ylabel('vx error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 50])
% xlim([5940 6060])
subplot(3,1,2)
if strcmp(A,'LS')
    plot(vy_fin-Mu(1:end-1,5),'o')
else
    plot(errvy,'o')
end
ylabel('vy error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 50])
% xlim([5940 6060])
subplot(3,1,3)
if strcmp(A,'LS')
    plot(vz_fin-Mu(1:end-1,6),'o')
else
    plot(errvz,'o')
end
ylabel('vz error')
xlabel('time of analysis [sec]')
if strcmp(A,'KF') && ~isempty(alt_time)
    hold on
    xline(alt_time(1),'r','Altimeter')
end
% ylim([0 50])
% xlim([5940 6060])

%% Profiles
figure
subplot(2,1,1)
plot(vel_app,'mo')
hold on
plot(vel_u,'co')
title('Velocity Profile')
ylabel('velocity [m/s]')
xlabel('time of analysis [sec]')
legend('approximation','user')
ylim([0 2000])
% xlim([5900 6060])
subplot(2,1,2)
plot(-acc_app,'yo')
hold on
plot(acc_u,'go')
title('Acceleration Profile')
ylabel('Acceleration [m/s^2]')
xlabel('time of analysis [sec]')
legend('approximation','user')
ylim([-30 30])
% xlim([5900 6060])

%% CDF
figure
cdfplot(sol)
ylabel('Cumulative Distribution Function')
xlabel('Error values')
hold on
yline(0.50,'-.b')
xline(prc50,'-.r','50 prc value')
yline(0.95,'-.b')
xline(prc95,'-.r','95 prc value')
yline(0.99,'-.b')
xline(prc99,'-.r','99 prc value')

figure
cdfplot(sol(1:5950))
title('CDF for errors until altimeter activation')
ylabel('Cumulative Distribution Function')
xlabel('Error values')
hold on
yline(0.50,'-.b')
xline(pnoalt50,'-.r','50 prc value')
yline(0.95,'-.b')
xline(pnoalt95,'-.r','95 prc value')
yline(0.99,'-.b')
xline(pnoalt99,'-.r','99 prc value')

figure
cdfplot(sol(5950:end))
title('CDF for errors after altimeter activation')
ylabel('Cumulative Distribution Function')
xlabel('Error values')
hold on
yline(0.50,'-.b')
xline(palt50,'-.r','50 prc value')
yline(0.95,'-.b')
xline(palt95,'-.r','95 prc value')
yline(0.99,'-.b')
xline(palt99,'-.r','99 prc value')

% %% Horizontal e Vertical
% figure
% subplot(2,1,1)
% plot(hor,'bo')
% title('Horizontal Error')
% ylabel('\epsilon [meters]')
% xlabel('time of analysis [sec]')
% % ylim([0 70])
% subplot(2,1,2)
% plot(ver,'go')
% title('Vertical Error')
% ylabel('\epsilon [meters]')
% xlabel('time of analysis [sec]')
% % ylim([0 120])

%% TRUE TRAJECTORY %%
addpath('Codici_Finali\Dynamic_User_scenario\Variables');
load('xLS.mat');    load('yLS.mat');    load('zLS.mat');
load('xKF.mat');    load('yKF.mat');    load('zKF.mat');
load('xSF.mat');    load('ySF.mat');    load('zSF.mat');
load('Mu.mat')

figure
plot(sqrt(Mu(:,1).^2+Mu(:,2).^2),'k-*')
hold on
plot(sqrt(xLS.^2+yLS.^2),'bo')
plot(sqrt(xKF.^2+yKF.^2),'go')
plot(sqrt(xSF.^2+ySF.^2),'ro')
title('Horizontal trajectory')
ylabel('[meters]')
xlabel('time of analysis [sec]')
xlim([6020,6050])
legend('user','LS','EKF','SF')

figure
plot(Mu(:,3),'k-*')
hold on
plot(zLS,'bo')
plot(zKF,'go')
plot(zSF,'ro')
title('Vertical trajectory')
ylabel('[meters]')
xlabel('time of analysis [sec]')
xlim([6020,6050])
legend('user','LS','EKF','SF')