         %% MOON - Static Positioning with EKF with dynamic model %%
clear
close all
clc
addpath('Codici_Finali\Static_User_scenario\Variables');
load('data1.mat');  load('data2.mat');  load('data3.mat');  load('data4.mat');
load('M1.mat'); load('M2.mat'); load('M3.mat'); load('M4.mat');
addpath('Codici_Finali\Functions');
load('uLS.mat');

tic
% 1. Check Elevation
% 2. Slant Range + Errors
% 3. KF
r_moon=1737.4e3; %Moon radius [m]
c=299792458;
ind_t=1; i=1;
n=4; %nsat
%user fixed position = South Pole
user=[0,0,-r_moon];
lon=0;
lat=-pi/2;
u_vect=[cos(lon)*cos(lat), sin(lon)*cos(lat), sin(lat)];

                                %ERRORS
%Satellite position: AWGN with zero mean and standard deviation sigma
sigma_sat=10; %standard deviation [m]
noise1=sigma_sat*randn(length(data1),1); %randi(100,length(data1),1);
noise2=sigma_sat*randn(length(data1),1); %randi(100,length(data1),1);
noise3=sigma_sat*randn(length(data1),1); %randi(100,length(data1),1);
noise4=sigma_sat*randn(length(data1),1); %randi(100,length(data1),1);

%Clock Error values
sigma0=1e-10; %[sec]
sigma1=9.486e-12;
sigma2=1.643e-17;
Delta=60; %[sec]

%Multipah values
a_mp=0.1633;
b_mp=1.1846;
c_mp=-0.0511;

% Initializations
u_start=zeros(8,1); %State vector [x,v_x,y,v_y,z,v_z,cdt,d(cdt)]
u_start([1,3,5])=uLS;
P_start=(300^2)*eye(length(u_start));
TOW=nan(length(data1),1);   av=nan(length(data1),1);    e_ck=nan(length(data1),1);
x_err=nan(length(data1),1); y_err=nan(length(data1),1);
z_err=nan(length(data1),1); t_fin=nan(length(data1),1);
SR_m=nan(length(data1),4);  SR_n=nan(length(data1),4);

while ind_t<=length(data1)-1000
    flag=0;
%     disp(['Iteration: ' num2str(ind_t)])
    while flag==0
        TOW(ind_t)=Delta*(ind_t-1);
        %line-of-sight vectors
        rho1=(M1(ind_t,1:3)-user)/norm(M1(ind_t,1:3)-user);
        rho2=(M2(ind_t,1:3)-user)/norm(M2(ind_t,1:3)-user);
        rho3=(M3(ind_t,1:3)-user)/norm(M3(ind_t,1:3)-user);
        rho4=(M4(ind_t,1:3)-user)/norm(M4(ind_t,1:3)-user);
        %elevations
        E(1)=rad2deg(asin(dot(rho1,u_vect)));
        E(2)=rad2deg(asin(dot(rho2,u_vect)));
        E(3)=rad2deg(asin(dot(rho3,u_vect)));
        E(4)=rad2deg(asin(dot(rho4,u_vect)));
        for j=1:n
            if E(j)<5
                flag=0;
                u_start=zeros(8,1);
                u_start([1,3,5])=uLS;
                P_start=(300^2)*eye(length(u_start));
                av(ind_t)=0;
                x_err(ind_t)=nan;
                y_err(ind_t)=nan;
                z_err(ind_t)=nan;
                ind_t=ind_t+1;
                break
            else 
                flag=1; %EL di tutti i satelliti è >5 deg
                av(ind_t)=1;
            end
        end
    end 
    
    % Errors
    % Clock: SDE -> azzerare t ogni 12 ore
    t=TOW(ind_t);
    while t>43200
        t=t-43200;
    end
    e_ck(ind_t)=sigma0+sqrt(sigma1^2*t*Delta+(sigma2^2*(t*Delta)^3)/3);
    % Multipah
    e_mp=a_mp+b_mp*exp(c_mp*E);
    % Total errors
    err1=noise1(ind_t)+c*e_ck(ind_t)+e_mp(1);
    err2=noise2(ind_t)+c*e_ck(ind_t)+e_mp(2);
    err3=noise3(ind_t)+c*e_ck(ind_t)+e_mp(3);
    err4=noise4(ind_t)+c*e_ck(ind_t)+e_mp(4);
    
    % Slant Range master
    SR(1)=sqrt((M1(ind_t,1)-user(1))^2+(M1(ind_t,2)-user(2))^2+(M1(ind_t,3)-user(3))^2);
    SR(2)=sqrt((M2(ind_t,1)-user(1))^2+(M2(ind_t,2)-user(2))^2+(M2(ind_t,3)-user(3))^2);
    SR(3)=sqrt((M3(ind_t,1)-user(1))^2+(M3(ind_t,2)-user(2))^2+(M3(ind_t,3)-user(3))^2);
    SR(4)=sqrt((M4(ind_t,1)-user(1))^2+(M4(ind_t,2)-user(2))^2+(M4(ind_t,3)-user(3))^2);
    
    % KF
    %DT tra epoca corrente e precedente
    DT=60; %1 minuto 
    
    u_approx=u_start;
    P_approx=P_start;

    %MATRICES    
    %State Tansition Matrix
    F_sub=[1 DT;0 1];     %linear model - costant velocity   
    F=zeros(length(u_start));
    row=1;
    while row<length(F)
       F(row:row+1,row:row+1)=F_sub;
       row=row+2;
    end
    %State error autocovariance matrix - Process Noise
    sigma_axyz=1e-3*ones(3,1); %1e-3
    sigma_t=1e-4; %1e-4
    sigma_dt=1e-4; %1e-4
    
    G=[(DT^2)/2 0 0; DT 0 0; 0 (DT^2)/2 0; 0 DT 0; 0 0 (DT^2)/2; 0 0 DT];
    acc=(sigma_axyz.^2).*eye(3);
    Q=G*acc*G';
    %Discreto
    Q_dt=[(sigma_t^2)+(DT^4)*(sigma_dt^2)/4 (DT^3)*(sigma_dt^2)/2;
        (DT^3)*(sigma_dt^2)/2 (DT^2)*(sigma_dt^2)];  
%     %Continuo
%     Q_dt=[DT*(sigma_t^2)+(DT^5)*(sigma_dt^2)/20 (DT^4)*(sigma_dt^2)/8;
%         (DT^4)*(sigma_dt^2)/8 (DT^3)*(sigma_dt^2)/3];
    Q(7:8,7:8)=Q_dt;
    
    %Measurement error autocovariance matrix
    sigma_rho=1e3; %1e3
    R=(sigma_rho^2)*eye(length(SR));

    x=[M1(ind_t,1),M2(ind_t,1),M3(ind_t,1),M4(ind_t,1)];
    y=[M1(ind_t,2),M2(ind_t,2),M3(ind_t,2),M4(ind_t,2)];
    z=[M1(ind_t,3),M2(ind_t,3),M3(ind_t,3),M4(ind_t,3)];
        
    %Measurement Matrix
    ax=-(x-u_approx(1))./(sqrt((x-u_approx(1)).^2+(y-u_approx(3)).^2+(z-u_approx(5)).^2));
    ay=-(y-u_approx(3))./(sqrt((x-u_approx(1)).^2+(y-u_approx(3)).^2+(z-u_approx(5)).^2));
    az=-(z-u_approx(5))./(sqrt((x-u_approx(1)).^2+(y-u_approx(3)).^2+(z-u_approx(5)).^2));
    col_vel=zeros(n,1);
    H=[ax',col_vel,ay',col_vel,az',col_vel,ones(n,1),col_vel]; 
    
    SR_app=sqrt((x-u_approx(1)).^2+(y-u_approx(3)).^2+(z-u_approx(5)).^2);
%     SR_app(1)=sqrt((x(1)-u_approx(1))^2+(y(1)-u_approx(3))^2+(z(1)-u_approx(5))^2)+err1;
%     SR_app(2)=sqrt((x(2)-u_approx(1))^2+(y(2)-u_approx(3))^2+(z(2)-u_approx(5))^2)+err2;
%     SR_app(3)=sqrt((x(3)-u_approx(1))^2+(y(3)-u_approx(3))^2+(z(3)-u_approx(5))^2)+err3;
%     SR_app(4)=sqrt((x(4)-u_approx(1))^2+(y(4)-u_approx(3))^2+(z(4)-u_approx(5))^2)+err4;
    
    %sfera di incertezza
    [xp(1),yp(1),zp(1)]=myspheresat(x(1),y(1),z(1),c*e_ck(ind_t)+noise1(ind_t));
    [xp(2),yp(2),zp(2)]=myspheresat(x(2),y(2),z(2),c*e_ck(ind_t)+noise2(ind_t));
    [xp(3),yp(3),zp(3)]=myspheresat(x(3),y(3),z(3),c*e_ck(ind_t)+noise3(ind_t));
    [xp(4),yp(4),zp(4)]=myspheresat(x(4),y(4),z(4),c*e_ck(ind_t)+noise4(ind_t));
    for s=1:n
        [xu(s),yu(s),zu(s)]=mysphere(user(1),user(2),user(3),e_mp(s));
    end
    SR_master=sqrt((x-user(1)).^2+(y-user(2)).^2+(z-user(3)).^2);
    SR_new=sqrt((xp-xu).^2+(yp-yu).^2+(zp-zu).^2);
        
                %%% UPDATE %%
    %Time update portion
    u_start1=F*u_approx;
    P_start1=F*P_approx*F'+Q; 
    %Kalman Gain
    K=(P_start1*H')*inv(H*P_start1*H'+R);
    %Measurement update portion
    z_meas=SR_new-SR_app;
    u_start=u_start1+K*(z_meas'); %-H*u_start1);
    P_start=P_start1-K*H*P_start1;
        
    x_err(ind_t)=u_start(1);
    y_err(ind_t)=u_start(3);
    z_err(ind_t)=u_start(5);
    t_fin(ind_t)=u_start(7)/c;
    
    SR_m(ind_t,:)=SR_master;
    SR_n(ind_t,:)=SR_new;
    
    clear rho1 rho2 rho3 rho4 E SR SR_app
    clear x y z u_start1 P_start1 ax ay az H K R Q
    ind_t=ind_t+1;
end
hor=sqrt((x_err-user(1)).^2+(y_err-user(2)).^2);
ver=sqrt((z_err-user(3)).^2);
sol=sqrt((x_err-user(1)).^2+(y_err-user(2)).^2+(z_err-user(3)).^2);

%Percentile calculations
[hprc50,hprc95,hprc99,vprc50,vprc95,vprc99]=prc(hor,ver,30);
%Delete transitory terms
prc50=prctile(sol,50);  %pnotr50=prctile(sol(200:594),50);  
prc95=prctile(sol,95);  %pnotr95=prctile(sol(200:594),95);
prc99=prctile(sol,99);  %pnotr99=prctile(sol(200:594),99);    
[pnotr50,pnotr95,pnotr99]=percentiles(sol,30);

toc

%% hor e ver
close all

figure
subplot(2,1,1)
plot(hor,'b--o')
title('Horizontal Error')
ylabel('\epsilon [meters]')
xlabel('time of analysis [min]')
xlim([0 1e4])
ylim([0 10])
annotation('textbox','String',{'Hor 50 Prc: ' num2str(mean(hprc50))},'position',[.92 .8 .1 .1],'EdgeColor','b','FitBoxToText','on')
annotation('textbox','String',{'Hor 95 Prc: ' num2str(mean(hprc95))},'position',[.92 .7 .1 .1],'EdgeColor','b','FitBoxToText','on')
annotation('textbox','String',{'Hor 99 Prc: ' num2str(mean(hprc99))},'position',[.92 .6 .1 .1],'EdgeColor','b','FitBoxToText','on')
subplot(2,1,2)
plot(ver,'g--o')
title('Vertical Error')
ylabel('\epsilon [meters]')
xlabel('time of analysis [min]')
xlim([0 1e4])
ylim([0 10])
annotation('textbox','String',{'Ver 50 Prc: ' num2str(mean(vprc50))},'position',[.92 .32 .1 .1],'EdgeColor','g','FitBoxToText','on')
annotation('textbox','String',{'Ver 95 Prc: ' num2str(mean(vprc95))},'position',[.92 .22 .1 .1],'EdgeColor','g','FitBoxToText','on')
annotation('textbox','String',{'Ver 99 Prc: ' num2str(mean(vprc99))},'position',[.92 .12 .1 .1],'EdgeColor','g','FitBoxToText','on')

%% 3d error
figure
plot(sol,'--o')
title('3D Error')
ylabel('\epsilon [meters]')
xlabel('time of analysis [min]')
ylim([0 15])
xlim([0 1e4])
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(max(pnotr50))},'position',[.8 .4 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'95 Prc no tr: ' num2str(max(pnotr95))},'position',[.8 .3 .1 .1],'FitBoxToText','on')
annotation('textbox','String',{'99 Prc no tr: ' num2str(max(pnotr99))},'position',[.8 .2 .1 .1],'FitBoxToText','on')

%% cdf e SRerror
figure
cdfplot(hor)
hold on
cdfplot(ver)
ylabel('Cumulative Distribution Function')
xlabel('Error values')
legend('Horizontal CDF','Vertical CDF')

figure
sgtitle('\bf SR_{master} - SR_{errors} for each satellite')
subplot(2,2,1)
plot(SR_m(:,1)-SR_n(:,1),'b.')
xlabel('time of analysis [min]')
xlim([0 1e4])
% ylim([-30 30])
title('Satellite 1')
subplot(2,2,2)
plot(SR_m(:,2)-SR_n(:,2),'b.')
xlabel('time of analysis [min]')
xlim([0 1e4])
% ylim([-30 30])
title('Satellite 2')
subplot(2,2,3)
plot(SR_m(:,3)-SR_n(:,3),'b.')
xlabel('time of analysis [min]')
xlim([0 1e4])
% ylim([-30 30])
title('Satellite 3')
subplot(2,2,4)
plot(SR_m(:,4)-SR_n(:,4),'b.')
xlabel('time of analysis [min]')
xlim([0 1e4])
% ylim([-30 30])
title('Satellite 4')
