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

%% Input Values: 
% [x_n,y_n,z_n] ... input values for 1st iteration (zeros, previous ENU coordinates of the wanted aircraft 
% or coordinates of the nearest aircraft from previous layer)
% after that (for next iterations) x_n1, y_n1, z_n1 values are assigned
% [XE,YN,ZU] ... ENU coordinates of aircraft from previous layer
% dT ... delta time between [XE,YN,ZU] and [x_n,y_n,z_n], size must be 3x1 or 1x3
% tol ... error tolerance (tolerance of Mode S)
%% Output Values:
% [x,y,z] ... calculated position of wanted aircraft
% A ... amount of iterations
% R...distances including error

%% Input Data for Iteration
A=0;
x_n1=x_n; % x_n+1
y_n1=y_n; % y_n+1
z_n1=z_n; % z_n+1

[row,col]=size(dT); 

r = normrnd(0,tol,row,col); % Mode S error [s]; size must be the same as the size of dT
cs=299792458; % speed of light [m/s]
R=abs(cs.*(dT+r)); % distances from wanted aircraft

% 1st iteration
R01=sqrt((XE(1)-x_n)^2+(YN(1)-y_n)^2+(ZU(1)-z_n)^2); 
R02=sqrt((XE(2)-x_n)^2+(YN(2)-y_n)^2+(ZU(2)-z_n)^2); 
R03=sqrt((XE(3)-x_n)^2+(YN(3)-y_n)^2+(ZU(3)-z_n)^2); 
% Inverse Matrix A
invA=inv([(x_n-XE(1))/R01, (y_n-YN(1))/R01, (z_n-ZU(1))/R01;(x_n-XE(2))/R02, (y_n-YN(2))/R02, (z_n-ZU(2))/R02; 
    (x_n-XE(3))/R03, (y_n-YN(3))/R03, (z_n-ZU(3))/R03]);
d=invA*[R(1)-R01;R(2)-R02;R(3)-R03]; %[dx,dy,dz]

% Position of Wanted Aircraft
    x_n1=x_n1+d(1);
    y_n1=y_n1+d(2);
    z_n1=z_n1+d(3); 
    
% Sum of Iterations
    A=A+1; 
     
 while abs(z_n1-z_n)>10^(-3) || abs(x_n1-x_n)>10^(-3) || abs(y_n1-y_n)>10^(-3)
  
        if A==20
            break
        end
        
x_n=x_n1;
y_n=y_n1;
z_n=z_n1;

R01=sqrt((XE(1)-x_n)^2+(YN(1)-y_n)^2+(ZU(1)-z_n)^2); 
R02=sqrt((XE(2)-x_n)^2+(YN(2)-y_n)^2+(ZU(2)-z_n)^2); 
R03=sqrt((XE(3)-x_n)^2+(YN(3)-y_n)^2+(ZU(3)-z_n)^2); 

invA=inv([(x_n-XE(1))/R01, (y_n-YN(1))/R01, (z_n-ZU(1))/R01;(x_n-XE(2))/R02, (y_n-YN(2))/R02, (z_n-ZU(2))/R02; 
    (x_n-XE(3))/R03, (y_n-YN(3))/R03, (z_n-ZU(3))/R03]); % Inv Matrix A
d=invA*[R(1)-R01;R(2)-R02;R(3)-R03]; %[dx,dy,dz]

% Position of Wanted Aircraft
    x_n1=x_n1+d(1);
    y_n1=y_n1+d(2);
    z_n1=z_n1+d(3);  
     A=A+1; % sum of iterations
 end
    
 % Results 
    x=x_n1;
    y=y_n1;
    z=z_n1;
end

%%
