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=arctan\left(\frac{cos(\phi_2-\phi_1)\times cos\beta-0.15}{\sqrt{1-\left[cos(\phi_2-\phi_1)\times cos\beta\right]^2}}\right)\tag{1} \]
\[ A=arctan\left(\frac{tan(\phi_2-\phi_1)}{sin\beta}\right)\tag{2} \]
對於輸入時是XYZ座標的衛星位置和接收機位置還要進行座標轉換
先將接收機位置的XYZ座標轉換成大地座標系(BLH),轉換公式為:
\[ L=arctan\left(\frac{Y}{X}\right)\tag{3} \]
\[ B=arctan\left(\frac{Z(N+H)}{\sqrt{(X^2+Y^2)[N(1-e^2)+H]}}\right)\tag{4} \]
\[ H=\frac{Z}{sinB}-N(1-e^2)\tag{5} \]
\(N\)為卵冒圈的半徑,\(e\)表示橢球扁率,\(a\)和\(b\)分別表示長半軸和短半軸。
\[ N=\frac{a}{\sqrt{1-e^2sin^2B}},\ \ e^2=a^2-b^2\tag{6} \]
利用上面的式子有一個問題就是式\((4)\)和\((5)\)中有交叉變數。因此一般採用下面的方法迭代計算:
首先計算出\(B\)
\[ B=arctan\left(\frac{Z}{\sqrt{X^2+Y^2}}\right)\tag{7} \]然後利用\(B\)的初值求出\(H、N\)的初值,再求定\(B\)的值
\[ N=\frac{a}{\sqrt{1-e^2sin^2B}}\tag{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})\tag{9} \]
\[ e'^2=\frac{a^2-b^2}{b^2},\ \ \ \theta=arctan\left(\frac{Z·a}{\sqrt{X^2+Y^2}·b}\right)\tag{10} \]
\[ B=arctan\left(\frac{Z+e'^2bsin^3\theta}{\sqrt{X^2+Y^2}-e'^2acos^3\theta}\right)\tag{11} \]
\[ H=\frac{\sqrt{X^2+Y^2}}{cosB}-N\tag{12} \]