%funkce pro výpoèet 2D polohy u již asociovaných zpráv

function  XY=poloha_mlat_2(delta_1,delta_2,Rlat,Rlon,Ralt)

    
WGS84_ip1=[50.080661  14.395599 342];
WGS84_ip2=[50.067771 14.423582 214];
WGS84_ip3=[50.0504 14.4362 375];

WGS84_BOD=[50 14 300];

ECEF_ip1=WGS842ECEF(WGS84_ip1);
ECEF_ip2=WGS842ECEF(WGS84_ip2);
ECEF_ip3=WGS842ECEF(WGS84_ip3);
% ECEF_BOD=WGS842ECEF(WGS84_BOD);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

WGS84_cil=[Rlat Rlon Ralt ];
ECEF_cil=WGS842ECEF(WGS84_cil);
[e, n, u]=xyz2enu(WGS84_BOD(1),WGS84_BOD(2),WGS84_BOD(3),ECEF_cil(1),ECEF_cil(2),ECEF_cil(3));
P_cil=[e,n,u];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[e, n, u]=xyz2enu(WGS84_BOD(1),WGS84_BOD(2),WGS84_BOD(3),ECEF_ip1(1),ECEF_ip1(2),ECEF_ip1(3));
P1=[e,n,u];
[e, n, u]=xyz2enu(WGS84_BOD(1),WGS84_BOD(2),WGS84_BOD(3),ECEF_ip2(1),ECEF_ip2(2),ECEF_ip2(3));
P2=[e,n,u];
[e, n, u]=xyz2enu(WGS84_BOD(1),WGS84_BOD(2),WGS84_BOD(3),ECEF_ip3(1),ECEF_ip3(2),ECEF_ip3(3));
P3=[e,n,u];


c=299792458;
%=====================================

xc=P1(1,1);
yc=P1(1,2);
% zc=P1(1,3);

xb1=P2(1,1);
yb1=P2(1,2);
% zb1=P2(1,3);

xb2=P3(1,1);
yb2=P3(1,2);
% zb2=P3(1,3);

h1=(delta_1*(10^(-9)))*c;

h2=(delta_2*(10^(-9)))*c;


L1= (xb1^2) + (yb1^2) - (xc^2) - (yc^2);
L2= (xb2^2) + (yb2^2) - (xc^2) - (yc^2);

a=(h2*(xc-xb1))+(h1*(xb2-xc));
b=(h2*(yc-yb1))+(h1*(yb2-yc));
c=((h2*L1) - (h2*(h1^2)) - (h1*L2) + (h1*(h2^2)))/2;

e= -(a/b);
f= -(c/b);

P= 4*(h1^2) * (1+e^2);
Q=8*(h1^2)*(-xc + (e*f) - e*yc);
R=4*(h1^2)* (xc^2  + f^2 + yc^2 - (2*f*yc));
M= xc - xb1 + (e*(yc-yb1));
N=(2*f*(yc-yb1)) + L1 - (h1^2);

A=4*(M^2) - P;
B=(4*M*N) - Q;
C=(N^2)- R;

x1=(-B+(sqrt((B^2)-4*A*C)))/(2*A);
x2=(-B-(sqrt((B^2)-4*A*C)))/(2*A);

y1=(e*x1)+f;
y2=(e*x2)+f;



% Rozdíl spoèítané polohy vùèi pøijaté poloze pro urèení vzdálenosti
 chyba_1=[P_cil(1,1)-x1 , P_cil(1,2)-y1];
 chyba_2=[P_cil(1,1)-x2 , P_cil(1,2)-y2];
 

%zobrazí se nám hodnoty odchylek pro všechny asociované zprávy na vybrané
%kombinaci pøijímaèù
    if abs(chyba_1(1,1)) > abs(chyba_1(1,2))
        
        odchylka1=((chyba_1(1,1))^2-(chyba_1(1,2))^2)^(1/2);
    else
        odchylka1=((chyba_1(1,2))^2-(chyba_1(1,1))^2)^(1/2); 
    end
   %ošetøení aby pod odmocinou nevzniklo záporné èíslo 
    if abs(chyba_2(1,1)) > abs(chyba_2(1,2))
        
        odchylka2=((chyba_2(1,1))^2-(chyba_2(1,2))^2)^(1/2);
    else
        odchylka2=((chyba_2(1,2))^2-(chyba_2(1,1))^2)^(1/2);
    end


  XY=[odchylka1 , odchylka2, x1, y1];
end

