clc
clear all
close all
%% Input Values
% Reference point A0 (Geographical Coordinates)
alt0=39000; % in ft, orthometric height
Lat0=53.77;
Lon0=-9.94;

% Layers A1, A2, A3, A4 (Geographical Coordinates)
% Row No.=Group No.; Column No.=Order of the Aircraft in the Group
Alt=[37025 38975 40000 35000;39000 39000 38000 35000;32000 40000 37000 33000;39000 40000 38000 37000]; % in ft
Lat=[53.86 53.89 54.34 54.45;53.69 53.92 54.2 54.48;53.84 54 54.75 55;54.03 54.01 55.02 55.03];
Lon=[-10.68 -11.36 -10.75 -10.92;-13.09 -11.95 -11.92 -12.42;-14.26 -14.69 -14 -14.56;-16.99 -17.53 -16.79 -17.11];

% % Accuracy of Results, 4..RNP4 or 10...RNP10 or different value but in nautical miles
% acc_A2=4; % Accuracy for layer A2
% acc_A3=4; % Accuracy for layer A3
% acc_A4=10; % Accuracy for layer A4

% Aircraft Position Error
pos=3; %3 or 4 meters

% Amount of Simulations
SIM=10000;
%% Transforming Orthometric height to Ellipsoidal height
% A0, point of origin for transforming from geodetic to ENU coordinates
H0=convlength(alt0,'ft','m');
N0 = egm96geoid(Lat0,Lon0);
h0 = H0+N0;

% layers A1 to A4, for transformation to ENU
N = egm96geoid(Lat,Lon);
Alt_m14 = convlength(Alt,'ft','m');
Alt_m = Alt_m14+N;
%% Transform Geodetic Coordinates to ENU coordinates
wgs84 = wgs84Ellipsoid('meter'); % Reference spheroid
[xEast,yNorth,zUp] = geodetic2enu(Lat,Lon,Alt_m,Lat0,Lon0,h0,wgs84);
[row,col]=size(xEast);
%% Plotting 1st to 4th group
% 2D
for i=1:4
    plot(xEast(1,i),yNorth(1,i),'rx')
    text(xEast(1,i),yNorth(1,i)," A1"+i,'Color','red','FontSize',12)
hold on
plot(xEast(2,i),yNorth(2,i),'kx')
text(xEast(2,i),yNorth(2,i)," A2"+i,'Color','black','FontSize',12)
hold on
  plot(xEast(3,i),yNorth(3,i),'rx')
text(xEast(3,i),yNorth(3,i)," A3"+i,'Color','red','FontSize',12)
hold on
plot(xEast(4,i),yNorth(4,i),'kx')
text(xEast(4,i),yNorth(4,i)," A4"+i,'Color','black','FontSize',12)
hold on
% axis ij % changing direction y axis
% set(gca,'Xdir','reverse') %changing x axis direction
end
plot(0,0,'bx')
text(0,0," A0",'Color','blue','FontSize',12)
xlabel('xEast [m]','FontSize',12)
ylabel('yNorth [m]','FontSize',12)
grid on

% 3D Plot
% for i=1:4
%     plot3(xEast(1,i),yNorth(1,i),zUp(1,i),'rx')
%     text(xEast(1,i),yNorth(1,i),zUp(1,i)," A1"+i,'Color','red','FontSize',10)
% hold on
% plot3(xEast(2,i),yNorth(2,i),zUp(2,i),'kx')
% text(xEast(2,i),yNorth(2,i),zUp(2,i)," A2"+i,'Color','black','FontSize',10)
% hold on
%   plot3(xEast(3,i),yNorth(3,i),zUp(3,i),'rx')
% text(xEast(3,i),yNorth(3,i),zUp(3,i)," A3"+i,'Color','red','FontSize',10)
% hold on
% plot3(xEast(4,i),yNorth(4,i),zUp(4,i),'kx')
% text(xEast(4,i),yNorth(4,i),zUp(4,i)," A4"+i,'Color','black','FontSize',10)
% hold on
% end
% plot3(0,0,0,'bx')
% text(0,0,0,"A0",'Color','blue','FontSize',10)
% grid on
% xlabel('xEast [m]')
% ylabel('yNorth [m]')
% zlabel('zUp [m]')
%% Distances between layers
% row no.=aircraft's position within the group
% column no.=position of the aircraft from previous group
% e.g. position in matrix R_A2_A1(1,3) = distance between A21 and A13
% row no.: 1...A21; 2...A22; 3...A23; column no.: 1...A11; 2...A12; 3...A13
for i=1:col
for j=1:col
    R_A2_A1(i,j)=sqrt((xEast(2,i)-xEast(1,j))^2+(yNorth(2,i)-yNorth(1,j))^2+(zUp(2,i)-zUp(1,j))^2);
    R_A3_A2(i,j)=sqrt((xEast(3,i)-xEast(2,j))^2+(yNorth(3,i)-yNorth(2,j))^2+(zUp(3,i)-zUp(2,j))^2);
    R_A4_A3(i,j)=sqrt((xEast(4,i)-xEast(3,j))^2+(yNorth(4,i)-yNorth(3,j))^2+(zUp(4,i)-zUp(3,j))^2);
end
end
cs=299792458; % speed of light [m/s]
dT_A2_A1=R_A2_A1./cs; % delta Time between layer 1 and 2 [s]
dT_A3_A2=R_A3_A2./cs; % delta Time between layer 2 and 3 [s]
dT_A4_A3=R_A4_A3./cs; % delta Time between layer 3 and 4 [s]

%% Variables for storing results of simulation
A1(1,1:4)=0; % 1st column stores the layer A1 indices; columns 2:4 store A1 aircraft corrected by position error;
 A11(1,1:3)=0;
 A12(1,1:3)=0;
 A13(1,1:3)=0;
 A14(1,1:3)=0;

A21(1,2:4)=[xEast(2,1) yNorth(2,1) zUp(2,1)]; % first row = correct values of aircraft's center; next rows: sums of iterations and values of found x,y,z
 A21(2:SIM+1,1)=0; 
 A22(1,2:4)=[xEast(2,2) yNorth(2,2) zUp(2,2)];
 A22(2:SIM+1,1)=0;
 A23(1,2:4)=[xEast(2,3) yNorth(2,3) zUp(2,3)];
 A23(2:SIM+1,1)=0;
A24(1,2:4)=[xEast(2,4) yNorth(2,4) zUp(2,4)];
A24(2:SIM+1,1)=0;

 A31(1,2:4)=[xEast(3,1) yNorth(3,1) zUp(3,1)];
 A31(2:SIM+1,1)=0;
 A32(1,2:4)=[xEast(3,2) yNorth(3,2) zUp(3,2)];
 A32(2:SIM+1,1)=0;
 A33(1,2:4)=[xEast(3,3) yNorth(3,3) zUp(3,3)];
 A33(2:SIM+1,1)=0;
 A34(1,2:4)=[xEast(3,4) yNorth(3,4) zUp(3,4)];
 A34(2:SIM+1,1)=0;

 A41(1,2:4)=[xEast(4,1) yNorth(4,1) zUp(4,1)];
 A41(2:SIM+1,1)=0;
 A42(1,2:4)=[xEast(4,2) yNorth(4,2) zUp(4,2)];
 A42(2:SIM+1,1)=0;
 A43(1,2:4)=[xEast(4,3) yNorth(4,3) zUp(4,3)];
 A43(2:SIM+1,1)=0;
 A44(1,2:4)=[xEast(4,4) yNorth(4,4) zUp(4,4)];
 A44(2:SIM+1,1)=0;
 %% Assigning Values to Variables
% rnp_A2=unitsratio('meter','nautical mile')*acc_A2; % RNP 4 or RNP 10 accuracy for layer A2
% rnp_A3=unitsratio('meter','nautical mile')*acc_A3; % RNP 4 or RNP 10 accuracy for layer A3
% rnp_A4=unitsratio('meter','nautical mile')*acc_A4; % RNP 4 or RNP 10 accuracy for layer A4

a_row=1; % A1 row indexing
er=0; % storing amount of inaccurate results
prob_nan=0; % indexing for storing input values when the result of iteration is NaN
tol=0.25*10^(-6); % tolerance of Mode S [s]
%% Simulation
for j=1:SIM
%% Determining Positions of Aircraft in Layer A2
 % Determining the nearest aircraft
[~,ix]=min(dT_A2_A1,[],2); %find the nearest aircraft to use its xyz for x_n,y_n,z_n
idx=ix(:,1); % idx of aircraft from 1st group

for i=1:4
idx_av(i,1:3)=setdiff(1:4,idx(i)); % rest of available indices from 1st group; they are used for iterations
end
for i=1:4
XE=xEast(1,idx_av(i,1:3))+normrnd(0,pos,1,3); % normrnd generates position error
YN=yNorth(1,idx_av(i,1:3))+normrnd(0,pos,1,3);
ZU=zUp(1,idx_av(i,1:3))+normrnd(0,pos,1,3);

A1(a_row:a_row+2,1)=[idx_av(i,1:3)];
A1(a_row:a_row+2,2:4)=[(XE)',(YN)',(ZU)'];
a_row=a_row+3;

x_n=xEast(1,idx(i))+normrnd(0,pos);
y_n=yNorth(1,idx(i))+normrnd(0,pos);
z_n=zUp(1,idx(i))+normrnd(0,pos);

dT=dT_A2_A1(i,idx_av(i,1:3));

[x,y,z,A,R] = position(x_n, y_n,z_n,XE,YN,ZU,dT,tol); 

% if abs(x_n-x)>rnp_A3||abs(y_n-y)>rnp_A3||abs(z_n-z)>rnp_A3
%     er=er+1;
% elseif isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
if isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
prob_nan=prob_nan+1;
prob(prob_nan,1)="2"+i; % A21 or A22 or A23 or A24
prob(prob_nan,2:16)=[x_n,y_n,z_n,XE(1),YN(1),ZU(1),R(1),XE(2),YN(2),ZU(2),R(2),XE(3),YN(3),ZU(3),R(3)];
elseif i==1
A21(j+1,1)= A; % vector of sums of iterations
A21(j+1,2)=x;
A21(j+1,3)=y;
A21(j+1,4)=z;   
    elseif i==2
 A22(j+1,1)=A;    
 A22(j+1,2)=x;
 A22(j+1,3)=y;
 A22(j+1,4)=z;
    elseif i==3
 A23(j+1,1)=A;    
 A23(j+1,2)=x;
 A23(j+1,3)=y;
 A23(j+1,4)=z;
    else
 A24(j+1,1)=A;    
 A24(j+1,2)=x;
 A24(j+1,3)=y;
 A24(j+1,4)=z;   
end
end

%% Determining Positions of Aircraft in Layer A3
    xE(j,:)=[A21(j+1,2),A22(j+1,2),A23(j+1,2),A24(j+1,2)];
    yN(j,:)=[A21(j+1,3),A22(j+1,3),A23(j+1,3),A24(j+1,3)];
    zU(j,:)=[A21(j+1,4),A22(j+1,4),A23(j+1,4),A24(j+1,4)];
    
[~,ix]=min(dT_A3_A2,[],2); %find the closest aircraft to use its xyz for x_n,y_n,z_n (within simulation))
idx=ix(:,1); % idx of aircraft from 1st group

for i=1:4
idx_av(i,1:3)=setdiff(1:4,idx(i)); %rest of available indices from 1st group, which is used for iterations
end

for i=1:4
XE=xE(j,idx_av(i,1:3));
YN=yN(j,idx_av(i,1:3));
ZU=zU(j,idx_av(i,1:3));

x_n=xE(j,idx(i));
y_n=yN(j,idx(i));
z_n=zU(j,idx(i));

dT=dT_A3_A2(i,idx_av(i,1:3));

[x,y,z,A,R] = position(x_n, y_n,z_n,XE,YN,ZU,dT,tol);

% if abs(x_n-x)>rnp_A3||abs(y_n-y)>rnp_A3||abs(z_n-z)>rnp_A3
%     er=er+1;
% elseif isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
if isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
prob_nan=prob_nan+1;
prob(prob_nan,1)="3"+i; % A31 or A32 or A33 or A34
prob(prob_nan,2:16)=[x_n,y_n,z_n,XE(1),YN(1),ZU(1),R(1),XE(2),YN(2),ZU(2),R(2),XE(3),YN(3),ZU(3),R(3)];
elseif i==1
A31(j+1,1)= A; % vector of sums of iterations
A31(j+1,2)=x;
A31(j+1,3)=y;
A31(j+1,4)=z;   
    elseif i==2
 A32(j+1,1)=A;    
 A32(j+1,2)=x;
 A32(j+1,3)=y;
 A32(j+1,4)=z;
    elseif i==3
 A33(j+1,1)=A;    
 A33(j+1,2)=x;
 A33(j+1,3)=y;
 A33(j+1,4)=z;
    else
 A34(j+1,1)=A;    
 A34(j+1,2)=x;
 A34(j+1,3)=y;
 A34(j+1,4)=z;   
end
end
%% Determining Positions of Aircraft in Layer A4

    xE(j,:)=[A31(j+1,2),A32(j+1,2),A33(j+1,2),A34(j+1,2)];
    yN(j,:)=[A31(j+1,3),A32(j+1,3),A33(j+1,3),A34(j+1,3)];
    zU(j,:)=[A31(j+1,4),A32(j+1,4),A33(j+1,4),A34(j+1,4)];

[val,ix]=min(dT_A4_A3,[],2); %find the closest aircraft to use its xyz for x_n,y_n,z_n (within simulation))
idx=ix(:,1); % idx of aircraft from 1st group

for i=1:4
idx_av(i,1:3)=setdiff(1:4,idx(i)); %rest of available indices from 1st group, which is used for iterations
end

for i=1:4
XE=xE(j,idx_av(i,1:3));
YN=yN(j,idx_av(i,1:3));
ZU=zU(j,idx_av(i,1:3));

x_n=xE(j,idx(i));
y_n=yN(j,idx(i));
z_n=zU(j,idx(i));

dT=dT_A4_A3(i,idx_av(i,1:3));

[x,y,z,A,R] = position(x_n, y_n,z_n,XE,YN,ZU,dT,tol); 

% if abs(x_n-x)>rnp_A3||abs(y_n-y)>rnp_A3||abs(z_n-z)>rnp_A3
%     er=er+1;
% elseif isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
if isnan(x)==1 ||isnan(y)==1 || isnan(z)==1
prob_nan=prob_nan+1;
prob(prob_nan,1)="4"+i; % A41 or A42 or A43 or A44
prob(prob_nan,2:16)=[x_n,y_n,z_n,XE(1),YN(1),ZU(1),R(1),XE(2),YN(2),ZU(2),R(2),XE(3),YN(3),ZU(3),R(3)];
elseif i==1
A41(j+1,1)= A; % vector of sums of iterations
A41(j+1,2)=x;
A41(j+1,3)=y;
A41(j+1,4)=z;   
    elseif i==2
 A42(j+1,1)=A;    
 A42(j+1,2)=x;
 A42(j+1,3)=y;
 A42(j+1,4)=z;
    elseif i==3
 A43(j+1,1)=A;    
 A43(j+1,2)=x;
 A43(j+1,3)=y;
 A43(j+1,4)=z;
    else
 A44(j+1,1)=A;    
 A44(j+1,2)=x;
 A44(j+1,3)=y;
 A44(j+1,4)=z;   
end
end
end
 %% A1 Filtration
 [row,~]=size(A1);
 a11_row=0;
 a12_row=0;
 a13_row=0;
 a14_row=0;
 for i=1:row
     if A1(i,1)==1
         a11_row=a11_row+1;
         A11(a11_row,1:3)=A1(i,2:4);
     elseif A1(i,1)==2
           a12_row=a12_row+1;
         A12(a12_row,1:3)=A1(i,2:4);
     elseif A1(i,1)==3
           a13_row=a13_row+1;
         A13(a13_row,1:3)=A1(i,2:4);
     else
           a14_row=a14_row+1;
         A14(a14_row,1:3)=A1(i,2:4);
     end
 end
%% Removing Outliers
A11_corrected=rmoutliers(A11(1:end,:));
A12_corrected=rmoutliers(A12(1:end,:));
A13_corrected=rmoutliers(A13(1:end,:));
A14_corrected=rmoutliers(A14(1:end,:));

A21_corrected=rmoutliers(A21(:,2:4));
A22_corrected=rmoutliers(A22(:,2:4));
A23_corrected=rmoutliers(A23(:,2:4));
A24_corrected=rmoutliers(A24(:,2:4));

A31_corrected=rmoutliers(A31(:,2:4));
A32_corrected=rmoutliers(A32(:,2:4));
A33_corrected=rmoutliers(A33(:,2:4));
A34_corrected=rmoutliers(A34(:,2:4));

A41_corrected=rmoutliers(A41(:,2:4));
A42_corrected=rmoutliers(A42(:,2:4));
A43_corrected=rmoutliers(A43(:,2:4));
A44_corrected=rmoutliers(A44(:,2:4));
%% Calculating DRMS
A11_std(1:3)=std(A11_corrected); %standard deviation
A_drms(1)=2*sqrt(A11_std(1)^2+A11_std(2)^2); %2DRMS (2D)
A12_std(1:3)=std(A12_corrected);
A_drms(2)=2*sqrt(A12_std(1)^2+A12_std(2)^2); 
A13_std(1:3)=std(A13_corrected);
A_drms(3)=2*sqrt(A13_std(1)^2+A13_std(2)^2); 
A14_std(1:3)=std(A14_corrected);
A_drms(4)=2*sqrt(A14_std(1)^2+A14_std(2)^2); 


A21_std=std(A21_corrected); %standard deviation
A_drms(5)=2*sqrt(A21_std(1)^2+A21_std(2)^2); %2DRMS (2D)
A22_std=std(A22_corrected);
A_drms(6)=2*sqrt(A22_std(1)^2+A22_std(2)^2); 
A23_std=std(A23_corrected);
A_drms(7)=2*sqrt(A23_std(1)^2+A23_std(2)^2); 
A24_std=std(A24_corrected);
A_drms(8)=2*sqrt(A24_std(1)^2+A24_std(2)^2); 

A31_std=std(A31_corrected); %standard deviation
A_drms(9)=2*sqrt(A31_std(1)^2+A31_std(2)^2); %2DRMS (2D)
A32_std=std(A32_corrected);
A_drms(10)=2*sqrt(A32_std(1)^2+A32_std(2)^2); 
A33_std=std(A33_corrected);
A_drms(11)=2*sqrt(A33_std(1)^2+A33_std(2)^2); 
A34_std=std(A34_corrected);
A_drms(12)=2*sqrt(A34_std(1)^2+A34_std(2)^2); 

A41_std=std(A41_corrected); %standard deviation
A_drms(13)=2*sqrt(A41_std(1)^2+A41_std(2)^2); %2DRMS (2D)
A42_std=std(A42_corrected);
A_drms(14)=2*sqrt(A42_std(1)^2+A42_std(2)^2); 
A43_std=std(A43_corrected);
A_drms(15)=2*sqrt(A43_std(1)^2+A43_std(2)^2); 
A44_std=std(A44_corrected);
A_drms(16)=2*sqrt(A44_std(1)^2+A44_std(2)^2);
%% Plotting Results
hold on
plot(A11_corrected(:,1),A11_corrected(:,2),'o')
plot(A12_corrected(:,1),A12_corrected(:,2),'o')
plot(A13_corrected(:,1),A13_corrected(:,2),'o')
plot(A14_corrected(:,1),A14_corrected(:,2),'o')

plot(A21_corrected(:,1),A21_corrected(:,2),'o')
plot(A22_corrected(:,1),A22_corrected(:,2),'o')
plot(A23_corrected(:,1),A23_corrected(:,2),'o')
plot(A24_corrected(:,1),A24_corrected(:,2),'o')

plot(A31_corrected(:,1),A31_corrected(:,2),'o')
plot(A32_corrected(:,1),A32_corrected(:,2),'o')
plot(A33_corrected(:,1),A33_corrected(:,2),'o')
plot(A34_corrected(:,1),A34_corrected(:,2),'o')

plot(A41_corrected(:,1),A41_corrected(:,2),'o')
plot(A42_corrected(:,1),A42_corrected(:,2),'o')
plot(A43_corrected(:,1),A43_corrected(:,2),'o')
plot(A44_corrected(:,1),A44_corrected(:,2),'o')
%% Plotting 2DRMS
RNP4=unitsratio('meter','nautical mile')*4;
RNP10=unitsratio('meter','nautical mile')*10;

figure
tx={' A11',' A12',' A13',' A14',' A21',' A22',' A23',' A24',' A31',' A32',' A33',' A34',' A41',' A42',' A43',' A44'};

plot(A_drms,'--*','MarkerEdgeColor','r','LineWidth',1)
text(1:16,A_drms,tx,'Color','black','FontSize',10)
hold on
plot([1 16], [RNP4 RNP4],'LineWidth',1)
plot([1 16], [RNP10 RNP10],'LineWidth',1)
legend('2DRMS','RNP4','RNP10','Location','northwest')
xlabel('x')
ylabel('y [m]')
grid on

A_m(1)=mean(A_drms(1:4));
A_m(2)=mean(A_drms(5:8));
A_m(3)=mean(A_drms(9:12));
A_m(4)=mean(A_drms(13:16));
figure
tx={' A1',' A2',' A3',' A4'};
plot(A_m,'*--','MarkerEdgeColor','r','LineWidth',1)
text(1:4,A_m,tx)
hold on
plot([1 4], [RNP4 RNP4],'LineWidth',1)
plot([1 4], [RNP10 RNP10],'LineWidth',1)
legend('2DRMS','RNP4','RNP10','Location','northwest')
xlabel('x')
ylabel('y [m]')
grid on