           %% MOON - Dynamic Positioning with Dynamic Kalman Filter (NCV model)%%
clear
clc
addpath('Codici_Finali\Dynamic_User_scenario\Variables');
load('datau.mat');  load('data1.mat');  load('data2.mat');  load('data3.mat');  load('data4.mat');
load('Mu.mat');  load('M1.mat'); load('M2.mat'); load('M3.mat'); load('M4.mat');
DT=diff(str2double(data1(:,2)));
addpath('Codici_Finali\Dynamic_User_scenario\Functions');

tic

% 1. Check User under Satellites
% 2. Slant Range + Noise
% 3. KF
r_moon=1737.4e3; %Moon radius [m]
ind_t=2;
n=4; %nsat
c=299792458;

                                    %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=1; %[sec]

%Initializations
u_start=zeros(12,1); %State vector [x,v_x,a_x,y,v_y,a_y,z,v_z,a_z,cdt,d(cdt),d(d(cdt))]
load('u_start_LS')
u_start([1,4,7])=u_LS; %initial position = LS 1st iteration
P_start=(300^2)*eye(length(u_start));
av=nan(length(data1),1);    e_ck=nan(length(data1),1);
x_fin=nan(length(data1),1); y_fin=nan(length(data1),1);
z_fin=nan(length(data1),1); t_fin=nan(length(data1),1);
vx_fin=av;  vy_fin=av;  vz_fin=av;
SR_m=nan(length(data1),4);  SR_n=nan(length(data1),4);
alt_time=[];

while ind_t<=length(data1)
    ind_k=ind_t-1;  %indice dell'epoca k-1 (per il calcolo del passo k)
    % Check User under Satellites
    flag=0;
    while flag==0   
        if norm(Mu(ind_t,:))<norm(M1(ind_t,:)) && norm(Mu(ind_t,:))<norm(M2(ind_t,:)) && ...
                norm(Mu(ind_t,:))<norm(M3(ind_t,:)) && norm(Mu(ind_t,:))<norm(M4(ind_t,:))
            flag=1;
            av(ind_t)=1;
        else
            flag=0;
            u_start=zeros(12,1);
            u_start([1,4,7])=u_LS;
            av(ind_t)=0;
            x_fin(ind_t)=nan;
            y_fin(ind_t)=nan;
            z_fin(ind_t)=nan;
            ind_t=ind_t+1;
        end
    end
    
    % Errors
    % Clock: SDE -> azzerare t ogni 12 ore
    t=str2double(data1(ind_t,2));
    while t>43200
        t=t-43200;
    end
    e_ck(ind_t)=sigma0+sqrt(sigma1^2*t*Delta+(sigma2^2*(t*Delta)^3)/3);
    % Total errors
    err1=noise1(ind_t)+c*e_ck(ind_t);
    err2=noise2(ind_t)+c*e_ck(ind_t);
    err3=noise3(ind_t)+c*e_ck(ind_t);
    err4=noise4(ind_t)+c*e_ck(ind_t);
    
    % Slant Range Master
    SR(1)=sqrt((M1(ind_k,1)-Mu(ind_k,1))^2+(M1(ind_k,2)-Mu(ind_k,2))^2+(M1(ind_k,3)-Mu(ind_k,3))^2);
    SR(2)=sqrt((M2(ind_k,1)-Mu(ind_k,1))^2+(M2(ind_k,2)-Mu(ind_k,2))^2+(M2(ind_k,3)-Mu(ind_k,3))^2);
    SR(3)=sqrt((M3(ind_k,1)-Mu(ind_k,1))^2+(M3(ind_k,2)-Mu(ind_k,2))^2+(M3(ind_k,3)-Mu(ind_k,3))^2);
    SR(4)=sqrt((M4(ind_k,1)-Mu(ind_k,1))^2+(M4(ind_k,2)-Mu(ind_k,2))^2+(M4(ind_k,3)-Mu(ind_k,3))^2);
    
    % KF   
    u_approx=u_start;
    P_approx=P_start;

    %MATRICES    
    %State Tansition Matrix
    F_sub=[1 DT(ind_k) 0; 0 1 0; 0 0 0];
%     F_sub=[1 DT(ind_k) (DT(ind_k)^2)/2; 0 1 DT(ind_k); 0 0 1];
    F=zeros(length(u_start));
    row=1;
    while row<length(F)
        F(row:row+2,row:row+2)=F_sub;
        row=row+3;
    end
    %State error autocovariance matrix - Process Noise
    sigma_xyz=1e-2*ones(3,1);   %ACC: 1e1 / VEL: 1e-2
    sigma_t=1;
    sigma_dt=1;
    
    G=[(DT(ind_k)^2)/2 0 0 0; DT(ind_k) 0 0 0; 0 0 0 0; 0 (DT(ind_k)^2)/2 0 0; 0 DT(ind_k) 0 0; 0 0 0 0; 0 0 (DT(ind_k)^2)/2 0; 0 0 DT(ind_k) 0; 0 0 0 0; 0 0 0 (DT(ind_k)^2)/2; 0 0 0 DT(ind_k); 0 0 0 0];
%     G=[(DT(ind_k)^3)/6 0 0 0; (DT(ind_k)^2)/2 0 0 0; DT(ind_k) 0 0 0; 0 (DT(ind_k)^3)/6 0 0; 0 (DT(ind_k)^2)/2 0 0; 0 DT(ind_k) 0 0; 0 0 (DT(ind_k)^3)/6 0; 0 0 (DT(ind_k)^2)/2 0; 0 0 DT(ind_k) 0; 0 0 0 (DT(ind_k)^3)/6; 0 0 0 (DT(ind_k)^2)/2; 0 0 0 DT(ind_k)];
    sigMat=(sigma_xyz.^2).*eye(3); sigMat(4,4)=sigma_dt^2;
    Q=G*sigMat*G';
    Q(10,10)=Q(10,10)+sigma_t^2;
    
    %Measurement error autocovariance matrix
    sigma_rho=1e-2; %ACC: 1e3 / VEL: 1e-2
    R=(sigma_rho^2)*eye(length(SR));

    x=[M1(ind_k,1),M2(ind_k,1),M3(ind_k,1),M4(ind_k,1)];
    y=[M1(ind_k,2),M2(ind_k,2),M3(ind_k,2),M4(ind_k,2)];
    z=[M1(ind_k,3),M2(ind_k,3),M3(ind_k,3),M4(ind_k,3)];
        
    %Measurement Matrix
    ax=-(x-u_approx(1))./(sqrt((x-u_approx(1)).^2+(y-u_approx(4)).^2+(z-u_approx(7)).^2));
    ay=-(y-u_approx(4))./(sqrt((x-u_approx(1)).^2+(y-u_approx(4)).^2+(z-u_approx(7)).^2));
    az=-(z-u_approx(7))./(sqrt((x-u_approx(1)).^2+(y-u_approx(4)).^2+(z-u_approx(7)).^2));
    c0=zeros(n,1);
    H=[ax',c0,c0,ay',c0,c0,az',c0,c0,c0,c0,c0];
%     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));
%     c0=zeros(n,1);
%     H=[ax',c0,ay',c0,az',c0,c0,c0]; 
    
    SR_app(1)=sqrt((x(1)-u_approx(1))^2+(y(1)-u_approx(4))^2+(z(1)-u_approx(7))^2)+err1;
    SR_app(2)=sqrt((x(2)-u_approx(1))^2+(y(2)-u_approx(4))^2+(z(2)-u_approx(7))^2)+err2;
    SR_app(3)=sqrt((x(3)-u_approx(1))^2+(y(3)-u_approx(4))^2+(z(3)-u_approx(7))^2)+err3;
    SR_app(4)=sqrt((x(4)-u_approx(1))^2+(y(4)-u_approx(4))^2+(z(4)-u_approx(7))^2)+err4;
%     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;
    
            %%% 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-SR_app;
    u_start=u_start1+K*(z_meas');
    P_start=P_start1-K*H*P_start1;
    
    x_fin(ind_t)=u_start(1);    vx_fin(ind_t)=u_start(2);  
    y_fin(ind_t)=u_start(4);    vy_fin(ind_t)=u_start(5);
    z_fin(ind_t)=u_start(7);    vz_fin(ind_t)=u_start(8);
    t_fin(ind_t)=u_start(10)/c;
%     x_fin(ind_t)=u_start(1);
%     y_fin(ind_t)=u_start(3);
%     z_fin(ind_t)=u_start(5);
%     t_fin(ind_t)=u_start(7)/c;
    
    clear SR SR_app x y z u_start1 P_start1 ax ay az H K R Q
    ind_t=ind_t+1;
end
errx=abs(x_fin-Mu(:,1)); errvx=abs(vx_fin-Mu(:,4));
erry=abs(y_fin-Mu(:,2)); errvy=abs(vy_fin-Mu(:,5));
errz=abs(z_fin-Mu(:,3)); errvz=abs(vz_fin-Mu(:,6));
sol=sqrt((errx).^2+(erry).^2+(errz).^2);
%horizontal & vertical error
hor=sqrt(errx.^2+erry.^2);
ver=sqrt(errz.^2);

% user profile
vel_app=sqrt((vx_fin).^2+(vy_fin).^2+(vz_fin).^2);
acc_app=diff(vel_app)./DT;
vel_u=sqrt((Mu(:,4)).^2+(Mu(:,5)).^2+(Mu(:,6)).^2);
acc_u=(vel_u(2:end)-vel_u(1:end-1))./DT;

%Percentile calculations: Delete transitory terms
prc50=prctile(sol,50);  pnotr50=prctile(sol(1800:end),50);    palt50=prctile(sol(5900:end),50);
prc95=prctile(sol,95);  pnotr95=prctile(sol(1800:end),95);    palt95=prctile(sol(5900:end),95);
prc99=prctile(sol,99);  pnotr99=prctile(sol(1800:end),99);    palt99=prctile(sol(5900:end),99);    

toc