ECEF2BLH

ECEF2BLH

学习和理解推到过程：https://blog.csdn.net/charlee44/article/details/119979831

LLA坐标系转ECEF坐标系

LLA坐标系下的(lon,lat,alt)转换为ECEF坐标系下点(X,Y,Z)
$$\{\begin{array}{l}X=(N+alt)cos(lat)\ast cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N(1-e^2)+alt)sin(lat)\end{array}$$
2）其中e为椭球偏心率，N为基准椭球体的曲率半径
$$\{\begin{array}{l}{e}^2=(\frac{a^2-b^2}{a^2})\\ N=\frac{a}{\sqrt{1-e^{2}si{n}^2(lat)}}\end{array}$$
3）由于WGS-84下极扁率f=(a−b)/a 偏心率e和极扁率f之间的关系：

$$f=(a-b)/a$$

$$\begin{gathered} e^2=\frac{(a^2-b^2)}{a^2} \\ =\frac{(a-b)(a+b)}{a^2} \\ =\frac{(a-b)(2a-(a-b))}{a^2} \\ =\frac{(a-b)}{a}\ast \frac{(2a-(a-b))}{a} \\ =f\ast (2-f) \end{gathered}$$

(4) 坐标转换公式也可以为
$$\{\begin{array}{l}X=(N+alt)cos(lat)\ast cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N(1-f{)}^2+alt)sin(lat)\end{array}$$
(5) 坐标转换公式也可以为
$$\begin{gathered} 1-f=1-(a-b)/a \\ =\frac{a-a+b}{a} \\ =\frac{b}{a} \end{gathered}$$

$$\{\begin{array}{l}X=(N+alt)cos(lat)\ast cos(lon)\\ Y=(N+alt)cos(lat)sin(lon)\\ Z=(N\ast \frac{a^2}{b^2}+alt)sin(lat)\end{array}$$

ECEF坐标系转LLA坐标系

$$\begin{gathered} lon=arctan(\frac{y}{x}) \\ alt=\frac{p}{cos(lat)-N} \\ lat=arctan[\frac{z}{p}\ast (1-e^2\ast \frac{N}{N+alt}{)}^{-1}] \\ p=\sqrt{x^2+y^2} \end{gathered}$$

具体求解算法参考：坐标系转换之LLA、ECEF、ENU互转 – MathSword数值计算软件

算法二

大地经纬度坐标与地心地固坐标的的转换_地心坐标转换为经纬度坐标matlab-CSDN博客

BLH->XYZ

将P点所在的子午椭圆放在平面上，以圆心为坐标原点，建立平面直接坐标系：

对照地心地固坐标系，很容易得出：

$$\{\begin{array}{l}Z=y\\ X=x\ast cosL\\ Y=x\ast sinL\end{array}$$ (1)

椭球上一点$P$:

​ 图（1）

满足：

$a^2{x}^2+b^2{y}^2=1$ (1-2)

对x求导，有：

$$\frac{\mathrm{d}y}{\mathrm{d}x}=-\frac{b^2}{a^2}\frac{x}{y}$$ (2)

又根据解析几何可知，函数曲线（椭圆）某一点（就是P点）的倒数为该点切线的斜率，也就是正切值：
$$\frac{dy}{dx}=tan(90^o+B)=-cotB\text{\hspace{1em}(3)}$$ (3)

联立公式（2）(3)，可得：

$$\begin{gathered} y=\frac{b^2}{a^2}tanB \\ =x(\frac{a^2-(a^2-b^2)}{a^2})tanB \\ =x(1-\frac{a^2-b^2}{a^2})tanB \end{gathered}$$

又：$$e=\frac{\sqrt{a^2-b^2}}{a}$$,所以：

$$y=x(1-e^2)tanB\text{\hspace{1em}(4)}$$ (4)

其中，e为椭圆第一偏心率

令Pn的距离为N，那么显然有：

x=NcosB (4-2)

根据（4）式可得：
$$y=N(1-e^2)sinB$$ (4-3)

将其带入（1）式，可得到椭球上P点的坐标为：

$$\{\begin{array}{l}X=NcosBcosL\\ Y=NcosBsinL\\ Z=N(1-e^2)sinB\end{array}$$
那么唯一的未知量就是Pn的长度N了，将（4）式带入到椭圆方程式(1.2)：

$$\frac{x^2}{a^2}+\frac{x^2(1-e^2{)}^{2}ta{n}^{2}B}{b^2})=1$$

又： $e^2=\frac{a^2-b^2}{a^2}$ 故：$b^2=a^2(1-e^2)$，则

$$\frac{x^2}{a^2}+\frac{x^2(1-e^2{)}^{2}ta{n}^{2}B}{a^2(1-e^2)})=\frac{x^2(1+(1-e^2)ta{n}^{2}B)}{a^2}=1$$,等式两边乘以$a^{2}co{s}^{2}B$

$x^2[co{s}^{2}B+(1-e^2)si{n}^{2}B]=a^{2}co{s}^{2}B$

$x^2[co{s}^{2}B+si{n}^{2}B-e^{2}si{n}^{2}B]=x^2(1-e^{2}si{n}^{2}B)=a^{2}co{s}^{2}B$

简化为：

$x=\frac{acosB}{\sqrt{1-e^{2}si{n}^{2}B}}$ (6)

联立（5）（6）得：

$$N=\frac{a}{sqrt1-e^{2}si{n}^{2}B}$$ (6-2)

通过式（5）式（6），可以计算椭球上某一点的坐标。但这个点并不是我们真正要求的点，我们要求的点P(B,L,H)是椭球面沿法向量向上H高度的点：

​ 图（2）

P点在椭球面上的点为$P_0$,那么根据矢量相加的性质，有：

$$P=P_0+H.n$$ （6-3）

其中，$P_0$也就是式（5），而n是$P_0$在椭球面的返现单位矢量。

矢量在任意位置的方向都是一样的，那么我们可以假设存在一个单位球(球的半径为单位1)，将法线单位矢量移动到球心位置，可得法线单位矢量为：

$$n=\left[\begin{array}{c}cosBcosL\\ cosBsinL\\ sinB\end{array}\right]$$ (7)

因此有

$$P=\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{c}(N+H)cosBcosL\\ (N+H)cosBsinL\\ [N(1-e^2)+H]sinB\end{array}\right]$$ （8）

其中：

$ N = \frac{a}{\sqrt{1-e^2sin2B}}$$ (9)

XYZ->BLH

根据式（8），可知,:

$\frac{Y}{X}=\frac{(N+H)cosBsinL}{(N+H)cosBcosL}=tanL$

故，经度：

$L=arctan(\frac{Y}{X})$ (10)

不过纬度Ｂ就不是那么好算了，首先需要计算法线Pn在赤道两侧的长度。根据图1，有

$y=PQsinB$

与式（4-3）比较可得：

$PQ=N(1-e2)$

显然，由于

$Pn=N=PQ+Qn$

有：$Qn=Ne2$

接下来如下图所示，对图1做辅助线：

图（3）

有：

$$\{\begin{array}{l}P{P}^{‘‘}=Z\\ O{P}^{‘‘}=\sqrt{x^2+y^2}\\ P{P}^{‘‘‘}=O{K}_p=Q{K}_{p}sinB=N{e}^{2}sinB\\ P^{‘‘}{P}^{‘‘‘}=P{P}^{‘‘}+P{P}^{‘‘‘}\end{array}$$

因而可得：

$tanB=\frac{Z+N{e}^{2}sinB}{\sqrt{x^2+x^2}}$ (11)

这个式子两边都有待定量B，需要用迭代法进行求值。具体可参看代码实现，初始的待定值可取

$tanB=\frac{z}{\sqrt{x^2+y^2}}$

大地纬度B已知，那么求高度H就非常简单了，直接根据式（8）中的第三式逆推可得：

$H = \frac{Z}{sinB} - N(1-e^2) $ (12)

汇总三式，可得：

$\{\begin{array}{l}L=arctan(\frac{Y}{X})\\ tanB=\frac{Z+N{e}^{2}sinB}{\sqrt{x^2+x^2}}\\ H=\frac{Z}{sinB}-N(1-e^2)\end{array}$