                 %% SF with the exclusion of a satellite %%

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]

%Altimeter
dist=1e4;   %Minimum distance to define altimeter = 10 Km
SamR=0.1;   %Sample Rate
CosB=0.1;   %Constant Bias = 0.1 m
NoiD=0.02;   %Noise Density = % of Slant Range

%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=av;
x_fin=av; y_fin=av; z_fin=av; t_fin=av; Norm_app=av; Norm_Mu=av;
vx_fin=av; vy_fin=av; vz_fin=av;   ax_fin=av; ay_fin=av; az_fin=av;
HDOP=av; VDOP=av;

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         
        % Check User under Satellites   
        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
    
    Norm_app(ind_t)=norm([u_start(1),u_start(4),u_start(7)]);
    alt_app=Norm_app(ind_t)-r_moon;
    
    if abs(alt_app)>dist
    % 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);
    
    % 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);
    
    % KF   
    u_approx=u_start;
    P_approx=P_start;

    %MATRICES    
    %State Tansition Matrix
    F_sub=[1 DT(ind_k) (DT(ind_k)^2)/2; 0 1 DT(ind_k); 0 0 1];     %costant acceleration   
    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=1e1*ones(3,1);
    sigma_t=1e-3;
    sigma_dt=1e-4;
    
    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=1e3;
    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,ones(n,1),c0,c0]; 
    
    %Uncerainty on receiver
    [xu,yu,zu]=myspheresat(u_approx(1),u_approx(4),u_approx(7),10*randn);
    [xp(1),yp(1),zp(1)]=myspheresat(x(1),y(1),z(1),err1);
    [xp(2),yp(2),zp(2)]=myspheresat(x(2),y(2),z(2),err2);
    [xp(3),yp(3),zp(3)]=myspheresat(x(3),y(3),z(3),err3);
    [xp(4),yp(4),zp(4)]=myspheresat(x(4),y(4),z(4),err4);
    SR_app(1)=sqrt((xp(1)-xu)^2+(yp(1)-yu)^2+(zp(1)-zu)^2)+err1;
    SR_app(2)=sqrt((xp(2)-xu)^2+(yp(2)-yu)^2+(zp(2)-zu)^2)+err2;
    SR_app(3)=sqrt((xp(3)-xu)^2+(yp(3)-yu)^2+(zp(3)-zu)^2)+err3;
    SR_app(4)=sqrt((xp(4)-xu)^2+(yp(4)-yu)^2+(zp(4)-zu)^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');%-H*u_start1);
    P_start=P_start1-K*H*P_start1;
        
    x_fin(ind_t)=u_start(1);    vx_fin(ind_t)=u_start(2);    ax_fin(ind_t)=u_start(3);  
    y_fin(ind_t)=u_start(4);    vy_fin(ind_t)=u_start(5);    ay_fin(ind_t)=u_start(6);
    z_fin(ind_t)=u_start(7);    vz_fin(ind_t)=u_start(8);    az_fin(ind_t)=u_start(9);
    t_fin(ind_t)=u_start(10)/c;
    
        %DOP
        [lat,lon,h]=Bowring(u_approx(1),u_approx(4),u_approx(7),3476.2e3,3472e3);  % Lat & Lon (Ellipsoid Moon)
        Rrot=[-sin(lon) cos(lon) 0;
            -cos(lon)*sin(lat) -sin(lon)*sin(lat) cos(lat);
            cos(lon)*cos(lat) sin(lon)*cos(lat) sin(lat)]; % Rotation Matrix
        D=Rrot'*((H(:,[1,4,7])'*H(:,[1,4,7]))^-1)*Rrot;
        HDOP(ind_t)=sqrt(D(1,1)+D(2,2));
        VDOP(ind_t)=sqrt(D(3,3));
        
    else
        % Altimeter
    %determinare punto sulla superficie lunare = intersezione retta che
    %definisce Lander e Sfera che rappresenta luna
    if u_start(7)>=0 && u_start(4)<0
        xL=-sqrt(r_moon^2/(1+(u_start(4)/u_start(1))^2+(u_start(7)/u_start(1))^2));
        yL=xL*(u_start(4)/u_start(1));
        zL=xL*(u_start(7)/u_start(1));
    else
        xL=sqrt(r_moon^2/(1+(u_start(4)/u_start(1))^2+(u_start(7)/u_start(1))^2));
        yL=xL*(u_start(4)/u_start(1));
        zL=xL*(u_start(7)/u_start(1));
    end
    
        % 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);
    
    % 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);
    
    % KF   
    u_approx=u_start;
    P_approx=P_start;

    %MATRICES    
    %State Tansition Matrix
    F_sub=[1 DT(ind_k) (DT(ind_k)^2)/2; 0 1 DT(ind_k); 0 0 1];     %costant acceleration   
    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=1e1*ones(3,1);
    sigma_t=1e-3;
    sigma_dt=1e-4;
    
    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=1e3;
    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,1);
    H=[ax',c0,c0,ay',c0,c0,az',c0,c0,ones(n-1,1),c0,c0]; 
    
    %Uncerainty on receiver
    [xu,yu,zu]=myspheresat(u_approx(1),u_approx(4),u_approx(7),10*randn);
    [xp(1),yp(1),zp(1)]=myspheresat(x(1),y(1),z(1),err1);
    [xp(2),yp(2),zp(2)]=myspheresat(x(2),y(2),z(2),err2);
    [xp(3),yp(3),zp(3)]=myspheresat(x(3),y(3),z(3),err3);
%     [xp(4),yp(4),zp(4)]=myspheresat(x(4),y(4),z(4),err4);
    SR_app(1)=sqrt((xp(1)-xu)^2+(yp(1)-yu)^2+(zp(1)-zu)^2)+err1;
    SR_app(2)=sqrt((xp(2)-xu)^2+(yp(2)-yu)^2+(zp(2)-zu)^2)+err2;
    SR_app(3)=sqrt((xp(3)-xu)^2+(yp(3)-yu)^2+(zp(3)-zu)^2)+err3;
%     SR_app(4)=sqrt((xp(4)-xu)^2+(yp(4)-yu)^2+(zp(4)-zu)^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');%-H*u_start1);
    P_start=P_start1-K*H*P_start1;
    
        % Errore
%         theta=1.3;
%         hz=arc(r_moon,Norm_Mu(ind_t),theta); 
        hz=3e3+100*randn;   %0.002*Norm_Mu(ind_t)
        hx=hz; hy=hz;
        nmesh=1000;
        xp=xL; yp=yL; zp=zL;
        [xL,yL,zL,~,~,~]=alterr(xp,yp,zp,nmesh,hx,hy,hz);
                            % KF con altimetro %
        % Measurement Error Matrix
        R(n,n)=(1e3)^2;
        
        alt_m=sqrt((xL-Mu(ind_t,1))^2+(yL-Mu(ind_t,2))^2+(zL-Mu(ind_t,3))^2);
        alt_app=sqrt((xL-u_start(1))^2+(yL-u_start(4))^2+(zL-u_start(7))^2);
        ax_alt=-(xp-u_start(1))./(sqrt((u_start(1)-xp).^2+(u_start(4)-yp).^2+(u_start(7)-zp).^2));
        ay_alt=-(yp-u_start(4))./(sqrt((u_start(1)-xp).^2+(u_start(4)-yp).^2+(u_start(7)-zp).^2));
        az_alt=-(zp-u_start(7))./(sqrt((u_start(1)-xp).^2+(u_start(4)-yp).^2+(u_start(7)-zp).^2));
        H=[H; ax_alt 0 0 ay_alt 0 0 az_alt 0 0 0 0 0]; 
        z_meas=[z_meas , alt_m-alt_app];

        %%% 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
        u_start=u_start1+K*(z_meas');%-H*u_start1);
        P_start=P_start1-K*H*P_start1;
        
        x_fin(ind_t)=u_start(1);    vx_fin(ind_t)=u_start(2);    ax_fin(ind_t)=u_start(3);  
        y_fin(ind_t)=u_start(4);    vy_fin(ind_t)=u_start(5);    ay_fin(ind_t)=u_start(6);
        z_fin(ind_t)=u_start(7);    vz_fin(ind_t)=u_start(8);    az_fin(ind_t)=u_start(9);
        t_fin(ind_t)=u_start(10)/c;
        
%         %DOP
%         [lat,lon,h]=Bowring(u_approx(1),u_approx(4),u_approx(7),3476.2e3,3472e3);  % Lat & Lon (Ellipsoid Moon)
%         Rrot=[-sin(lon) cos(lon) 0;
%             -cos(lon)*sin(lat) -sin(lon)*sin(lat) cos(lat);
%             cos(lon)*cos(lat) sin(lon)*cos(lat) sin(lat)]; % Rotation Matrix
%         D=Rrot'*((H(:,[1,4,7])'*H(:,[1,4,7]))^-1)*Rrot;
%         HDOP(ind_t)=sqrt(D(1,1)+D(2,2));
%         VDOP(ind_t)=sqrt(D(3,3));
    end
    
    clear SR SR_app x y z u_start1 P_start1 ax ay az H K R Q
    ind_t=ind_t+1;
end
alt_time=5950;
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);
% user profile
vel_app=sqrt((vx_fin).^2+(vy_fin).^2+(vz_fin).^2);
acc_app=sqrt((ax_fin).^2+(ay_fin).^2+(az_fin).^2);
vel_u=sqrt((Mu(:,4)).^2+(Mu(:,5)).^2+(Mu(:,6)).^2);
acc_u=diff(vel_u)./DT;

%Percentile calculations: Delete transitory terms
prc50=prctile(sol,50);  pnotr50=prctile(sol(1800:end),50);  
prc95=prctile(sol,95);  pnotr95=prctile(sol(1800:end),95);
prc99=prctile(sol,99);  pnotr99=prctile(sol(1800:end),99);    
%Percentile for altimeter comparison
palt50=prctile(sol(5950:end),50);   pnoalt50=prctile(sol(1:5950),50);
palt95=prctile(sol(5950:end),95);   pnoalt95=prctile(sol(1:5950),95);
palt99=prctile(sol(5950:end),99);   pnoalt99=prctile(sol(1:5950),99);

toc