clear all
close all
clc


%Sim parameters
sim_time = 200;                                                                            %simulation lenght 
n = 3;                                                                                     %number of spatial dimension 
c = 299792458;                                                                             %light speed
f_carry = 1090*(10^(6));                                                                   %ADS-B carry freq
SNR = 70;

%Airport zone parameters
r = 3000;                                                                                  %Airport radius 
xc = 5000;
yc = 0;
theta = linspace(0,2*pi);
x = r*cos(theta) + xc;                                                                     %Airport zone boundaries (ATZ)
y = r*sin(theta) + yc;
z = 5000;                                                                                  %Airport vertical boundaries
N_of_A_max = 7;
Number_of_aircraft =1;% round(3+(N_of_A_max-3)*rand(1,1));                                    %Number of planes in simulation (1-10)

%Signal_strength reciever
xc_SS = 4700;
yc_SS = 100;
r_rec_SS = 50;
x_SS = r_rec_SS*cos(theta) + xc_SS;
y_SS = r_rec_SS*sin(theta) + yc_SS;
P_tr = 100;
G_tr = 1;
G_rec = 1;

%Recievers AoA co-ordinates
xc_Reci1 = 4700;
yc_Reci1 = 0;
xc_Reci2 = 5300;
yc_Reci2 = 0;
r_rec = 50;                                                                                  

x_Reci1 = r_rec*cos(theta) + xc_Reci1;                                                                   
y_Reci1 = r_rec*sin(theta) + yc_Reci1;                                                                                 
x_Reci2 = r_rec*cos(theta) + xc_Reci2;                                                                   
y_Reci2 = r_rec*sin(theta) + yc_Reci2;

wave_length = c/f_carry;
antenna_distance = wave_length/2;
reciever_distance = abs(xc_Reci1 - xc_Reci2);


%.....Reciever AoA 1......................
x1_Reci1 = xc_Reci1 - antenna_distance/2; 
x2_Reci1 = xc_Reci1 + antenna_distance/2;
y1_Reci1 = yc_Reci1;  
y2_Reci1 = yc_Reci1;  
P_tr_Reci1 = 100;
G_tr_Reci1 = 1;
G_rec_Reci1 = 1;

%.....Reciever AoA 2 .................
x1_Reci2 = xc_Reci2 - antenna_distance/2;
x2_Reci2 = xc_Reci2 + antenna_distance/2;
y1_Reci2 = yc_Reci2; 
y2_Reci2 = yc_Reci2; 


%.........Recievers for TDoA Multilat...........
xc_Reci1_mlat = 3000;    %4700
yc_Reci1_mlat = -1000;   %-500
xc_Reci2_mlat = 7000;    %5300
yc_Reci2_mlat = -1000;   %-500
xc_Reci3_mlat = 5000;    %5000
yc_Reci3_mlat = 2000;    %500
zc_Reci_mlat  = 0;
r_rec = 50;  

x_Reci1_mlat = r_rec*cos(theta) + xc_Reci1_mlat;                                                                   
y_Reci1_mlat = r_rec*sin(theta) + yc_Reci1_mlat;                                                                                 
x_Reci2_mlat = r_rec*cos(theta) + xc_Reci2_mlat;                                                                   
y_Reci2_mlat = r_rec*sin(theta) + yc_Reci2_mlat;
x_Reci3_mlat = r_rec*cos(theta) + xc_Reci3_mlat;                                                                   
y_Reci3_mlat = r_rec*sin(theta) + yc_Reci3_mlat;


%Airplane initial parameters loading
Aircraft_position_init = zeros(Number_of_aircraft,n);
Aircraft_position = zeros(sim_time,Number_of_aircraft,n);
Aircraft_velocity_mag = zeros(Number_of_aircraft);
Aircraft_velocity_angle_x = zeros(Number_of_aircraft);
Aircraft_velocity_angle_y = zeros(Number_of_aircraft);
fi_aircraft = zeros(Number_of_aircraft);
Aircraft_velocity_x = zeros(sim_time);
Aircraft_velocity_y = zeros(sim_time);
direction_vector_x = zeros(sim_time);
direction_vector_y = zeros(sim_time);
Aircraft_dist_from_aport = zeros(sim_time,Number_of_aircraft);
Zone_flag = zeros(sim_time,Number_of_aircraft);
Alert_flag = zeros(sim_time,Number_of_aircraft,Number_of_aircraft);
Zone_number_of_planes = 0;
Aircraft_mutual_distance = zeros(sim_time,Number_of_aircraft,Number_of_aircraft);

%Airplane flight datas loading
callsign_array = ["\leftarrow OK-FOR","\leftarrow OK-MRK","\leftarrow OK-DSL","\leftarrow OK-ASY","\leftarrow OK-ASB","\leftarrow Krystov 15","\leftarrow Krystov 01"]; %Aircraft callsign 
for a=1:sim_time
  if a == 1  
   for i=1:Number_of_aircraft
     k = round(1 +(100-1)*rand(1,1));
      for j=1:n
       switch j    
         case 1
           if  x(k) >= 0    
            Aircraft_x_min = x(k)+1000;
            Aircraft_x_max = x(k)+2000;
            Aircraft_position_init(i,j) =  Aircraft_x_min + (Aircraft_x_max - Aircraft_x_min)*rand(1,1);
            
           else
            Aircraft_x_min = x(k)-1000;
            Aircraft_x_max = x(k)-2000;
            Aircraft_position_init(i,j) =  Aircraft_x_min + (Aircraft_x_max - Aircraft_x_min)*rand(1,1);    
           end
         case 2
           if y(k) >= 0 
            Aircraft_y_min = y(k)+1000;
            Aircraft_y_max = y(k)+2000; 
            Aircraft_position_init(i,j) =  Aircraft_y_min + (Aircraft_y_max - Aircraft_y_min)*rand(1,1);
           else
            Aircraft_y_min = y(k)-1000;
            Aircraft_y_max = y(k)-2000; 
            Aircraft_position_init(i,j) =  Aircraft_y_min + (Aircraft_y_max - Aircraft_y_min)*rand(1,1);   
           end
         case 3
            Aircraft_position_init(i,j) =  z*rand(1,1);
       end
       Aircraft_position(a,i,j) = Aircraft_position_init(i,j);
      end
      Aircraft_velocity_mag(i,1) = 50 + (110-50)*rand(1,1);  
      Aircraft_velocity_angle_x(i,1) = -0.95 +(0.95+0.95)*rand(1,1);
      if abs(Aircraft_velocity_angle_x(i,1)) >= 0.01 
          Aircraft_velocity_angle_x(i,1) = Aircraft_velocity_angle_x(i,1);
      else
          Aircraft_velocity_angle_x(i,1) = 0.01;
      end
      Aircraft_velocity_angle_y(i,1) = sqrt(1-Aircraft_velocity_angle_x(i,1)^2);
      Aircraft_velocity_angle_y_sign = round(-1 +(1+1)*rand(1,1));
      if Aircraft_velocity_angle_y_sign == 0
          Aircraft_velocity_angle_y_sign = 1;
      end
      Aircraft_velocity_angle_y(i,1) = Aircraft_velocity_angle_y_sign*Aircraft_velocity_angle_y(i,1);
      if Aircraft_position(a,i,1) >= 0 && Aircraft_position(a,i,2) >= 0
          if Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                fi_aircraft(i,1) = pi + atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1));  %angle of aircraft velocity vector in radians reffered to its origin co-ordinates
                
          elseif Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) = (pi/2 + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) + pi/2;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                  fi_aircraft(i,1) = ((3/2)*pi + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) - pi/2;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) =  atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1)) + pi;
                  
          else
                  fi_aircraft(i,1) = 0;
          end
      elseif Aircraft_position(a,i,1) >= 0 && Aircraft_position(a,i,2) <= 0
          if Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                fi_aircraft(i,1) = (pi + atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1))) - pi/2;  %angle of aircraft velocity vector in radians reffered to its origin co-ordinates
                
          elseif Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) = pi/2 + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1))) ;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                  fi_aircraft(i,1) = ((3/2)*pi + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) - pi;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) =  atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1)) + pi/2;
                  
          else
                  fi_aircraft(i,1) = 0;
          end
      elseif Aircraft_position(a,i,1) <= 0 && Aircraft_position(a,i,2) <= 0
          if Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                fi_aircraft(i,1) = (pi + atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1))) - pi;  %angle of aircraft velocity vector in radians reffered to its origin co-ordinates
                
          elseif Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) = (pi/2 + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) - pi/2 ;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                  fi_aircraft(i,1) = ((3/2)*pi + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) + pi/2;
                  
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) =  atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1));
                  
          else
                  fi_aircraft(i,1) = 0;
          end
          elseif Aircraft_position(a,i,1) <= 0 && Aircraft_position(a,i,2) >= 0
          if Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                fi_aircraft(i,1) = (pi + atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1))) + pi/2;  %angle of aircraft velocity vector in radians reffered to its origin co-ordinates
              
          elseif Aircraft_velocity_angle_x(i,1) <= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) = (pi/2 + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)))) + pi ;
                
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) <= 0 
                  fi_aircraft(i,1) = (3/2)*pi + atan(abs(Aircraft_velocity_angle_x(i,1)/Aircraft_velocity_angle_y(i,1)));
                
          elseif Aircraft_velocity_angle_x(i,1) >= 0 && Aircraft_velocity_angle_y(i,1) >= 0 
                  fi_aircraft(i,1) =  atan(Aircraft_velocity_angle_y(i,1)/Aircraft_velocity_angle_x(i,1)) - pi/2;
               
          else
                  fi_aircraft(i,1) = 0;
          end
      else
          fi_aircraft(i,1) = 0;
      end
    Aircraft_velocity_x(i) = cos(fi_aircraft(i,1));
    Aircraft_velocity_y(i) = sin(fi_aircraft(i,1));
    direction_vector_x(i) = Aircraft_velocity_mag(i,1)*Aircraft_velocity_x(i);
    direction_vector_y(i) = Aircraft_velocity_mag(i,1)*Aircraft_velocity_y(i);
   end
  else
   for i = 1:Number_of_aircraft      
    direction_vector_x(i) = Aircraft_velocity_mag(i,1)*Aircraft_velocity_x(i);
    direction_vector_y(i) = Aircraft_velocity_mag(i,1)*Aircraft_velocity_y(i);
    Aircraft_position(a,i,1) = Aircraft_position(a-1,i,1) + direction_vector_x(i);
    Aircraft_position(a,i,2) = Aircraft_position(a-1,i,2) + direction_vector_y(i);
  %  Aircraft_dist_from_aport(a,i) = sqrt((Aircraft_position(a,i,1) - xc)^2+(Aircraft_position(a,i,2) - yc)^2);   %Aircraft distance from airport center
    
    
    Aircraft_dist_from_x1y1_Reci1(a,i) = sqrt((Aircraft_position(a,i,1) - x1_Reci1)^2+(Aircraft_position(a,i,2) - y1_Reci1)^2);
    Aircraft_dist_from_x2y2_Reci1(a,i) = sqrt((Aircraft_position(a,i,1) - x2_Reci1)^2+(Aircraft_position(a,i,2) - y2_Reci1)^2);
    Aircraft_dist_from_x1y1_Reci2(a,i) = sqrt((Aircraft_position(a,i,1) - x1_Reci2)^2+(Aircraft_position(a,i,2) - y1_Reci2)^2);
    Aircraft_dist_from_x2y2_Reci2(a,i) = sqrt((Aircraft_position(a,i,1) - x2_Reci2)^2+(Aircraft_position(a,i,2) - y2_Reci2)^2);

    Aircraft_dist_from_Reci1(a,i) = sqrt((Aircraft_position(a,i,1) - xc_Reci1)^2+(Aircraft_position(a,i,2) - yc_Reci1)^2);
    Aircraft_dist_from_Reci2(a,i) = sqrt((Aircraft_position(a,i,1) - xc_Reci2)^2+(Aircraft_position(a,i,2) - yc_Reci2)^2);
    
    Aircraft_dist_SS(a,i) = sqrt((Aircraft_position(a,i,1) - xc_SS)^2+(Aircraft_position(a,i,2) - yc_SS)^2);
    L_loss(a,i) = (wave_length/(4*pi*Aircraft_dist_SS(a,i)))^2;
    P_rec(a,i) = awgn(P_tr*G_tr*G_rec*L_loss(a,i),SNR,'measured');
    
    L_loss_Reci1(a,i) = (wave_length/(4*pi*Aircraft_dist_from_Reci1(a,i)))^2;
    P_rec_Reci1(a,i) = awgn(P_tr_Reci1*G_tr_Reci1*G_rec_Reci1*L_loss_Reci1(a,i),SNR,'measured');
    
    %......Aircraft distance from Antenna recievers....TDoA/ToA for AoA............
    toa1_Reci1(a,i) = Aircraft_dist_from_x1y1_Reci1(a,i)/c;
    toa2_Reci1(a,i) = Aircraft_dist_from_x2y2_Reci1(a,i)/c;
    toa1_Reci2(a,i) = Aircraft_dist_from_x1y1_Reci2(a,i)/c;
    toa2_Reci2(a,i) = Aircraft_dist_from_x2y2_Reci2(a,i)/c;
    TDoA_Reci1(a,i) = toa1_Reci1(a,i) - toa2_Reci1(a,i);
    TDoA_Reci2(a,i) = toa2_Reci2(a,i) - toa1_Reci2(a,i);
    
    %.....Angle of arrival with North refference.......
    
     delta_fi_Reci1(a,i) = abs((c*TDoA_Reci1(a,i)));
     delta_fi_Reci2(a,i) = abs((c*TDoA_Reci2(a,i)));
     alpha_g = pi/2;
     alpha_g_deg = rad2deg(alpha_g);
     alpha_m(a,i) = asin((delta_fi_Reci1(a,i))/(antenna_distance)); 
     alpha_m_deg(a,i) = rad2deg(alpha_m(a,i));
     if TDoA_Reci1(a,i) >= 0
     alpha_deg(a,i) = alpha_g_deg - alpha_m_deg(a,i);
     else
     alpha_deg(a,i) = alpha_g_deg + alpha_m_deg(a,i);   
     end
     alpha(a,i) = deg2rad(alpha_deg(a,i));
     
     beta_g = 2*pi;
     beta_g_deg = rad2deg(beta_g);
     beta_m(a,i) = asin((delta_fi_Reci2(a,i))/(antenna_distance));
     beta_m_deg(a,i) = rad2deg(beta_m(a,i));
     if TDoA_Reci2(a,i) >= 0
     beta_deg(a,i) = real(beta_g_deg - beta_m_deg(a,i));
     else
     beta_deg(a,i) = real(beta_g_deg + beta_m_deg(a,i));   
     end
     beta_deg_recal(a,i) = beta_deg(a,i)-270;
     beta_recal(a,i) = deg2rad(beta_deg_recal(a,i));
     Reci1_dist(a,i) = reciever_distance*((sin(beta_recal(a,i)))/(sin(pi-(beta_recal(a,i) + alpha(a,i)))));
     Reci2_dist(a,i) = reciever_distance*((sin(alpha(a,i)))/(sin(pi-(beta_recal(a,i) + alpha(a,i)))));
     
                                    
     %........Position of Aircraft based on Reciever 1........................ 
     Aircraft_position_AoA_x_Reci1(a,i) = Reci1_dist(a,i)*cos(alpha(a,i)) + abs(xc_Reci1);
     Aircraft_position_AoA_y_Reci1(a,i) = Reci1_dist(a,i)*sin(alpha(a,i)) + abs(yc_Reci1);
     Aircraft_position_AoA_x_Reci1(a,i) = awgn(Aircraft_position_AoA_x_Reci1(a,i),SNR,'measured');
     Aircraft_position_AoA_y_Reci1(a,i) = awgn(Aircraft_position_AoA_y_Reci1(a,i),SNR,'measured');
     %........Position of Aircraft based on Reciever 2........................
     Aircraft_position_AoA_x_Reci2(a,i) = -1*(Reci2_dist(a,i)*cos(beta_recal(a,i)) - abs(xc_Reci2));
     Aircraft_position_AoA_y_Reci2(a,i) = Reci2_dist(a,i)*sin(beta_recal(a,i)) + abs(yc_Reci2);
     Aircraft_position_AoA_x_Reci2(a,i) = awgn(Aircraft_position_AoA_x_Reci2(a,i),SNR,'measured');
     Aircraft_position_AoA_y_Reci2(a,i) = awgn(Aircraft_position_AoA_y_Reci2(a,i),SNR,'measured');

         Aircraft_position_AoA_x(a,i) = Aircraft_position_AoA_x_Reci2(a,i);
         if a > 1
             P_rec_delta(a,i) = P_rec(a,i) - P_rec(a-1,i);
             Aircraft_position_AoA_y_delta(a,i) = (Aircraft_position_AoA_y_Reci2(a,i) - Aircraft_position_AoA_y_Reci2(a-1,i));
             if ((P_rec_delta(a,i) < 0 && Aircraft_position_AoA_y_delta(a,i) < 0)  || (P_rec_delta(a,i) > 0 && Aircraft_position_AoA_y_delta(a,i) < 0)) 
                 if P_rec_Reci1(a,i) <= P_rec(a,i)
                  Aircraft_position_AoA_y(a,i) = Aircraft_position_AoA_y_Reci2(a,i); 
                 else
                  Aircraft_position_AoA_y(a,i) = -1*Aircraft_position_AoA_y_Reci2(a,i);  
                 end
             else
                 if P_rec_Reci1(a,i) <= P_rec(a,i)
                  Aircraft_position_AoA_y(a,i) = (Aircraft_position_AoA_y_Reci2(a,i));
                 else
                  Aircraft_position_AoA_y(a,i) = -1*(Aircraft_position_AoA_y_Reci2(a,i));   
                 end
                
             end
         else
         Aircraft_position_AoA_y(a,i) = Aircraft_position_AoA_y_Reci2(a,i);
        
         end

   %..........Error difference due to awgn.........................
         delta_position_error_AoA_SSy(a,i) = Aircraft_position(a,i,2)-Aircraft_position_AoA_y(a,i); 
         delta_position_error_AoA_SSx(a,i) = Aircraft_position(a,i,1)-Aircraft_position_AoA_x(a,i);
         
         if abs(delta_position_error_AoA_SSy(a,i))<0.5
             delta_position_error_AoA_SSy(a,i) = 0;
         end
         if abs(delta_position_error_AoA_SSx(a,i))<0.5
             delta_position_error_AoA_SSx(a,i)=0;
         end
         
         if abs(delta_position_error_AoA_SSy(a,i))>500
             delta_position_error_AoA_SSy(a,i) = 1000;
         end
         if abs(delta_position_error_AoA_SSx(a,i))>500
             delta_position_error_AoA_SSx(a,i)=1000;
         end
         
    %.............Multilat_TDoA.......................................
    Aircraft_dist_from_Reci1_mlat(a,i) = sqrt((Aircraft_position(a,i,1) - xc_Reci1_mlat)^2+(Aircraft_position(a,i,2) - yc_Reci1_mlat)^2);
    Aircraft_dist_from_Reci2_mlat(a,i) = sqrt((Aircraft_position(a,i,1) - xc_Reci2_mlat)^2+(Aircraft_position(a,i,2) - yc_Reci2_mlat)^2);
    Aircraft_dist_from_Reci3_mlat(a,i) = sqrt((Aircraft_position(a,i,1) - xc_Reci3_mlat)^2+(Aircraft_position(a,i,2) - yc_Reci3_mlat)^2);
    
    toa_Reci1_mlat(a,i) = Aircraft_dist_from_Reci1_mlat(a,i)/c;
    toa_Reci2_mlat(a,i) = Aircraft_dist_from_Reci2_mlat(a,i)/c;
    toa_Reci3_mlat(a,i) = Aircraft_dist_from_Reci3_mlat(a,i)/c;
%     h1_Reci_mlat(a,i) = Aircraft_dist_from_Reci1_mlat(a,i) - Aircraft_dist_from_Reci3_mlat(a,i);
%     h2_Reci_mlat(a,i) = Aircraft_dist_from_Reci2_mlat(a,i) - Aircraft_dist_from_Reci3_mlat(a,i);
    h1_Reci_mlat(a,i) = (toa_Reci1_mlat(a,i) - toa_Reci3_mlat(a,i))*c;
    h2_Reci_mlat(a,i) = (toa_Reci2_mlat(a,i) - toa_Reci3_mlat(a,i))*c;
    
    L1 = xc_Reci1_mlat^2 + yc_Reci1_mlat^2 - xc_Reci3_mlat^2 - yc_Reci3_mlat^2;
    L2 = xc_Reci2_mlat^2 + yc_Reci2_mlat^2 - xc_Reci3_mlat^2 - yc_Reci3_mlat^2;
    
    a_mlat(a,i)=(h2_Reci_mlat(a,i)*(xc_Reci3_mlat-xc_Reci1_mlat))+(h1_Reci_mlat(a,i)*(xc_Reci2_mlat-xc_Reci3_mlat));
    b_mlat(a,i)=(h2_Reci_mlat(a,i)*(yc_Reci3_mlat-yc_Reci1_mlat))+(h1_Reci_mlat(a,i)*(yc_Reci2_mlat-yc_Reci3_mlat));
    c_mlat(a,i)=(((h2_Reci_mlat(a,i))*L1) - (h2_Reci_mlat(a,i)*((h1_Reci_mlat(a,i))^2)) - ((h1_Reci_mlat(a,i))*L2) + ((h1_Reci_mlat(a,i))*((h2_Reci_mlat(a,i))^2)))/2;
    
    e_mlat(a,i)= -((a_mlat(a,i))/(b_mlat(a,i)));
    f_mlat(a,i)= -((c_mlat(a,i))/(b_mlat(a,i)));
    
    P_mlat(a,i)= 4*((h1_Reci_mlat(a,i))^2) * (1+(e_mlat(a,i))^2);
    Q_mlat(a,i)= 8*((h1_Reci_mlat(a,i))^2)*(-xc_Reci3_mlat + ((e_mlat(a,i))*(f_mlat(a,i))) - (e_mlat(a,i))*(yc_Reci3_mlat));
    R_mlat(a,i)= 4*((h1_Reci_mlat(a,i))^2)* ((xc_Reci3_mlat)^2 + (f_mlat(a,i))^2 + (yc_Reci3_mlat)^2 - (2*(f_mlat(a,i))*(yc_Reci3_mlat)));
    M_mlat(a,i)= xc_Reci3_mlat - xc_Reci1_mlat + ((e_mlat(a,i))*(yc_Reci3_mlat-yc_Reci1_mlat));
    N_mlat(a,i)= (2*(f_mlat(a,i))*(yc_Reci3_mlat-yc_Reci1_mlat)) + L1 - ((h1_Reci_mlat(a,i))^2);
    
    A_mlat(a,i)= 4*((M_mlat(a,i))^2) - P_mlat(a,i);
    B_mlat(a,i)= (4*(M_mlat(a,i))*(N_mlat(a,i))) - Q_mlat(a,i);
    C_mlat(a,i)= ((N_mlat(a,i))^2)- R_mlat(a,i);
    
    x1_mlat(a,i)=(-B_mlat(a,i)+(sqrt(((B_mlat(a,i))^2)-4*(A_mlat(a,i))*(C_mlat(a,i)))))/(2*(A_mlat(a,i)));
    x2_mlat(a,i)=(-B_mlat(a,i)-(sqrt(((B_mlat(a,i))^2)-4*(A_mlat(a,i))*(C_mlat(a,i)))))/(2*(A_mlat(a,i)));
    
    x1_mlat(a,i) = awgn(x1_mlat(a,i),SNR,'measured');
    x2_mlat(a,i) = awgn(x2_mlat(a,i),SNR,'measured');
    
    y1_mlat(a,i)=((e_mlat(a,i))*(x1_mlat(a,i))) + f_mlat(a,i);
    y2_mlat(a,i)=((e_mlat(a,i))*(x2_mlat(a,i))) + f_mlat(a,i);
    
    y1_mlat(a,i) = awgn(y1_mlat(a,i),SNR,'measured');
    y2_mlat(a,i) = awgn(y2_mlat(a,i),SNR,'measured');

    %......Data fusion AoA & TDoA hyperbolic method to filter incorrect co-ordinates........
    Delta_AoAx_TDoA_x1(a,i) = Aircraft_position_AoA_x(a,i) - x1_mlat(a,i); 
    Delta_AoAx_TDoA_x2(a,i) = Aircraft_position_AoA_x(a,i) - x2_mlat(a,i);
    Delta_AoAy_TDoA_y1(a,i) = Aircraft_position_AoA_y(a,i) - y1_mlat(a,i);
    Delta_AoAy_TDoA_y2(a,i) = Aircraft_position_AoA_y(a,i) - y2_mlat(a,i);
    
    
    
    %......Data fusion AoA & TDoA hyperbolic method to filter incorrect co-ordinates.......
    if  abs(Delta_AoAx_TDoA_x2(a,i)) >= 100
        Aircraft_position_X(a,i) = x1_mlat(a,i);
    else
        
        Aircraft_position_X(a,i) = x2_mlat(a,i);
        
    end
    
    if  abs(Delta_AoAy_TDoA_y2(a,i)) >= 100
        Aircraft_position_Y(a,i) = y1_mlat(a,i);
    else
       
        Aircraft_position_Y(a,i) = y2_mlat(a,i);
        
    end
    
    delta_position_error_Hyperx(a,i) = Aircraft_position(a,i,1)- Aircraft_position_X(a,i);
    delta_position_error_Hypery(a,i) = Aircraft_position(a,i,2)- Aircraft_position_Y(a,i);
    
    %..........ATZ zone traffic control................................
    Aircraft_dist_from_aport(a,i) = sqrt((Aircraft_position_AoA_x(a,i) - xc)^2+(Aircraft_position_AoA_y(a,i) - yc)^2);
    if Aircraft_dist_from_aport(a,i) < r
        Zone_flag(a,i) = 1;
        if Aircraft_dist_from_aport(a,i) <= r && Aircraft_dist_from_aport(a-1,i) > r 
            Zone_number_of_planes = Zone_number_of_planes + 1;
        end    
    else
        Zone_flag(a,i) = 0;
    end
   end
   
   %...........Relative position beetween aircrafts................
   for i = 1:Number_of_aircraft
      for j = 1:Number_of_aircraft
        %Aircraft_mutual_distance(a,i,j) = sqrt(((Aircraft_position(a,i,1) - Aircraft_position(a,j,1))^2) + ((Aircraft_position(a,i,2) - Aircraft_position(a,j,2))^2));
         Aircraft_mutual_distance(a,i,j) = sqrt((Aircraft_position_AoA_x(a,i)-Aircraft_position_AoA_x(a,j))^2 + ((Aircraft_position_AoA_y(a,i) - Aircraft_position_AoA_y(a,j))^2));
         if Aircraft_mutual_distance(a,i,j) <= 2000 && i~=j                   %Alert trigger
            Alert_flag(a,i,j) = 1;
         else
            Alert_flag(a,i,j) = 0;
        end   
      end
   end
  end
  
end



figure(1);
for a=1:sim_time
    milliPause(10)
   plot(x,y)
   axis equal
   hold on
   plot(x_Reci1,y_Reci1,x_Reci2,y_Reci2)
   hold on
   plot(x_SS,y_SS)
   hold on
   plot(x_Reci1_mlat,y_Reci1_mlat,x_Reci2_mlat,y_Reci2_mlat,x_Reci3_mlat,y_Reci3_mlat)
   hold on
     for i =1:Number_of_aircraft  
       if Zone_flag(a,i) == 1 
               col = 'y';
%                 plot(Aircraft_position(a,i,1),Aircraft_position(a,i,2),'or','MarkerSize',5,'MarkerFaceColor',col)
%                 text(Aircraft_position(a,i,1),Aircraft_position(a,i,2),callsign_array(i))
%                 plot(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
%                 text(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),callsign_array(i))
               plot(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
               text(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),callsign_array(i))
                hold on
                axis([0 10000 -5000 5000])
       end  
     end
     for i =1:Number_of_aircraft  
       for j=1:Number_of_aircraft
        if Zone_flag(a,i) == 1  && Alert_flag(a,i,j) == 1   
               col = 'r';
%                 plot(Aircraft_position(a,i,1),Aircraft_position(a,i,2),'or','MarkerFaceColor',col)
%                 text(Aircraft_position(a,i,1),Aircraft_position(a,i,2),callsign_array(i),'Color',col)
%                 plot(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
%                 text(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),callsign_array(i))
               plot(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
               text(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),callsign_array(i))
                hold on
                axis([0 10000 -5000 5000])
        end        
       end
     end
     for i =1:Number_of_aircraft  
       if  Zone_flag(a,i) == 0
           if i <= 6                                                                              %Limited number of colors
              col = de2bi(i+1,3);                                                                 %Integer to binaray value for plot color parameter except of red color for collision alert
%                plot(Aircraft_position(a,i,1),Aircraft_position(a,i,2),'or','MarkerSize',5,'MarkerFaceColor',col)
%                text(Aircraft_position(a,i,1),Aircraft_position(a,i,2),callsign_array(i))
%                plot(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
%                text(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),callsign_array(i))
               plot(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
               text(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),callsign_array(i))
               hold on
               axis([0 10000 -5000 5000])
           else
              col = 'b';  
%                plot(Aircraft_position(a,i,1),Aircraft_position(a,i,2),'or','MarkerSize',5,'MarkerFaceColor',col)
%                text(Aircraft_position(a,i,1),Aircraft_position(a,i,2),callsign_array(i))
%                plot(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
%                text(Aircraft_position_AoA_x(a,i),Aircraft_position_AoA_y(a,i),callsign_array(i))
               plot(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),'or','MarkerSize',5,'MarkerFaceColor',col)
               text(Aircraft_position_X(a,i),Aircraft_position_Y(a,i),callsign_array(i))
               hold on
               axis([0 10000 -5000 5000])
           end
        end
     end
     hold off
     pause(.0001)  
end
 
 

figure(2) 
plot(delta_position_error_AoA_SSx)
axis([0 sim_time -1000 1000])
title('AoA x position deviation')
xlabel('N')
ylabel('Deviation x')

figure(3)
plot(delta_position_error_AoA_SSy) 
axis([0 sim_time -1000 1000])
title('AoA y position deviation')
xlabel('N')
ylabel('Deviation y')


figure(4) 
plot(delta_position_error_Hyperx)
axis([0 sim_time -1000 1000])
title('TDoA hyperbolic x position deviation')
xlabel('N')
ylabel('Deviation x')

figure(5)
plot(delta_position_error_Hypery) 
axis([0 sim_time -1000 1000])
title('TDoA hyperbolic y position deviation')
xlabel('N')
ylabel('Deviation y')
  





