坐标系、欧拉角、旋转矩阵、四元数

坐标系

1、地心惯性坐标系（$i$系，$inertialframe$）
地心惯性坐标系是太阳系内的一个惯性坐标系，不随地球而转动。地心惯性坐标系用$o_i{x}_i{y}_i{z}_i$表示，原点为地球中心，$o_i{x}_i$和$o_i{y}_i$轴在地球赤道平面内，其中$o_i{x}_i$轴指向春分点(赤道面与黄道面的交线再与天球相交的交点之一，赤经为0，赤纬为0)，春分点是天文测量中确定恒星时的起始点，$o_i{z}_i$轴为地球自转轴，并指向北极。惯性传感器的输出就是以该坐标系为参考基准的。

2、地球坐标系（$e$系，$earthframe$）
地球坐标系用$o_e{x}_e{y}_e{z}_e$表示，原点为地球中心，$o_e{x}_e$和$o_e{y}_e$轴在地球赤道平面内，其中指向本初子午线，轴为地球自转轴，并指向北极。系与地球固连，也称为地心地固坐标系（$Earth-CenteredEarth-Fixed,ECEF$），地球坐标系相对于惯性坐标系的角运动大小就是地球自转角速率，其值通常取
$${\omega }_{ie}=7.2921151467rad/s=15.0410671786°/h$$

3、地理坐标系（$g$系，$geographicframe$）
地理坐标系用表示$o_g{x}_g{y}_g{z}_g$，原点定义为运载体的重心或中心，$o_g{x}_g$轴指向地理东向，$o_g{y}_g$轴指向地理北向，$o_g{z}_g$轴垂直于当地旋转椭球面指向天向。地理坐标系相对于地球坐标系的关系可由运载体的地理坐标表示，即经度、纬度和椭球高度。

4、导航坐标系($n$系，$navigationframe$)
导航坐标系用表示$o_n{x}_n{y}_n{z}_n$，它是惯导系统在求解导航参数时所采用的参考坐标系。纯惯导系统的高度通道在原理上是发散的，因而惯导系统多采用当地水平坐标系作为参考坐标系，以实现水平和高度通道的解耦，在惯导系统长时间导航定位时只进行水平定位解算，而简单地将高度通道设置为固定值。地理坐标系就是一种当地水平坐标系，并且它的$o_g{y}_g$轴指向北向，随运载体在地球表面上的移动而移动。除地球极点外（实际应用中需排除极点附近），各地的地理坐标系是唯一的，书中（严恭敏的书《捷联惯导算法于组合导航原理》）选取“东–北–天”地理坐标系作为导航参考坐标系，能够适用于除极区外的全球导航应用，这种惯导系统常常称为指北方位惯导系统。

5、载体坐标系(或称体系，$b$系，$bodyframe$)
载体坐标系用$o_b{x}_b{y}_b{z}_b$表示，其原点定义为运载体的重心或中心，$o_b{x}_b$轴沿载体横轴向右，$o_b{y}_b$轴沿载体纵轴向前，$o_b{z}_b$轴沿载体立轴向上。系与载体固连，载体坐标系相对于导航坐标系的方位关系可用一组欧拉角来描述。
$$\begin{array}{rl}{\omega }_{nb}^n& ={\omega }_{ib}^b-{\omega }_{in}^b={\omega }_{ib}^b-C_b^n({\omega }_{in}^n\times )C_n^b\\ {\omega }_{in}^n& ={\omega }_{ie}^n-{\omega }_{en}^n\end{array}$$
${\omega }_{nb}^n$：机体坐标系（$b$系）相对于导航坐标系（$n$系）的角速度在导航坐标系（$n$系）得投影。
${\omega }_{ib}^b$：陀螺输出的是机体坐标系（$b$系）相对于惯性坐标系（$i$系）的角速度。
${\omega }_{in}^n$：导航坐标系（$n$系）相对于惯性系（$i$系）的角速度。
${\omega }_{ie}^n$：地球自转引起的导航坐标系（$n$系）旋转，地球坐标系（$e$系）相对于惯性坐标系（$i$系）的角速度${\omega }_{ie}$在导航坐标系（$n$系）下投影。
${\omega }_{en}^n$：惯导系统在地球表面附近移动因地球表面弯曲而引起的导航坐标系（$n$系）旋转。

$$\begin{array}{rl}{\omega }_{ie}^n& =[0{\omega }_{ie}cos(Latitude){\omega }_{ie}sin(Latitude)]\\ {\omega }_{en}^n& =[-\frac{v_N}{R_M+h}-\frac{v_E}{R_N+h}\frac{v_E}{R_N+h}tan(Latitude)]\end{array}$$

注：由于$i$系是绝对不动的惯性参考坐标系，它与时间无关，不需标注时刻；而$n$系和$b$系相对于$i$系都是动坐标系，均跟时间有关，需标注时刻。
例如：
$$C_{b(m)}^{n(m)}=C_i^{n(m)}{C}_{b(m)}^i$$
角标括号中的符号$m$表示$t_m$时刻。
上述描述，均摘自严龚敏老师的《捷联惯导算法与组合导航原理》。

导航坐标系-东北天坐标系（ENU）对应机体坐标系-右前上坐标系（RFU）

导航参考坐标系为“东-北-天”（ENU）地理坐标系，机体坐标系满足右手定则为“右-前-上”（RFU）坐标系。

旋转顺序和相对应的旋转角度

确定由n系到b系得旋转顺序为$Z—X—Y$，n系到b系分别对应欧拉角$yaw(Z)$，$pitch(X)$，$roll(Y)$，满足右手旋转时为正值。
$yaw$：绕$Z$轴旋转的角度，逆时针旋转并且满足右手定则为正值。注意：北偏西方向对应的角度值是正值。
$pitch$：绕$X$轴旋转的角度，逆时针旋转并且满足右手定则为正值。
$roll$：绕$Y$轴旋转的角度，逆时针旋转并且满足右手定则为正值。

欧拉角到旋转矩阵（Euler2DCM.m）

输入：欧拉角$eule{r}_{nb}$是n系到b系旋转的角度，旋转顺序为$Z—X—Y$，分别对应欧拉角$yaw(Z)$，$pitch(X)$，$roll(Y)$（满足右手旋转时为正值）。
输出：姿态旋转矩阵矩阵$DC{M}_{nb}$即为$C_n^b$，旋转顺序为$Z—X—Y$。

Z轴对应旋转角度$yaw$的旋转矩阵为（$yaw$：满右手旋转的为正值）
$$RotationMatri{x}_Z=\left[\begin{array}{ccc}cos(yaw)& sin(yaw)& 0\\ -sin(yaw)& cos(yaw)& 0\\ 0& 0& 1\end{array}\right]$$
X轴对应旋转角度$pitch$的旋转矩阵为（$pitch$：满右手旋转的为正值）
$$RotationMatri{x}_X=\left[\begin{array}{ccc}1& 0& 0\\ 0& cos(pitch)& sin(pitch)\\ 0& -sin(pitch)& cos(pitch)\end{array}\right]$$
Y轴对应旋转角度$roll$的旋转矩阵为（$roll$：满右手旋转的为正值）
$$RotationMatri{x}_Y=\left[\begin{array}{ccc}cos(pitch)& 0& -sin(pitch)\\ 0& 1& 0\\ sin(pitch)& 0& cos(pitch)\end{array}\right]$$
姿态旋转矩阵$C_n^b$（左乘）为
$$C_n^b=RotationMatri{x}_Y\ast RotationMatri{x}_X\ast RotationMatri{x}_Z$$
C n b = [ c o s ( y a w ) ∗ c o s ( r o l l ) − s i n ( h e a d i n g ) ∗ s i n ( p i t c h ) ∗ s i n ( r o l l ) s i n ( y a w ) ∗ c o s ( r o l l ) + c o s ( y a w ) ∗ s i n ( p i t c h ) ∗ s i n ( r o l l ) − c o s ( p i