1. 基本概念

1.1欧拉角

欧拉旋转定理指出：任何一个旋转都可以用三个旋转的参数来表示。

1. 三个旋转角的组合方式（是xyz还是yzx还是zxy）

为了方便，我们用x指代只绕x轴的旋转，用y指代只绕y轴进行的旋转。在描述欧拉角的时候可以有以下方式：xyz， yzx，zxy 或者是反向顺序 zyx xzy yxz，共六种。

2. 旋转角度的参考坐标系统（旋转是相对于固定的坐标系还是相对于自身的坐标系）

在描述这3个旋转角的时候，可以是相对于某个固定的坐标系统（称为extrinsic rotations），也可以是相对于物体自身的坐标系统（称为intrinsic rotations）

3. 使用旋转角度是左手系还是右手系

旋转角度是正还是负，如果是右手系，那么符合右手定则决定正负，左手系则符合左手定则决定正负。

4. 三个旋转角的记法
   
   图 旋转的记法

1.2左乘右乘

左乘还是右乘

左乘——相对于固定坐标系进行变换，（固定坐标系—我们上面提到的固定x轴旋转……，这里的固定，其实我们已经暗暗地定义了一个坐标系，这个坐标系我们潜意识认为它是不变的，这也就是世界坐标系。除了平时我们假设了固定坐标系就是我们认识的世界坐标系，其他情况是我们需要将坐标系1变换到坐标系2，这时，我们的坐标系2可以设为该固定坐标系）

V’ = R * V = Rz * Ry * Rx V，变换顺序是先绕x转，再绕y转，最后绕z转。
右乘——相对于自身坐标系进行变换，每变一次下一次需要以新坐标系为标准进行变换。比如第一次变换后，原x轴的位置变为y轴，那么下一次绕y轴的变换，就会绕之前的x轴变换。

参考 https://blog.csdn.net/silence1214/article/details/8634664

https://blog.csdn.net/a6333230/article/details/88343282?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.control&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-1.control

1.3东北天坐标系

1. 姿态角范围

   (1) 俯仰角(-90 ~ 90deg)；
    (2) 横滚角(-180 ~ 180deg)；
    (3) 航向角(-180 ~ 180deg，可转换为0~360deg)；

2. 轴向

   X轴向东：绕此轴旋转决定俯仰角；
   Y轴向北：绕此轴旋转决定横滚角；
   Z轴向上：绕此轴旋转决定航向角。

1.4载体坐标系

　右手坐标系（正负符合右手定则）：

  X轴向右：绕此轴旋转决定俯仰角；
  Y轴向前，速度的方向：绕此轴旋转决定横滚角；
  Z轴向上：绕此轴旋转决定航向角。

1.5捷联惯性导航系统

捷联惯性导航系统（Strap-down Inertial Navigation System）是把惯性仪表直接固连在载体上，用计算机来完成导航平台功能的惯性导航系统。

将惯性测量器件直接固连在载体上，再将其输出通过数学平台（又称捷联矩阵之转换到导航坐标系的参量），进行导航解算。系统的惯性测量器件为角速率陀螺仪和加速度计，它们固连在载体上，测得的都是载体坐标系下的物理量。

2. 通过ECEF转换到参考点附近的ENU坐标系上

推导过程参考：导航中坐标系及坐标转换

使用ECEF坐标系下求解三维度距离，并在参考点附近进行转换

• 1.参考点P=[lat,lon,height],经纬度

图转换过程

3. 东北天坐标系到载体坐标系

• 使用右手定则确定正负
• 转换使用右乘矩阵（矩阵左乘）的方式，即旋转时是绕载体自己的轴进行旋转，动态
• 旋转顺序为 Z -> X -> Y ，第一次绕 Z 轴旋转，称为航向角 Yaw，第二次 X 绕 轴旋转，称为俯仰角 Pitch，
  第三次绕 Y 轴旋转，称为翻滚角 Roll

各轴正向的旋转矩阵，如下图所示：

图 旋转矩阵

因为偏航角的正方向与定义的方向相反，故矩阵有所变化，指导过程如下，以下为手写计算图：

图推导过程

1. GPS坐标WGS84到东北天坐标系ENU
2. 常用导航坐标系 及 转换关系 （理论+程序）
3. 初等矩阵的左乘右乘与行列变换的关系
4. 导航中坐标系及坐标转换
5. 左乘右乘

常用坐标系介绍

地理坐标系 (Geographic Coordinate System, GCS)

可以说是最为广泛应用的一个地球坐标系，它给出一点的大地纬度、大地经度和大地高程而更加直观地告诉我们该点在地球中的位置，故又被称作纬经高坐标系。WGS-84坐标系的X轴指向BIH(国际时间服务机构)1984.0定义的零子午面(Greenwich)和协议地球极(CTP)赤道的交点。Z轴指向CTP方向。Y轴与X、Z轴构成右手坐标系。

一句话解释就是：把前面提到的ECEF坐标系用在GPS中，就是WGS-84坐标系。

其中：
（1）：大地纬度是过用户点P的基准椭球面法线与赤道面的夹角。纬度值在-90°到+90°之间。北半球为正，南半球为负。
（2）：大地经度是过用户点P的子午面与本初子午线之间的夹角。经度值在-180°到+180°之间。
（3）：大地高度h是过用户点P到基准椭球面的法线距离，基准椭球面以内为负，以外为正。

参考资料:地理信息系统中常用坐标系

地心地固坐标系 (ECEF)

在GPS中使用的被称为地球中心，地球固定(ECEF)。ECEF使用三维XYZ坐标(以米为单位)来描述GPS用户或卫星。“地球”这个词来自于坐标轴的原点(0,0,0)位于质心处(通过多年的跟踪卫星确定轨迹)。“地球固定”这个词意味着坐标轴相对于地球是固定的(也就是说，它们随地球旋转)。其 x 轴延伸通过本初子午线（经度 0 度）和赤道（0度纬度）。z 轴延伸穿过真正的北极（即与地球自转轴重合）。y 轴完成右手坐标系，穿过赤道和 90 度经度，如图 1所示

图1 ECEF直角坐标系

当地东、北、上 (ENU) 坐标

站心坐标系也叫东北天坐标系 ENU，用于表示以观察者为中心的其他物体
的运动规律。以站心为坐标系原点，z 轴与椭球法线重合，向上为正，y 与椭球
短半轴重合（北向），x 轴与椭球长半轴重合（东向）

基坐标相互转化

地理坐标系到地心地固坐标系 (GCS to ECEF)

这里因为地球近似为一个椭球,所以沿着椭球表面的法线方向不在$O_e$与$M$的连线方向,首先我们假设它的椭球表面的法线方向的反向延长线交$Z$轴与于点$M{}^{\prime }$,设$|PQ|=N,|PM|=h$,$在X_{ECEF}{-}_{ECEF}{-}_{ECEF}$基坐标系下,根据几何关系,M的坐标为
$\left((N+h)\cos \left(\phi \right)\cos (\lambda ),(N+h)\cos \left(\phi \right)\sin (\lambda ),(N+h)\sin \left(\phi \right)\right)-OQ)$

然后我们将$M$点所在的子午圈椭圆投影到一个二维的平面上,如下图所示,(gps的高度h不是与地心在一条直线上)

首先设p点所在的子午圈的方程设为
$$\frac{x^2}{R_e^2}+\frac{z^2}{R_p^2}=1\text{\hspace{1em}①}$$ 椭圆的扁率定义为
$$f=\frac{R_e-R_p}{R_e}$$ 椭圆的离心率定义为
$$e=\frac{\sqrt{R_e^2-R_p^2}}{R_e}\text{\hspace{1em}②}$$由②式可得$$R_p=Re\sqrt{1-e^2}\text{\hspace{1em}③}$$
将椭圆方程①两边同时对$x$求导,并结合③式,可得$$\frac{2x}{R_e^2}+\frac{2z\mathrm{d}z/\mathrm{d}x}{R_e^2\left(1-e^2\right)}=0$$移项整理得$$\frac{\mathrm{d}z}{\mathrm{d}x}=-\left(1-e^2\right)\frac{x}{z}$$式中$\frac{\mathrm{d}z}{\mathrm{d}x}$表示椭圆在P点的切线的斜率,显然切线PE$\perp$法线PQ,所以$K_{PE}{K}_{PQ}=-1$,因为$PQ$的斜率为$\tan \phi$,则
$\frac{\mathrm{d}z}{\mathrm{d}x}\tan \phi =-\left(1-e^2\right)\frac{x}{z}\tan \phi =-1$
整理可得$$z=x\left(1-e^2\right)\tan \phi \text{\hspace{1em}④}$$
结合③④带入椭圆方程①中,可得到以地理纬度$\phi$的椭圆的参数方程$$\begin{array}{l}x=\frac{R_e}{\sqrt{1-e^2{\sin }^2\phi }}\cos \phi \\ z=\frac{R_e\left(1-e^2\right)}{\sqrt{1-e^2{\sin }^2\phi }}\sin \phi \end{array}\}\text{\hspace{1em}⑤}$$
根据子午圈投影的图可得$$x=Ncos\phi \text{\hspace{1em}⑥}$$ 将⑤中的$x$带入⑥式中$$N=\frac{R_e}{\sqrt{1-e^2{\sin }^2\phi }}$$
因此参数方程可以简写为$$\begin{array}{l}x=N\cos \phi \\ z=N\left(1-e^2\right)\sin \phi \end{array}\}\text{\hspace{1em}⑥}$$
通过上面的椭圆参数方程⑥,结合投影到$2D平面上的Z轴与ECEF的Z轴$是同一个可得$M$点的坐标系$z_M=N\left(1-e^2\right)\sin \phi +hsin\phi =($,结合之前所说的{ECEF坐标系下$P$的坐标系为$$\left((N+h)\cos \left(\phi \right)\cos (\lambda ),(N+h)\cos \left(\phi \right)\sin (\lambda ),(N+h)\sin \left(\phi \right)\right)-OQ)$$
可以得出
$x_M=(N+h)cos(\phi )cos(\lambda )$
$y_M=(N+h)cos(\phi )sin(\lambda )$
$z_M=\left(\frac{b^2}{a^2}N+h\right)\sin \phi$

附上各参数的含义
$\phi =latitude$
$\lambda =longitude$
$h=heightaboveellipsoid(meters)$
$N=RadiusofCurvature(meters),definedas:\frac{a}{\sqrt{1-e^2{\sin }^2\phi }}$,这里的a就是我们上面推导公式的$R_e$,详细的参考$⑥$下面的推导.

WGS84 Parameters
$a=6378137$(椭圆的长半轴)这里表示赤道椭圆的长半轴
$b=a(1-f)=6356752.31424518$
$f=\frac{1}{298.257223563}$
$e=\frac{\sqrt{a^2-b^2}}{a^2}$详细含义参照②

我们把$M$点放大,来说说gps的高度

高度$h,$gps ,$的$altitude,(从上面图中可知gps的测量高度是GPS天线与参考椭球面的垂直距离,WGS84椭球面在世界范围内近似大地水准面),地球的长半轴$a$与短半轴$b$是已知的,然后地理纬度$\phi$也是已知的,所以M点在$ECEF$坐标系下已知.

地心地固坐标系到东北天、站心坐标系 ECEF to ENU

这里采用几何意义的方式来讲这个转化关系,这里为了直观,我们在p点画ENU坐标系
因为$East(xLocal)$的方向与M点所在的子午线(P点子午线)垂直,所以我们首先把绕着$Z_{ECEF}$旋转至$X_{ECEF}$与$East(xLocal)$的方向一致①$ECEF坐标系$绕着$Z_{ECEF}$逆时针旋转$(\frac{\pi }{2}+\lambda )$,然后绕着$X_{ECEF}$旋转至$Z_{ECEF}$与$Up(ZLocal)$方向一致②绕着$X_{ECEF}$逆时针旋转$(\frac{\pi }{2}-\phi )$③平移的部分就是之前LLA转到ECEF坐标系下的$M$的坐标
$R_{z_{ecef}}[(\frac{\pi }{2}+\lambda )]=\left(\begin{array}{ccc}\cos [(\frac{\pi }{2}+\lambda )]& \sin [(\frac{\pi }{2}+\lambda )]& 0\\ -\sin [(\frac{\pi }{2}+\lambda )]& \cos [(\frac{\pi }{2}+\lambda )]& 0\\ 0& 0& 1\end{array}\right)$

$R_{x_{ecef}}[(\frac{\pi }{2}-\phi )]=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos [(\frac{\pi }{2}-\phi )]& \sin [(\frac{\pi }{2}-\phi )]\\ 0& -\sin [(\frac{\pi }{2}-\phi )]& \cos [(\frac{\pi }{2}-\phi )]\end{array}\right]$

$R_{x_{ecef}}[(\frac{\pi }{2}-\phi )]R_{z_{ecef}}[(\frac{\pi }{2}+\lambda )]=\left(\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\cos \lambda \sin \phi & -\sin \lambda \sin \phi & \cos \phi \\ \cos \lambda \cos \phi & \sin \lambda \cos \phi & \sin \phi \end{array}\right)$

在车辆运动中,我们一般车辆启动假设gps天线的初始位置在 { X r , Y r , Z r } \left\{X_{r}, Y_{r}, Z_{r}\right\} {Xr​,Yr​,Zr​},车辆运动的位置为 { X p , Y p , Z p } \left\{X_{p}, Y_{p}, Z_{p}\right\} {Xp​,Yp​,Zp​},则基于启动为原点的ENU的坐标为:
$\left[\begin{array}{l}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}-\sin \lambda & \cos \lambda & 0\\ -\cos \lambda \sin \phi & -\sin \lambda \sin \phi & \cos \phi \\ \cos \lambda \cos \phi & \sin \lambda \cos \phi & \sin \phi \end{array}\right]\left[\begin{array}{c}{X}_p-X_r\\ Y_p-Y_r\\ Z_p-Z_r\end{array}\right]$
这里讲的也很好

地理坐标系到东北天、站心坐标系(GCS to ENU)

GCS和ENU之间的坐标系转换通过先转换为ECEF实现。
代码有时间再放上来吧

这是一篇经纬高（LLA）坐标转东北天坐标（ENU）的详细推导，并给出近似转换的过程和结果

参考资料：
https://blog.csdn.net/qq_34213260/article/details/109133847

ECEF坐标系和ENU坐标系之间的关系如上图所示，各坐标系的定义在此不再赘述。

LLA坐标：Lat、Lon、Alt坐标，表示在ECEF坐标系下，其中Lat和Lon用角度（${}^{。}$）或弧度（rad）表示，Alt用米（m）表示；
ENU坐标：东北天坐标系下的坐标，该坐标系的原点需要指定，原点向东为x轴，原点向北为y轴，原点指向天为z轴，构成右手直角坐标系，该系下的坐标表示都是米制单位；

LLA坐标转换成ENU坐标的过程主要分为两步：LLA->ECEF->ENU

其中主要参数的定义如下：

$$b=6356752.3142$$
$$e^2=f(2-f)$$
$$N=\frac{a}{\sqrt{1-e^{2}si{n}^2(lat)}}$$
其中b为椭球短半径，e为椭球偏心率

（1）LLA->ECEF:（无需给定起点，一对一转换）
从最上面一幅图中可以轻易得到同一点在这两个坐标系之间的转换关系，如下：
$$X_{ecef}=(N+alt)cos(lat)cos(lon)$$
$$Y_{ecef}=(N+alt)cos(lat)sin(lon)$$
$$Z_{ecef}=(N(1-e^2)+alt)sin(lat)$$
（2）ECEF->ENU:（需要给定起点）
假设ENU坐标系的坐标原点对应的ECEF坐标和LLA坐标分别为：
$$O_{ecef}=(X_0,Y_0,Z_0)$$
$$O_{lla}=(la{t}_0,lo{n}_0,al{t}_0)$$
假设空间中有另一点P，且：
$$P_{ecef}=(X,Y,Z)$$
则P在ENU系下的坐标为：
$$P_{enu}=R_{ECEF}^{ENU}(P_{ecef}-O_{ecef})\text{\hspace{1em}(1)}$$
其中只有$R_{ECEF}^{ENU}$是未知的，下面对其进行推导：

$R_{ECEF}^{ENU}$可以分解为如下三个旋转过程：ECEF–>绕$Z_{ecef}$轴逆时针旋转$\lambda$（经度）–>绕$Y_{ecef}^{{}^{\prime }}$轴逆时针旋转（$90-\varphi$）（$\varphi :$纬度）–>绕$Z_{ecef}^{{}^{\prime \prime }}$逆时针旋转90–>ENU

上述问题是典型的空间中点不动，坐标系转动的问题，按顺序记三次旋转分别为$R_1、R_2、R_3$，则：
$$R_1=\left[\begin{array}{ccc}cos(\lambda )& sin(\lambda )& 0\\ -sin(\lambda )& cos(\lambda )& 0\\ 0& 0& 1\end{array}\right]$$
$$R_2=\left[\begin{array}{ccc}sin(\varphi )& 0& -cos(\varphi )\\ 0& 1& 0\\ cos(\varphi )& 0& sin(\varphi )\end{array}\right]$$
$$R_3=\left[\begin{array}{ccc}0& 1& 0\\ -1& 0& 0\\ 0& 0& 1\end{array}\right]$$
则：
$$R_{ECEF}^{ENU}=R_3\ast R_2\ast R_1=\left[\begin{array}{ccc}-sin(\lambda )& cos(\lambda )& 0\\ -sin(\varphi )cos(\lambda )& -sin(\varphi )sin(\lambda )& cos(\varphi )\\ cos(\varphi )cos(\lambda )& cos(\varphi )sin(\lambda )& sin(\varphi )\end{array}\right]$$
将上式带入到公式（1）中可以得到：
$$P_{enu}=R_{ECEF}^{ENU}(P_{ecef}-O_{ecef})=\left[\begin{array}{ccc}-sin(lo{n}_0)& cos(lo{n}_0)& 0\\ -sin(la{t}_0)cos(lo{n}_0)& -sin(la{t}_0)sin(lo{n}_0)& cos(la{t}_0)\\ cos(la{t}_0)cos(lo{n}_0)& cos(la{t}_0)sin(lo{n}_0)& sin(la{t}_0)\end{array}\right]\ast \left[\begin{array}{c}X-X_0\\ Y-Y_0\\ Z-Z_0\end{array}\right]\text{\hspace{1em}(2)}$$
至此完成了LLA坐标到ENU坐标的转换，且无近似，下面对公式（2）进行近似推导

由于
$$X=(N+alt)cos(lat)cos(lon)$$
$$Y=(N+alt)cos(lat)sin(lon)$$
$$Z=(N(1-e^2)+alt)sin(lat)$$
$$X_0=(N_0+al{t}_0)cos(la{t}_0)cos(lo{n}_0)$$
$$Y_0=(N_0+al{t}_0)cos(la{t}_0)sin(lo{n}_0)$$
$$Z_0=(N_0(1-e^2)+al{t}_0)sin(la{t}_0)$$
所以
$$\begin{array}{rl}X-X_0& =(N+alt)cos(lat)cos(lon)-(N_0+al{t}_0)cos(la{t}_0)cos(lo{n}_0)\\ & =(N+alt)cos(lat)cos(lon)-(N+al{t}_0)cos(la{t}_0)cos(lo{n}_0)\\ & =(N+al{t}_0+\delta alt)cos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)-(N+al{t}_0)cos(la{t}_0)cos(lo{n}_0)\\ & =(N+al{t}_0)cos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)-(N+al{t}_0)cos(la{t}_0)cos(lo{n}_0)+\delta altcos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)\\ & =(N+al{t}_0)\left[cos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)-cos(la{t}_0)cos(lo{n}_0)\right]+\delta altcos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)\\ & =(N+al{t}_0)\left[(cos(la{t}_0)cos(\delta lat)-sin(la{t}_0)sin(\delta lat))(cos(lo{n}_0)cos(\delta lon)-sin(lo{n}_0)sin(\delta lon))-cos(la{t}_0)cos(lo{n}_0)\right]+\delta altcos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)\\ & =(N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)cos(\delta lat)sin(\delta lon)-sin(la{t}_0)cos(lo{n}_0)sin(\delta lat)cos(\delta lon))+\delta altcos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)\\ & =(N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-sin(la{t}_0)cos(lo{n}_0)\ast \delta lat)+\delta altcos(la{t}_0+\delta lat)cos(lo{n}_0+\delta lon)\\ & =(N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-sin(la{t}_0)cos(lo{n}_0)\ast \delta lat)+\delta alt\left[(cos(la{t}_0)-sin(la{t}_0)\ast \delta lat)(cos(lo{n}_0)-sin(lo{n}_0)\ast \delta lon)\right]\\ & =(N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-sin(la{t}_0)cos(lo{n}_0)\ast \delta lat)+\delta alt\left[cos(la{t}_0)cos(lo{n}_0)-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-cos(lo{n}_0)sin(la{t}_0)\ast \delta lat\right]\\ & =(N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-sin(la{t}_0)cos(lo{n}_0)\ast \delta lat)+\delta altcos(la{t}_0)cos(lo{n}_0)\end{array}$$
同理，有
$$Y-Y_0=(N+al{t}_0)(\delta loncos(la{t}_0)cos(lo{n}_0)-\delta latsin(la{t}_0)sin(lo{n}_0))+\delta altcos(la{t}_0)sin(lo{n}_0)$$
将以上两式带入到（2）中可以得到点P在ENU系下的东向坐标：
$$\begin{array}{rl}e& =-sin(lo{n}_0)\ast (X-X_0)+cos(lo{n}_0)\ast (Y-Y_0)\\ & =-sin(lo{n}_0)((N+al{t}_0)(-cos(la{t}_0)sin(lo{n}_0)\ast \delta lon-sin(la{t}_0)cos(lo{n}_0)\ast \delta lat)+\delta altcos(la{t}_0)cos(lo{n}_0))+cos(lo{n}_0)((N+al{t}_0)(\delta loncos(la{t}_0)cos(lo{n}_0)-\delta latsin(la{t}_0)sin(lo{n}_0))+\delta altcos(la{t}_0)sin(lo{n}_0))\\ & =(N+al{t}_0)(sin(lo{n}_0)cos(la{t}_0)sin(lo{n}_0)\ast \delta lon)+(N+al{t}_0)(cos(la{t}_0)co{s}^2(lo{n}_0)\ast \delta lon)\\ & =(N+al{t}_0)\ast \delta loncos(la{t}_0)(si{n}^2(lo{n}_0)+co{s}^2(lo{n}_0))\\ & =(N+al{t}_0)\ast \delta loncos(la{t}_0)\\ & \approx a\ast \delta loncos(la{t}_0)\end{array}$$
类似的，可以得到n、u坐标，最终得到如下的近似结果：
$$P_{enu}=\left[\begin{array}{c}e\\ n\\ u\end{array}\right]=\left[\begin{array}{c}a\ast \delta loncos(la{t}_0)\\ a\ast \delta lat\\ \delta alt\end{array}\right]$$