                 %% MOON - Dynamic Positioning with Least Squares %%
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. LS 
r_moon=1737.4e3; %Moon radius [m]
ind_t=1;
n=4; %nsat
c=299792458;
N=1000;    %Number iteration for Monte Carlo

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

dist=-1e4;   %Minimum distance to define altimeter = 10 Km

%Initializations
u_start=[0,0,0]';
tu_start=0;         
maxiter=50;
toll=1e-4;
TOW=nan(length(data1),1);   av=nan(length(data1),1);    e_ck=nan(length(data1),1);
x_fin=nan(length(data1),N); y_fin=nan(length(data1),N);
z_fin=nan(length(data1),N); t_fin=nan(length(data1),1);
sol=nan(length(data1),N);   pos=sol;    errx=sol;  erry=sol;   errz=sol;
SR_m=nan(length(data1),4);  SR_n=nan(length(data1),4);
HDOP=nan(length(data1),1);  VDOP=nan(length(data1),1);

for ii=1:N
    clear F F_sub G sigMat P_approx P_start u_approx u_start alt alt_app alt_m 
    clear SR SR_app x y z u_start1 ax ay az H K R Q
    ind_t=2;
    u_start=zeros(3,1);
while ind_t<=length(data1)
    % Check User under Satellites
    flag=0;
    while flag==0
        TOW(ind_t)=Delta*(ind_t-1);      
        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=[0,0,0]';
            av(ind_t)=0;
            x_fin(ind_t,ii)=nan;
            y_fin(ind_t,ii)=nan;
            z_fin(ind_t,ii)=nan;
            ind_t=ind_t+1;
        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);
    % 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_t,1)-Mu(ind_t,1))^2+(M1(ind_t,2)-Mu(ind_t,2))^2+(M1(ind_t,3)-Mu(ind_t,3))^2);
    SR(2)=sqrt((M2(ind_t,1)-Mu(ind_t,1))^2+(M2(ind_t,2)-Mu(ind_t,2))^2+(M2(ind_t,3)-Mu(ind_t,3))^2);
    SR(3)=sqrt((M3(ind_t,1)-Mu(ind_t,1))^2+(M3(ind_t,2)-Mu(ind_t,2))^2+(M3(ind_t,3)-Mu(ind_t,3))^2);
    SR(4)=sqrt((M4(ind_t,1)-Mu(ind_t,1))^2+(M4(ind_t,2)-Mu(ind_t,2))^2+(M4(ind_t,3)-Mu(ind_t,3))^2);
    
    % LS
    for niter=1:maxiter
        u_approx=u_start;
        tu_approx=tu_start;
        
        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)];
        
        radius=sqrt((x-u_approx(1)).^2+(y-u_approx(2)).^2+(z-u_approx(3)).^2);    %geometric range
        ax=(x-u_approx(1))./radius;
        ay=(y-u_approx(2))./radius;
        az=(z-u_approx(3))./radius;
        H=[ax',ay',az',ones(length(x),1)];
        
        SR_app(1)=sqrt((x(1)-u_approx(1))^2+(y(1)-u_approx(2))^2+(z(1)-u_approx(3))^2)+err1;
        SR_app(2)=sqrt((x(2)-u_approx(1))^2+(y(2)-u_approx(2))^2+(z(2)-u_approx(3))^2)+err2;
        SR_app(3)=sqrt((x(3)-u_approx(1))^2+(y(3)-u_approx(2))^2+(z(3)-u_approx(3))^2)+err3;
        SR_app(4)=sqrt((x(4)-u_approx(1))^2+(y(4)-u_approx(2))^2+(z(4)-u_approx(3))^2)+err4;

        Drho=SR_app-SR;
        Dx=(H'*H)\(H'*Drho');       %Offset in user position & time bias [Dxu, Dyu, Dzu, -c*Dtu]
    
        %DOP
        D=(H'*H)^-1;
        HDOP(ind_t)=sqrt(D(1,1)+D(2,2));        %Horizontal
        VDOP(ind_t)=sqrt(D(3,3));               %Vertical

        %ESTIMATE UPDATE
        u_start=u_approx+Dx(1:end-1);
        tu_start=tu_approx+Dx(end);
        
        if norm(Dx(1:3))<=toll
            break
        end
        
        clear x y z SR_app radius ax ay az H Drho
    end
    x_fin(ind_t,ii)=u_start(1);
    y_fin(ind_t,ii)=u_start(2);
    z_fin(ind_t,ii)=u_start(3);
    t_fin(ind_t)=tu_start;
    
    % Altimeter
    Norm_Mu(ind_t)=norm(Mu(ind_t,1:3));
    Norm_app(ind_t)=norm([u_start(1),u_start(2),u_start(3)]);
    alt=Norm_Mu(ind_t)-r_moon;
    alt_app=Norm_app(ind_t)-r_moon;
    %determinare punto sulla superficie lunare = intersezione retta che
    %definisce Lander e Sfera che rappresenta luna
    if u_start(3)>=0 && u_start(2)<0
        xL=-sqrt(r_moon^2/(1+(u_start(2)/u_start(1))^2+(u_start(3)/u_start(1))^2));
        yL=xL*(u_start(2)/u_start(1));
        zL=xL*(u_start(3)/u_start(1));
    else
        xL=sqrt(r_moon^2/(1+(u_start(2)/u_start(1))^2+(u_start(3)/u_start(1))^2));
        yL=xL*(u_start(2)/u_start(1));
        zL=xL*(u_start(3)/u_start(1));
    end
    
    if abs(alt_app)<dist
        % Errore
        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);
%         % No errors
%         xp=xL; yp=yL; zp=zL;
        
                            % LS con altimetro %
    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(2))^2+(zL-u_start(3))^2);
    
        ax_alt=-(xp-u_start(1))./(sqrt((u_start(1)-xp).^2+(u_start(2)-yp).^2+(u_start(3)-zp).^2));
        ay_alt=-(yp-u_start(2))./(sqrt((u_start(1)-xp).^2+(u_start(2)-yp).^2+(u_start(3)-zp).^2));
        az_alt=-(zp-u_start(3))./(sqrt((u_start(1)-xp).^2+(u_start(2)-yp).^2+(u_start(3)-zp).^2));
        H=[H; ax_alt ay_alt az_alt 0]; 

        Drho=[Drho , alt_m-alt_app];
        Dx=(H'*H)\(H'*Drho');       %Offset in user position & time bias [Dxu, Dyu, Dzu, -c*Dtu]
    
        %DOP
        D=(H'*H)^-1;
        HDOP(ind_t)=sqrt(D(1,1)+D(2,2));        %Horizontal
        VDOP(ind_t)=sqrt(D(3,3));               %Vertical

        %ESTIMATE UPDATE
        u_start=u_approx+Dx(1:end-1);
        tu_start=tu_approx+Dx(end);
    end
    x_fin(ind_t,ii)=u_start(1);
    y_fin(ind_t,ii)=u_start(2);
    z_fin(ind_t,ii)=u_start(3);
    t_fin(ind_t)=tu_start;
    
    clear x y z SR_app radius ax ay az H Drho
    ind_t=ind_t+1;
end
errx(:,ii)=x_fin(:,ii)-Mu(:,1);
erry(:,ii)=y_fin(:,ii)-Mu(:,2);
errz(:,ii)=z_fin(:,ii)-Mu(:,3);
sol(:,ii)=sqrt((errx(:,ii)).^2+(erry(:,ii)).^2+(errz(:,ii)).^2);
pos(:,ii)=sqrt((x_fin(:,ii)).^2+(y_fin(:,ii)).^2+(z_fin(:,ii)).^2);
end

sol=rms(sol')';      pos=rms(pos')';
errx=rms(errx')';    erry=rms(erry')';    errz=rms(errz')'; 
x_f=rms(x_fin')';    vx_fin=diff(x_f)./DT;
y_f=rms(y_fin')';    vy_fin=diff(y_f)./DT;
z_f=rms(z_fin')';    vz_fin=diff(z_f)./DT;

%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(2:end);
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);    
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