GNSS仰角和方位角的计算及代码，XYZ转BLH坐标的代码及原理

function [E,A]= Get_EA(sx,sy,sz,x,y,z)
%GET_EA Summary of this function goes here
%sx,sy,sz:站点的XYZ坐标，x,y,z:卫星的XYZ坐标
%   Detailed explanation goes here
[sb,sl]=XYZtoBLH(sx,sy,sz);
T=[-sin(sb)*cos(sl) -sin(sb)*sin(sl) cos(sb);
    -sin(sl)               cos(sl)         0;
    cos(sb)*cos(sl) cos(sb)*sin(sl)  sin(sb)];%transition matrix(XYZ to NEU)
deta_xyz=[x,y,z]-[sx,sy,sz];
NEU=T*(deta_xyz)';
E=atan(NEU(3)/sqrt(NEU(1)*NEU(1)+NEU(2)*NEU(2)));
A=atan(abs(NEU(2)/NEU(1)));
if NEU(1)>0
    if NEU(2)>0
    else
        A=2*pi-A;
    end
else
    if NEU(2)>0
        A=pi-A;
    else
        A=pi+A;
    end 
end
end

计算仰角(E)和方位角(A)的公式：

[E=arctanleft(frac{cos(phi_2-phi_1) imes coseta-0.15}{sqrt{1-left[cos(phi_2-phi_1) imes coseta ight]^2}} ight) ag{1} ]

[A=arctanleft(frac{tan(phi_2-phi_1)}{sineta} ight) ag{2} ]

对于输入时是XYZ坐标的卫星位置和接收机位置还要进行坐标转换

先将接收机位置的XYZ坐标转换成大地坐标系（BLH），转换公式为：

[L=arctanleft(frac{Y}{X} ight) ag{3} ]

[B=arctanleft(frac{Z(N+H)}{sqrt{(X^2+Y^2)[N(1-e^2)+H]}} ight) ag{4} ]

[H=frac{Z}{sinB}-N(1-e^2) ag{5} ]

(N)为卵冒圈的半径，(e)表示椭球扁率，(a)和(b)分别表示长半轴和短半轴。

[N=frac{a}{sqrt{1-e^2sin^2B}}, e^2=a^2-b^2 ag{6} ]

利用上面的式子有一个问题就是式((4)​)和((5)​)中有交叉变量。因此一般采用下面的方法迭代计算：

• 首先计算出(B)的初值

  [B=arctanleft(frac{Z}{sqrt{X^2+Y^2}} ight) ag{7} ]

• 然后利用(B)的初值求出(H、N)的初值，再求定(B)的值

  [N=frac{a}{sqrt{1-e^2sin^2B}} ag{8} ]

XYZ坐标转换为BLH坐标的matlab程序为：

function [B,L] = XYZtoBLH(X,Y,Z)
%WGS84坐标转换到大地经纬度
%   Detailed explanation goes here
a=6378137;
e2=0.0066943799013;
L=atan(abs(Y/X));
if Y>0
    if X>0
    else
        L=pi-L;
    end
else
    if X>0
        L=2*pi-L;
    else
        L=pi+L;
    end
end
B0=atan(Z/sqrt(X^2+Y^2));
while 1
    N=a/sqrt(1-e2*sin(B0)*sin(B0));
    H=Z/sin(B0)-N*(1-e2);
    B=atan(Z*(N+H)/(sqrt(X^2+Y^2)*(N*(1-e2)+H)));
    if abs(B-B0)<1e-6;break;end
    B0=B;
end
N=a/sqrt(1-e2*sin(B)*sin(B));
end

也可以采用如下的方法直接转换

[L=arctan(frac{Y}{X}) ag{9} ]

[e'^2=frac{a^2-b^2}{b^2}, heta=arctanleft(frac{Z·a}{sqrt{X^2+Y^2}·b} ight) ag{10} ]

[B=arctanleft(frac{Z+e'^2bsin^3 heta}{sqrt{X^2+Y^2}-e'^2acos^3 heta} ight) ag{11} ]

[H=frac{sqrt{X^2+Y^2}}{cosB}-N ag{12} ]