PSINS工具箱学习（二）姿态阵、四元数、欧拉角、等效旋转矢量的概念和转换

载体的姿态指的是载体坐标系和导航坐标系之间的方位关系，而两坐标系直接的方位关系可以等效成力学中的 刚体定点转动问题。在刚体定点转动理论中，描述动坐标系相对参考坐标系方位关系的传统方法有 欧拉角法、 方向余弦法、 四元数法。传统的方向余弦法和四元数法都是假设机体做定轴运动，在此种情况下没有 不可交换性误差。然而在捷联惯导系统中，惯性器件是直接安装在机体上，跟随机体的运动，所以假定机体运动为定轴不太符合实际环境，因此针对高动态境，还有更高精度的姿态解算方法： 等效旋转矢量法。

由于捷联惯导系统采用通过姿态矩阵计算建立的数学平台，在捷联惯导系统的姿态、速度和位置的更新算法中，姿态算法对整个系统的精度影响最大，它是算法研究和设计的核心。

姿态求解目标：知道初始姿态参数，实时根据陀螺仪的测量，输出姿态参数：

一、基础概念

1、坐标系定义

根据原点的位置和轴线得到指向，可以将惯导坐标系分为四类：

1. 惯性坐标系（ i 系 ）

也称 ECI，原点在地球质心，坐标轴不随地球自转（Z轴指向北极，X轴指向春分点）。惯性传感器输出以此为基准。尽管受到太阳引力场的作用，存在 $6\ast 10^{-7}g$ 的公转向心力；但在地球表面以地球做惯导参考基准时，运动载体与地球处于近似相同的太阳引力场中，可以不考虑其影响。

2. 地心地固坐标系（ e 系 )

也称 ECEF，原点在地图质心，坐标轴随地球自转（Z轴指北极，X轴指向赤道和本初子午线交点）。GNSS 系统最常用此坐标系。ECEF 相对于 ECI 的角运动大小就是地球自转角速率 ${\omega }_{ie}$。

3. 导航坐标系（ n 系）

也称当地水平坐标系，原点在载体，坐标轴不随载体变化。有两种描述：北东地（NED）、东北天（ENU）。是惯导求解参数时所使用的坐标系，惯导解算对重力加速度特别在意，可以由此坐标系显现出来。

4. 载体坐标系（ b 系 ）

原点在载体，坐标轴随载体变化。与导航坐标系相对于，也有两种描述：前右下（FRD）、右前上（RFU）。一套导航系统中有很多个 b 系：IMU的 b 系、载体的 b 系、相机的 b 系等等。b 系和 i 系的方位关系可以用一组欧拉角来描述。

在 PSINS 中：导航坐标系(n)：东E-北N-天U、载体坐标系(b)：右R-前F-上U

2、反对称阵

$$(\mathbf{V}\times )=\left[\begin{array}{ccc}0& -V_z& V_y\\ V_z& 0& -V_x\\ -V_y& V_x& 0\end{array}\right]$$

反对称阵要点：

1. 对角线上都为 0，上下两部分对称相反，即： $(V\times )=-(V\times {)}^T$ ，所以称之为反对称阵。

2. 一个向量叉乘另一个向量，等于点乘另一个向量的反对称阵，化叉乘为点乘。

3. 反对称阵的幂方通式：
   $$(\mathbf{V}\times {)}^i=\{\begin{array}{ll}(-1{)}^{(i-1)/2}{v}^{i-1}(\mathbf{V}\times )& i=1,3,5,\cdots \\ (-1{)}^{(i-2)/2}{v}^{i-2}(\mathbf{V}\times {)}^2& i=2,4,6,\cdots \end{array}$$

3、欧拉角

$$\mathbf{A}={\left[\begin{array}{lll}\theta & \gamma & \psi \end{array}\right]}^{\mathrm{T}}$$

欧拉角是一种用绕坐标轴转换的角度描述两个坐标系之间相对姿态的经典方法，物理含义直观，易于理解，尤其是用于描述载体坐标系相对于当地导航坐标系的运动。

欧拉角表示转动：三维空间中，一个直角坐标系相对于另一个直角坐标系的方位关系，可通过三个依次转动来描述，每次旋转所绕的坐标轴与前后旋转轴正交。欧拉证明任意两个正交坐标系之间的相对朝向关系可以通过一组不少于 3 的角度来描述，这三次旋转的转角称为一组欧拉角（欧拉角组）。

欧拉角表示姿态：想描述载体的姿态，可以指定一个参考坐标系，然后用一组欧拉角来描述与载体与固联的坐标系相对于参考坐标系的转动。在地球附近的导航应用中，参考坐标系一般默认选择当地地理坐标系， 而动坐标系为与运载体固联的坐标系。

• 航向角：载体纵轴正方向在当地水平面上的投影与当地地理北向的夹角，常取北偏东为正，一般用 ψ 表示，角度范围为 0 ∼ 360◦ 或 −180◦ ∼ +180◦。
• 俯仰角：载体纵轴正方向与其水平投影线之间的夹角，当载体“抬头”时定义为正，一般用 θ 表示，取值范围为 −90◦ ∼ 90◦。
• 横滚角：载体立轴正方向与载体纵轴所在铅垂面之间的夹角，当载体向右倾斜（如飞机右机翼下压）时为正，用 $\varphi$ 表示，取值范围为 −180◦ ∼ +180◦。

欧拉角要点：

1. 两个正交坐标系之间的相对朝向关系可以通过一组欧拉角组来描述, 给定一组欧拉角须同时指定对应的转轴顺序。PSINS中用的是东北天、312 欧拉角
2. 导航姿态角, 包括航向角、俯仰角和横滚角, 与载体的物理轴向紧密关联, 而与选择的载体数学坐标系无关。
3. 当俯仰角为 $\pm 90^{\circ }$ 时, 欧拉角变换存在奇异值, 此时无法区分横滚角和航向角。因此，欧拉角法不能用于全姿态导航应用。
4. 欧拉角组不能直接相加来表示两次转动的合成。

4、旋转矩阵/方向余弦阵

$$\mathbf{C}=\left[\begin{array}{lll}{c}_{xx}& c_{xy}& c_{xz}\\ c_{yx}& c_{yy}& c_{yz}\\ c_{zx}& c_{zy}& c_{zz}\end{array}\right]$$

方向余弦矩阵（Direction Cosine Matrix, DCM）用向量的方向余弦来表示的姿态矩阵，常用于描述两个坐标系的相对姿态。相比于欧拉角，方向余弦矩阵显得抽象许多，但它处理向量的投影变换非常方便。

我们知道，空间中的矢量的三个投影值就是坐标，同一个矢量在 $i$ 系和 $b$ 系有两套坐标，它们的关系可以表示为：
$$\mathbf{V}=V_x^i{\mathbf{i}}_i+V_y^i{\mathbf{j}}_i+V_z^i{\mathbf{k}}_i=V_x^b{\mathbf{i}}_b+V_y^b{\mathbf{j}}_b+V_z^b{\mathbf{k}}_b$$
即 $\left[\begin{array}{lll}{\mathbf{i}}_i& {\mathbf{j}}_i& {\mathbf{k}}_i\end{array}\right]\left[\begin{array}{c}{V}_x^i\\ V_y^i\\ V_z^i\end{array}\right]=\left[\begin{array}{lll}{\mathbf{i}}_b& {\mathbf{j}}_b& {\mathbf{k}}_b\end{array}\right]\left[\begin{array}{c}{V}_x^b\\ V_y^b\\ V_z^b\end{array}\right]=\left[\begin{array}{lll}{\mathbf{i}}_i& {\mathbf{j}}_i& {\mathbf{k}}_i\end{array}\right]\mathbf{P}\left[\begin{array}{c}{V}_x^b\\ V_y^b\\ V_z^b\end{array}\right]$

有：
$$\left[\begin{array}{c}{V}_x^i\\ V_y^i\\ V_z^i\end{array}\right]=\mathbf{P}\left[\begin{array}{c}{V}_x^b\\ V_y^b\\ V_z^b\end{array}\right]$$
记为 ${\mathbf{V}}^i=\mathbf{P}{\mathbf{V}}^b={\mathbf{C}}_b^i{\mathbf{V}}^b$，$C_b^i$ 称为坐标系变换矩阵 (i->b) 或 坐标变换矩阵 (b->i)，也称之为方向余弦阵、过渡矩阵。

方向余弦阵要点：

1. 由于都是单位阵，方向余弦阵中每一个元素都表示两套坐标系坐标轴夹角的余弦值，以此得名。
2. 区分清坐标系变换矩阵和坐标变换矩阵。
3. 方向余弦阵虽然有 9 个参数，但其中包含了 6 个约束条件，即行向量模为 1 且两两正交，只有 3 个参数是独立的。
4. 方向余弦矩阵是正交矩阵, ${\left({\mathbf{C}}_{\alpha }^{\beta }\right)}^{-1}={\left({\mathbf{C}}_{\alpha }^{\beta }\right)}^{\mathrm{T}}$ 。多次连续转动可以用相应的多个方向余弦矩阵连乘来表示: ${\mathbf{C}}_a^d={\mathbf{C}}_b^d{\mathbf{C}}_a^b$ 。
5. 当载体运动时，两个坐标系之间的相对姿态随时间变化，对应的方向余弦矩阵 ${\mathbf{C}}_b^R(t)$ 则是 一个时变矩阵, 其微分方程为 ${\dot{\mathrm{C}}}_b^R={\mathbf{C}}_b^R\left({\mathbf{\omega }}_{Rb}^b\times \right)$ 。
6. 可用毕卡迭代法求解方向余弦矩阵的微分方程，得到一个以无限重积分表示的级数，也被称为毕卡级数，上述级数是收玫的，但只有在 “定轴转动” 这一特殊情形下才能得到如下形式的闭合解：${\mathbf{C}}_b^R(t)=\mathbf{C}(0)\exp \left({\int }_0^t\mathbf{\Omega }(\tau )d\tau \right)$ 。
7. 姿态更新公式：${\mathbf{C}}_{b(k)}^i={\mathbf{C}}_{b(k-1)}^i{\mathbf{C}}_{b(k)}^{b(k-1)}$，${\mathbf{C}}_{b(k)}^{b(k-1)}=\mathbf{I}+\frac{\sin \Delta {\theta }_k}{\Delta {\theta }_k}\left[\Delta {\mathbf{\theta }}_k\times \right]+\frac{1-\cos \Delta {\theta }_k}{\Delta {\theta }_k^2}{\left[\Delta {\mathbf{\theta }}_k\times \right]}^2$
8. 对于普通惯性导航应用来说, $b$ 系定轴转动这一假设一般都不成立的, 当该条件不满足时, 仍然将陀螺输出的角增量套用到式中进行姿态更新的递推计算, 则会带来姿态更新求解的不可交换性误差，可用等效旋转矢量来解决这个问题。

5、四元数

$$\mathbf{Q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}=q_0+{\mathbf{q}}_v$$

我们知道复数可以看成是复平面上的向量，即一个二维的向量，将复数的概念再拓展可以得到四元数，用以描述三维空间中的向量。

四元数要点：

1. 四元数主要有以下表示方式: $\mathbf{q}=q_0+q_1\mathbf{i}+q_2\mathbf{j}+q_3\mathbf{k}$ 、向量坐标形式 $\mathbf{q}={\left[q_0,q_1,q_2,q_3\right]}^{\mathrm{T}}$ 、 $\mathbf{q}=q_s+{\mathbf{q}}_v$ 或三角函数形式 $\mathbf{q}=\cos \frac{\theta }{2}+\mathbf{u}\sin \frac{\theta }{2}$ (与等效旋转矢量有关，做了归一化，模值为一)

2. 四元数虚数单位之间的乘法有如下特点: 单位与单位自己相乘时, 表现与复数一样, 而两两之间的乘法则与三维向量的外积 (叉乘) 一样。
   $$\{\begin{array}{l}\mathbf{i}\circ \mathbf{i}=\mathbf{j}\circ \mathbf{j}=\mathbf{k}\circ \mathbf{k}=-1\\ \mathbf{i}\circ \mathbf{j}=\mathbf{k},\mathbf{j}\circ \mathbf{k}=\mathbf{i},\mathbf{k}\circ \mathbf{i}=\mathbf{j},\mathbf{j}\circ \mathbf{i}=-k,k\circ \mathbf{j}=-i,\mathbf{i}\circ \mathbf{k}=-\mathbf{j}\end{array}$$

3. 用四元数的旋转变换算子可实现三维向量的坐标变换: ${\mathbf{v}}^R={\mathbf{q}}_b^R\circ {\mathbf{v}}^b\circ {\mathbf{q}}_b^{R^{\ast }}$ 。

4. 单位四元数三角表示法：$\mathbf{Q}=q_0+{\mathbf{q}}_v=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}$ 具有很清晰的物理意义。常在四元数的右边加上角标，写成 ${\mathbf{Q}}_b^i$，中 $\mathbf{u}$ 表示动坐标系 ( $b$ 系) 相对于参考坐标系 ( $i$ 系) 旋转的单位转轴，$\varphi$ 表示旋转角度大小， $\varphi \mathbf{u}$ 表示等效旋转矢量。使用角标后，共轭四元数可记为 ${\mathbf{Q}}_i^b={\left({\mathbf{Q}}_b^i\right)}^{\ast }$，这与矩阵转置的表示方法类似，比如 ${\mathbf{C}}_i^b={\left({\mathbf{C}}_b^i\right)}^{\mathrm{T}}$ 。

5. 四元数的微分方程式: ${\dot{\mathbf{q}}}_b^R=\frac{1}{2}{\mathbf{q}}_b^R\circ \left[\begin{array}{c}0\\ {\mathbf{\omega }}_{Rb}^b\end{array}\right]$, 或写作矩阵形式: ${\dot{\mathbf{q}}}_b^R=\frac{1}{2}\mathbf{W}{\mathbf{q}}_b^R$ 。由此建立了四元数与旋转角速度之间的关系。

6. 求解四元数的微分方程, 当 $b$ 系满足 “定轴转动” 条件时, 可得解析解: ${\mathbf{q}}_b^R(t)=$ $\left[\mathbf{I}\cos \frac{\Delta \theta }{2}+\frac{\Theta }{\Delta \theta }\sin \frac{\Delta \theta }{2}\right]{\mathbf{q}}_b^R(0)$ 。

7. 还有八元数，用于描述螺旋运动，有线运动、角运动。

6、等效旋转矢量

欧拉在研究刚体运动时最早提出了等效旋转矢量的概念，他指出刚体的定点有限旋转都可以用绕经过该固定点的一个轴的一次转动来等效实现。并建立了刚体上单位矢量在转动前后的变换公式。现代捷联惯性导航姿态更新算法研究的关键在于如何使用陀螺的测量值构造出对应的等效旋转矢量，以尽量减小或消除姿态更新算法中的不可交换性误差。后续使用等效旋转矢量来计算姿态的变化，如用方向余弦矩阵或四元数来表示，将变得非常简单。

用几个角增量的采样值，根据各种子样算法，算一个等效旋转矢量，再用算出的等效旋转矢量更新方向余弦阵、四元数。

空间矢量 $r$ 绕转轴矢量 $u$ 转动 $\varphi$ ，转动得到的矢量 $r^{{}^{\prime }}$ 。四个量直接一定有函数关系 $r^{{}^{\prime }}=f(r,u,\varphi )$ ，利用一些几何关系，可以推导得到：
$$\begin{array}{rl}{\mathbf{r}}^{\prime }& =(\mathbf{u}\cdot \mathbf{r})\mathbf{u}+(\mathbf{u}\times \mathbf{r})\times \mathbf{u}\cos \varphi +\mathbf{u}\times \mathbf{r}\sin \varphi \\ & =\left[\mathbf{I}+(\mathbf{u}\times {)}^2\right]\mathbf{r}-(\mathbf{u}\times {)}^2\mathbf{r}\cos \varphi +\mathbf{u}\times \mathbf{r}\sin \varphi \\ & =\left[\mathbf{I}+\sin \varphi (\mathbf{u}\times )+(1-\cos \varphi )(\mathbf{u}\times {)}^2\right]\mathbf{r}=\mathbf{D}\mathbf{r}\end{array}$$
可记 $\mathbf{D}=\mathbf{I}+\sin \varphi (\mathbf{u}\times )+(1-\cos \varphi )(\mathbf{u}\times {)}^2$ ，此公式称为罗德里格旋转公式；$r^{\prime }$ 转到 $r$ 中间乘的矩阵称为罗德里格旋转矩阵，和转轴$u$ 及转角 $\varphi$ 有关。

将罗德里格旋转公式用于直角坐标系的单位坐标轴，可将参考坐标系转到动坐标系：
$$\begin{array}{c}{\mathbf{i}}_b=\mathbf{D}{\mathbf{i}}_i\\ {\mathbf{j}}_b=\mathbf{D}{\mathbf{j}}_i\\ {\mathbf{k}}_b=\mathbf{D}{\mathbf{k}}_i\end{array}\}$$

$$\{\begin{array}{c}基旋转变换关系：\left[\begin{array}{lll}{\mathbf{i}}_b& {\mathbf{j}}_b& {\mathbf{k}}_b\end{array}\right]=\mathbf{D}\left[\begin{array}{lll}{\mathbf{i}}_i& {\mathbf{j}}_i& {\mathbf{k}}_i\end{array}\right]\\ 基投影变换关系：\left[\begin{array}{lll}{\mathbf{i}}_b& {\mathbf{j}}_b& {\mathbf{k}}_b\end{array}\right]=\left[\begin{array}{lll}{\mathbf{i}}_i& {\mathbf{j}}_i& {\mathbf{k}}_i\end{array}\right]\mathbf{P}\end{array}$$

$$在i系投影：\begin{array}{l}\left[\begin{array}{lll}{\mathbf{i}}_b^i& {\mathbf{j}}_b^i& {\mathbf{k}}_b^i\end{array}\right]=\mathbf{D}\left[\begin{array}{lll}{\mathbf{i}}_i^i& {\mathbf{j}}_i^i& {\mathbf{k}}_i^i\end{array}\right]=\mathbf{D}\mathbf{I}\\ \left[\begin{array}{lll}{\mathbf{i}}_b^i& {\mathbf{j}}_b^i& {\mathbf{k}}_b^i\end{array}\right]=\left[\begin{array}{lll}{\mathbf{i}}_i^i& {\mathbf{j}}_i^i& {\mathbf{k}}_i^i\end{array}\right]\mathbf{P}=\mathbf{IP}\end{array}\}\mathbf{D}=\mathbf{P}={\mathbf{C}}_b^i$$

$P=D$ 表明，罗德里格旋转矩阵 $D$ 正好是从参考坐标系（$i$ 系）到动坐标系（$b$ 系）的过渡矩阵，它也是从 $b$ 系到 $i$ 系的坐标变换矩阵。因此可以将罗德里格旋转公式重新写为方向阵的形式：
$${\mathbf{C}}_b^i=\mathbf{I}+\sin \varphi (\mathbf{u}\times )+(1-\cos \varphi )(\mathbf{u}\times {)}^2$$
进一步, 若记 $\mathbf{\varphi }=\varphi \mathbf{u}$ 和 $\varphi =|\mathbf{\varphi }|$, 则有 $\mathbf{u}=\mathbf{\varphi }/\varphi$ ，得：
$${\mathbf{C}}_b^i=\mathbf{I}+\frac{\sin \varphi }{\varphi }(\mathbf{\varphi }\times )+\frac{1-\cos \varphi }{{\varphi }^2}(\mathbf{\varphi }\times {)}^2$$
这里 $\mathbf{\varphi }$ 称为等效旋转矢量（RV，简称旋转矢量）

等效旋转矢量要点：

1. 刚体的有限转动是不可交换的。刚体的定点有限旋转都可以用绕经过该固定点的一个轴的一次转动来实现。
2. 其矢量方向表示旋转的方向，模的大小表示旋转角度大小。
3. 当载体的角速度方向随时间变化而不是做 “定轴转动” 时, 直接将传感器角增量测量值代入方向余弦矩阵或四元数微分方程的求解计算式中，则会带来 “不可交换性误差”。
4. Bortz 方程： $\dot{\varphi }=\mathbf{\omega }+\frac{1}{2}(\varphi \times \omega )+\frac{1}{{\varphi }^2}\left[1-\frac{\varphi \sin \varphi }{2(1-\cos \varphi )}\right]\mathbf{\varphi }\times (\mathbf{\varphi }\times \mathbf{\omega })$ 。是一种常用的等效旋转矢量微分方程，利用等效旋转矢量进行转动不可交换误差补偿。
5. 等效旋转矢量的双子样求解式：${\varphi }_k=\Delta {\theta }_k+\frac{1}{12}\Delta {\theta }_{k-1}\times \Delta {\theta }_k$ 。

二、PSINS中的表示转换

1、姿态阵/姿态四元数/欧拉角的表示 Cnb/qnb/att

• 有角标的符号书写一般遵从规律是从左到右从上到下。

• 姿态阵：Cnb，表示从 b 系到 n 系的坐标变换矩阵。对应姿态四元数写为qnb。

• 欧拉角：att=[俯仰pitch; 横滚roll; 方位yaw]。没有按照转到顺序来写，如果按转动顺序，东北天坐标系转到右前下坐标系，常见先转方位角、然后俯仰角、横滚角

  

2、四元数计算

1. 四元数共轭：qconj

实部不变，虚部取反。

function qo = qconj(qi)
    qo = [qi(1); -qi(2:4)];

性质：

• 加的共轭等于共轭的加：$(\mathbf{P}+Q{)}^{\ast }=P^{\ast }+Q^{\ast }$
• 乘的共轭等于共轭的和：$(\mathbf{P}\circ Q{)}^{\ast }=Q^{\ast }\circ P^{\ast }$

2. 四元数归一化：qconj

function qnb = qnormlz(qnb)
    qnb = qnb/norm(qnb);

3. 四元数相乘：qmual

$$\{\begin{array}{l}\mathbf{i}\circ \mathbf{i}=\mathbf{j}\circ \mathbf{j}=\mathbf{k}\circ \mathbf{k}=-1\\ \mathbf{i}\circ \mathbf{j}=\mathbf{k},\mathbf{j}\circ \mathbf{k}=\mathbf{i},\mathbf{k}\circ \mathbf{i}=\mathbf{j},\mathbf{j}\circ \mathbf{i}=-k,k\circ \mathbf{j}=-i,\mathbf{i}\circ \mathbf{k}=-\mathbf{j}\end{array}$$

不满足交换率。$i、j、k$ 正向相乘等于后面一项，反向相乘要加负号。

function q = qmul(q1, q2)
    if length(q2)==3, q2=[0; q2]; end  % 0-scale quaternion
    q = [ q1(1) * q2(1) - q1(2) * q2(2) - q1(3) * q2(3) - q1(4) * q2(4);
          q1(1) * q2(2) + q1(2) * q2(1) + q1(3) * q2(4) - q1(4) * q2(3);
          q1(1) * q2(3) + q1(3) * q2(1) + q1(4) * q2(2) - q1(2) * q2(4);
          q1(1) * q2(4) + q1(4) * q2(1) + q1(2) * q2(3) - q1(3) * q2(2) ];

AI解释：
这个函数实现了两个四元数的乘法操作。四元数是一种用于表示旋转和仿射变换的数学对象。在这个函数中，q1和q2是两个四元数，函数将它们相乘，并将结果存储在变量q中。

1. 首先，函数会检查q2的长度是否为3。如果是，它会将q2扩展为长度为4的四元数，其中一个元素被设置为0。这是为了避免在后续计算中出现除以0的情况。
2. 接下来，函数通过使用四元数的乘法公式来计算结果。四元数的乘法可以通过将两个四元数的各个分量对应相乘，然后将乘积相加得到。
3. 最后，函数将计算得到的结果存储在变量q中，并返回该变量作为函数输出。

4. 四元数乘向量（三维向量的坐标变换）：qmulv

假设一个三维矢量 $r$，在载体坐标系（$b$ 系）和参考坐标系（$i$ 系）中的投影分别为：${\mathbf{r}}^b={\left[\begin{array}{lll}{r}_x^b& r_y^b& r_z^b\end{array}\right]}^{\mathrm{T}}$ 和 ${\mathbf{r}}^i={\left[\begin{array}{lll}{r}_x^i& r_y^i& r_z^i\end{array}\right]}^{\mathrm{T}}$，现对矢量 ${\mathbf{r}}^b$ (视为零标量四元数) 实施如下四元数乘法操作：
$$\begin{array}{rl}{\mathbf{Q}}_b^i\circ {\mathbf{r}}^b\circ {\mathbf{Q}}_i^b=& {\mathbf{M}}_{Q_b^i}\left({\mathbf{r}}^b\circ {\mathbf{Q}}_i^b\right)={\mathbf{M}}_{Q_b^i}\left({\mathbf{M}}_{Q_i^b}^{\prime }\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]\right)={\mathbf{M}}_{Q_b^i}{\mathbf{M}}_{Q_i^b}^{\prime }\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]=\\ & \left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{cccc}{q}_0& q_1& q_2& q_3\\ -q_1& q_0& -q_3& q_2\\ -q_2& q_3& q_0& -q_1\\ -q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ r_x^b\\ r_y^b\\ r_z^b\end{array}\right]=\\ & \left[\begin{array}{cccc}1& 0& 0& 0\\ 0& q_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 0& 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 0& 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]\left[\begin{array}{c}0\\ r_x^b\\ r_y^b\\ r_z^b\end{array}\right]\end{array}$$
可以发现，矩阵的右下角三阶对角分块矩阵恰好是姿态阵，可简写为：
$${\mathbf{Q}}_b^i\circ {\mathbf{r}}^b\circ {\mathbf{Q}}_i^b=\left[\begin{array}{cc}1& 0_{1\times 3}\\ 0_{3\times 1}& {\mathbf{C}}_b^i\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{r}}^b\end{array}\right]=\left[\begin{array}{c}0\\ {\mathbf{C}}_b^i{\mathbf{r}}^b\end{array}\right]=\left[\begin{array}{c}0\\ {\mathbf{r}}^i\end{array}\right]$$
这表明, $Q_b^i\circ {\mathbf{r}}^b\circ {\mathbf{Q}}_i^b$ 的结果也是一个零标量四元数，其虚部正好对应于方向余弦阵的坐标变换 ${\mathbf{r}}^i={\mathbf{C}}_b^i{\mathbf{r}}^b$ 。为了书写简洁，类似于矩阵的坐标变换表达习惯，可定义四元数与三维矢量的乘法 运算，即四元数坐标变换公式：
$$r^i=Q_b^i\otimes r^b$$
其含义是先进行四元数乘法运算，之后在提取出结果中的虚部。

function vo = qmulv(q, vi)
    qo1 =              - q(2) * vi(1) - q(3) * vi(2) - q(4) * vi(3);
    qo2 = q(1) * vi(1)                + q(3) * vi(3) - q(4) * vi(2);
    qo3 = q(1) * vi(2)                + q(4) * vi(1) - q(2) * vi(3);
    qo4 = q(1) * vi(3)                + q(2) * vi(2) - q(3) * vi(1);
    vo = vi;
    vo(1) = -qo1 * q(2) + qo2 * q(1) - qo3 * q(4) + qo4 * q(3);
    vo(2) = -qo1 * q(3) + qo3 * q(1) - qo4 * q(2) + qo2 * q(4);
    vo(3) = -qo1 * q(4) + qo4 * q(1) - qo2 * q(3) + qo3 * q(2);

5. 四元数加失准角误差：qaddphi

从真实导航系 ( $n$ 系) 到计算导航系 ( $n^{\prime }$ 系) 的失准角为 $\mathbf{\varphi }$, 反之，从 $n^{\prime }$ 系到 $n$ 系的失准角应为 $-\mathbf{\varphi }$，若将 $-\varphi$ 视为等效旋转矢量，则与其对应的变换四元数为 $Q_n^{n^{\prime }}$ 。程序中, 变量 qpb、qnb、phi分别代表 $Q_b^{n^{\prime }},Q_b^n$ 和 $\varphi$ 。

function qpb = qaddphi(qnb, phi)
    qpb = qmul(rv2q(-phi),qnb);

6. 四元数减失准角误差：qdelphi

对应于公式 $Q_b^n=Q_{n^{\prime }}^n{}^{\circ }{Q}_b^{\prime }$，其含义是：在计算姿态四元数中扣除失准角后，获得真实 (更精确的) 姿态四元数。

function qnb = qdelphi(qpb, phi)
    qnb = qmul(rv2q(phi), qpb);

7. 由四元数和真实四元数计算失准角误差：qq2phi

先根据公式 $Q_{n^{\prime }}^n=Q_b^n\circ \left(Q_b^{n^{\prime }}\right){}^{\ast }$ 求得误差四元数 $Q_{n^{\prime }}^n$，再由 $Q_{n^{\prime }}^n$ 求解失准角 $\mathbf{\varphi }$ 。

function phi = qq2phi(qpb, qnb)
    qnp = qmul(qnb, qconj(qpb));
    phi = q2rv(qnp);

3、各种姿态表示的转换

1. 欧拉角 -> 姿态阵：a2mat

欧拉角是用三次选择来表示姿态变化，将三次选择矩阵相乘得到方向余弦矩阵矩阵（姿态阵），其中转动顺序很关键。
$$\begin{array}{l}{\mathbf{C}}_b^t={\mathbf{C}}_{\psi }{\mathbf{C}}_{\mathbf{C}}{\mathbf{C}}_{\gamma }=\left[\begin{array}{ccc}{c}_{\psi }& -s_{\psi }& 0\\ s_{\dot{\psi }}& c_{\psi }& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& c_{\theta }& -s_{\theta }\\ 0& s_{\theta }& c_{\theta }\end{array}\right]\left[\begin{array}{ccc}{c}_{\gamma }& 0& s_{\gamma }\\ 0& 1& 0\\ -s_{\gamma }& 0& c_{\gamma }\end{array}\right]=\\ \left[\begin{array}{ccc}{c}_{\psi }{c}_{\gamma }-s_{\psi }{s}_{\theta }{s}_{\gamma }& -s_{\psi }{c}_{\theta }& c_{\psi }{s}_{\gamma }+s_{\psi }{s}_{\theta }{c}_{\gamma }\\ s_{\psi }{c}_{\gamma }+c_{\psi }{s}_{\theta }{s}_{\gamma }& c_{\psi }{c}_{\theta }& s_{\psi }{s}_{\gamma }-c_{\psi }{s}_{\theta }{c}_{\gamma }\\ -c_{\theta }{s}_{\gamma }& s_{\theta }& c_{\theta }{c}_{\gamma }\end{array}\right]=\left[\begin{array}{ccc}{C}_{11}& C_{12}& C_{13}\\ C_{21}& C_{22}& C_{23}\\ C_{31}& C_{32}& C_{33}\end{array}\right]\end{array}$$

function [Cnb, Cnbr] = a2mat(att)
% Convert Euler angles to direction cosine matrix(DCM).
% 欧拉角->姿态阵
% Prototype: [Cnb, Cnbr] = a2mat(att)
% Input: att - att=[pitch; roll; yaw] in radians
% Outputs: Cnb - DCM from navigation-frame(n) to body-frame(b), in yaw->pitch->roll
%                (3-1-2) rotation sequence
%          Cnbr - DCM in yaw->roll->pitch (3-2-1) rotation sequence
    s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3); 
    ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
            cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
           -ci*sj,           si,     ci*cj           ];
    if nargout==2  % dual Euler angle DCM
        Cnbr = [ cj*ck, si*sj*ck-ci*sk, ci*sj*ck+si*sk;
                 cj*sk, si*sj*sk+ci*ck, ci*sj*sk-si*ck;
                -sj,    si*cj,          ci*cj            ];
    endfunction [Cnb, Cnbr] = a2mat(att)
    s = sin(att); c = cos(att);
    si = s(1); sj = s(2); sk = s(3); 
    ci = c(1); cj = c(2); ck = c(3);
    Cnb = [ cj*ck-si*sj*sk, -ci*sk,  sj*ck+si*cj*sk;
            cj*sk+si*sj*ck,  ci*ck,  sj*sk-si*cj*ck;
           -ci*sj,           si,     ci*cj           ];
    if nargout==2  % dual Euler angle DCM
        Cnbr = [ cj*ck, si*sj*ck-ci*sk, ci*sj*ck+si*sk;
                 cj*sk, si*sj*sk+ci*ck, ci*sj*sk-si*ck;
                -sj,    si*cj,          ci*cj            ];
    end

AI解释：
这段代码是一个将欧拉角转换为姿态矩阵的函数。它生成两个输出参数：Cnb和Cnbr，分别表示从导航系到机体系的姿态矩阵和从导航系到机体系的另一种姿态矩阵。如果函数的输出参数个数为2（即要求同时生成Cnb和Cnbr），则还会计算Cnbr矩阵。Cnbr矩阵是按照"yaw-roll-pitch"的旋转顺序（3-2-1）构建的。

2. 姿态阵 -> 欧拉角：m2att

$$\begin{array}{l}\theta =\arcsin \left(C_{32}\right)\\ \{\begin{array}{l}\gamma =-\mathrm{atan}2\left(C_{31},C_{33}\right)\\ \psi =-\mathrm{atan}2\left(C_{12},C_{22}\right)\end{array}\left|C_{32}\right|⩽0.999999\\ \{\begin{array}{ll}\gamma =\mathrm{atan}2\left(C_{13},C_{11}\right)& \left|C_{32}\right|>\mathrm{0.999.999}\\ \psi =0& \end{array}\end{array}$$

function [att, attr] = m2att(Cnb)
% Convert direction cosine matrix(DCM) to Euler attitude angles.
% 姿态阵 -> 欧拉角
% Prototype: [att, attr] = m2att(Cnb)
% Input: Cnb - DCM from navigation-frame(n) to body-frame(b)
% Outputs: att - att=[pitch; roll; yaw] in radians, in yaw->pitch->roll
    att = [ asin(Cnb(3,2));
            atan2(-Cnb(3,1),Cnb(3,3)); 
            atan2(-Cnb(1,2),Cnb(2,2)) ];
    if nargout==2  % dual Euler angles
        attr = [ atan2(Cnb(3,2),Cnb(3,3)); 
                 asin(-Cnb(3,1)); 
                 atan2(Cnb(2,1),Cnb(1,1)) ];
    end

AI解释：
这段代码是一个MATLAB函数，用于将方向余弦矩阵（DCM）转换为欧拉姿态角。函数名为m2att，输入参数是DCM矩阵Cnb，输出参数为欧拉姿态角att和可选的旋转轴和旋转角度attr。代码的功能如下：

1. 将DCM矩阵Cnb的每一列表示为一个单位向量，代表导航坐标系（n）到机体坐标系（b）的旋转向量。
2. 使用这些旋转向量计算机体坐标系相对于导航坐标系的姿态，得到欧拉姿态角。
3. 将计算得到的欧拉姿态角存储在att数组中，按 yaw -> pitch -> roll 的顺序排列，以弧度为单位。
4. 如果输出参数attr被指定为双欧拉角形式（通过判断nargout变量的值），则还需要计算旋转轴和旋转角度。
5. 计算旋转轴的公式为：(Cnb(3,2), Cnb(3,3))，即通过DCM矩阵的第3行第2列和第3列的元素构成的向量。
6. 计算旋转角度的公式为：[-asin(-Cnb(3,1)); atan2(Cnb(2,1), Cnb(1,1))]，即先计算绕z轴旋转的角度（通过DCM矩阵的第3行第1列的元素的符号相反），再计算绕x’轴旋转的角度（通过DCM矩阵的第2行第1列和第1行第1列的元素）。
7. 将计算得到的旋转轴和旋转角度存储在attr数组中，以弧度为单位。
   需要注意的是，该代码中使用了MATLAB中的函数asin、atan2来表示反正弦和反余弦函数，以及判断nargout变量的值来确定输出参数的形式。

3. 四元数 -> 姿态阵：q2mat

$${\mathbf{C}}_b^n=\left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2& 2\left(q_1{q}_2-q_0{q}_3\right)& 2\left(q_1{q}_3+q_0{q}_2\right)\\ 2\left(q_1{q}_2+q_0{q}_3\right)& q_0^2-q_1^2+q_2^2-q_3^2& 2\left(q_2{q}_3-q_0{q}_1\right)\\ 2\left(q_1{q}_3-q_0{q}_2\right)& 2\left(q_2{q}_3+q_0{q}_1\right)& q_0^2-q_1^2-q_2^2+q_3^2\end{array}\right]$$

function Cnb = q2mat(qnb)
% Convert attitude quaternion to direction cosine matrix(DCM).
% 四元数 -> 姿态阵
% Prototype: Cnb = q2mat(qnb)
% Input: qnb - attitude quaternion
% Output: Cnb - DCM from body-frame to navigation-frame
    q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
    q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);     
    q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);  
    q44 = qnb(4)*qnb(4);
    Cnb = [ q11+q22-q33-q44,  2*(q23-q14),     2*(q24+q13);
            2*(q23+q14),      q11-q22+q33-q44, 2*(q34-q12);
            2*(q24-q13),      2*(q34+q12),     q11-q22-q33+q44 ];

4. 姿态阵 -> 四元数：m2qua

$$\begin{array}{l}{q}_0^2+q_1^2-q_2^2-q_3^2=C_{11}\\ q_0^2-q_1^2+q_2^2-q_3^2=C_{22}\\ q_0^2-q_1^2-q_2^2+q_3^2=C_{33}\\ q_0^2+q_1^2+q_2^2+q_3^2=1\end{array}\}\Rightarrow \begin{array}{l}\left|q_0\right|=0.5\sqrt{1+C_{11}+C_{22}+C_{33}}\\ \left|q_1\right|=0.5\sqrt{1+C_{11}-C_{22}-C_{33}}\\ \left|q_2\right|=0.5\sqrt{1-C_{11}+C_{22}-C_{33}}\\ \left|q_3\right|=0.5\sqrt{1-C_{11}-C_{22}+C_{33}}\end{array}$$

$$\begin{array}{l}2\left(q_1{q}_2-q_0{q}_3\right)=C_{12}\\ 2\left(q_1{q}_2+q_0{q}_3\right)=C_{21}\\ 2\left(q_1{q}_3+q_0{q}_2\right)=C_{13}\\ 2\left(q_1{q}_3-q_0{q}_2\right)=C_{31}\\ 2\left(q_2{q}_3-q_0{q}_1\right)=C_{23}\\ 2\left(q_2{q}_3+q_0{q}_1\right)=C_{32}\end{array}\}\Rightarrow \begin{array}{l}4q_0{q}_1=C_{32}-C_{23}\\ 4q_0{q}_2=C_{13}-C_{31}\\ 4q_0{q}_3=C_{21}-C_{12}\\ 4q_1{q}_2=C_{12}+C_{21}\\ 4q_1{q}_3=C_{13}+C_{31}\\ 4q_2{q}_3=C_{23}+C_{32}\end{array}\}$$

若仅根据式一，将难以确定四元数各元素的正负符号。如果已知四元数的某一个元素, 则根据式二可求解其他元素, 但须避免该已知元素为 0 。由四元数归一化条件 $q_0^2+q_1^2$ $+q_2^2+q_3^2=1$ 可知, 必然有 $\max \left(q_i^2\right)⩾1/4$ 成立, 也就是说，四个元素中必然存在某个 $\left|q_i\right|⩾$ $1/2$ 。实际应用时，可先根据式 一计算获得某一个较大的元素 $q_i$ (不妨取为正值)，再根据 式 (B. 10)计算剩余的其他三个元素。

function qnb = m2qua(Cnb)
% Convert direction cosine matrix(DCM) to attitude quaternion.
%
% Prototype: qnb = m2qua(Cnb)
% Input: Cnb - DCM from body-frame to navigation-frame
% Output: qnb - attitude quaternion
    C11 = Cnb(1,1); C12 = Cnb(1,2); C13 = Cnb(1,3); 
    C21 = Cnb(2,1); C22 = Cnb(2,2); C23 = Cnb(2,3); 
    C31 = Cnb(3,1); C32 = Cnb(3,2); C33 = Cnb(3,3); 
%     q0t = 0.5*sqrt(1+C11+C22+C33);
%     q1t = 0.5*sqrt(1+C11-C22-C33);
%     q2t = 0.5*sqrt(1-C11+C22-C33);
%     q3t = 0.5*sqrt(1-C11-C22+C33);
    if C11>=C22+C33
        q1 = 0.5*sqrt(1+C11-C22-C33);  qq4 = 4*q1;
        q0 = (C32-C23)/qq4; q2 = (C12+C21)/qq4; q3 = (C13+C31)/qq4;
    elseif C22>=C11+C33
        q2 = 0.5*sqrt(1-C11+C22-C33);  qq4 = 4*q2;
        q0 = (C13-C31)/qq4; q1 = (C12+C21)/qq4; q3 = (C23+C32)/qq4;
    elseif C33>=C11+C22
        q3 = 0.5*sqrt(1-C11-C22+C33);  qq4 = 4*q3;
        q0 = (C21-C12)/qq4; q1 = (C13+C31)/qq4; q2 = (C23+C32)/qq4;
    else
        q0 = 0.5*sqrt(1+C11+C22+C33);  qq4 = 4*q0;
        q1 = (C32-C23)/qq4; q2 = (C13-C31)/qq4; q3 = (C21-C12)/qq4;
    end
    qnb = [q0; q1; q2; q3];

AI解释：
函数接受一个输入参数Cnb，它是一个3x3的矩阵，表示从机体坐标系到导航坐标系的DCM。函数的输出是一个四元数向量qnb，表示给定DCM对应的态度。代码的实现如下：

1. 从输入的Cnb矩阵中提取出元素值，并分别赋值给变量C11、C12、C13、C21、C22、C23、C31、C32和C33。
2. 根据DCM的元素值，使用一系列条件语句来确定每个元素的计算方式，然后计算出四元数向量的各个元素。
3. 根据条件判断的结果，选择合适的计算方式来得到最终的四元数向量qnb。

5. 欧拉角 -> 四元数：a2qua

惯导姿态更新算法中常用的是四元数，惯导的输出结果常表示为欧拉角，需要进行四元数和欧拉角的相互转换，根据单位四元数的几何意义得：
$$\begin{array}{rl}{\mathbf{Q}}_b^n=& {\mathbf{Q}}_{\psi }\circ {\mathbf{Q}}_{\theta }\circ {\mathbf{Q}}_{\gamma }=\left(c_{\psi /2}+\mathbf{k}{s}_{\psi /2}\right)\circ \left(c_{\theta /2}+\mathbf{i}{s}_{\theta /2}\right)\circ \left(c_{\gamma /2}+\mathbf{j}{s}_{\gamma /2}\right)=\\ & \left(c_{\psi /2}{c}_{\theta /2}+\mathbf{i}{c}_{\psi /2}{s}_{\theta /2}+\mathbf{k}{s}_{\psi /2}{c}_{\theta /2}+\mathbf{k}\circ {\mathbf{i}}_{\psi /2}{s}_{\theta /2}\right)\circ \left(c_{\gamma /2}+\mathbf{j}{s}_{\gamma /2}\right)=\\ & \left(c_{\psi /2}{c}_{\theta /2}+\mathbf{i}{c}_{\psi /2}{s}_{\theta /2}+\mathbf{k}{s}_{\psi /2}{c}_{\theta /2}+\mathbf{j}{s}_{\psi /2}{s}_{\theta /2}\right)\circ \left(c_{\gamma /2}+\mathbf{j}{s}_{\gamma /2}\right)=\\ & \left[\begin{array}{l}{c}_{\psi /2}{c}_{\theta /2}{c}_{\gamma /2}-s_{\psi /2}{s}_{\theta /2}{s}_{\gamma /2}\\ c_{\psi /2}{s}_{\theta /2}{c}_{\gamma /2}-s_{\psi /2}{c}_{\theta /2}{s}_{\gamma /2}\\ s_{\psi /2}{s}_{\theta /2}{c}_{\gamma /2}+c_{\psi /2}{c}_{\theta /2}{s}_{\gamma /2}\\ s_{\psi /2}{c}_{\theta /2}{c}_{\gamma /2}+c_{\psi /2}{s}_{\theta /2}{s}_{\gamma /2}\end{array}\right]\end{array}$$

function qnb = a2qua(att)
% Convert Euler angles to attitude quaternion.
% 欧拉角 -> 四元数
% Prototype: qnb = a2qua(att)
% Input: att - att=[pitch; roll; yaw] in radians
% Output: qnb - attitude quaternion
    att2 = att/2;
    s = sin(att2); c = cos(att2);
    sp = s(1); sr = s(2); sy = s(3); 
    cp = c(1); cr = c(2); cy = c(3); 
    qnb = [ cp*cr*cy - sp*sr*sy;
            sp*cr*cy - cp*sr*sy;
            cp*sr*cy + sp*cr*sy;
            cp*cr*sy + sp*sr*cy ];

AI解释：
数的输出是一个四元数qnb，表示给定欧拉角对应的态度。代码的实现如下：

1. 将att向量除以2，得到att2向量，表示每个角度的一半。
2. 使用sin和cos函数计算att2向量的正弦和余弦值，得到对应的s和c向量。
3. 将s和c向量的元素分别赋值给变量sp、sr、sy、cp、cr和cy，以便后续计算。
4. 根据四元数的公式，计算四元数qnb的每个元素。

6. 四元数 -> 欧拉角：q2att

由四元数直接求解欧拉角并不容易，可以通过姿态阵作为中间过渡量。
$$\begin{array}{l}\theta =\arcsin \left(2\left(q_2{q}_3+q_0{q}_1\right)\right)\\ \left\{\begin{array}{ll}\gamma =-\mathrm{atan}2\left(2\left(q_1{q}_3-q_0{q}_2\right),q_0^2-q_1^2-q_2^2+q_3^2\right)& \\ \psi =-\mathrm{atan}2\left(2\left(q_1{q}_2-q_0{q}_3\right),q_0^2-q_1^2+q_2^2-q_3^2\right)& \left|2\left(q_2{q}_3+q_0{q}_1\right)\right|⩽0.999999\end{array}\right\}\\ \{\begin{array}{ll}\gamma =\mathrm{atan}2\left(2\left(q_1{q}_3+q_0{q}_2\right),q_0^2+q_1^2-q_2^2-q_3^2\right)& \left|2\left(q_2{q}_3+q_0{q}_1\right)\right|>0.999999\\ \psi =0& \end{array}\end{array}$$

function att = q2att(qnb)
% Convert attitude quaternion to Euler attitude angles.
% 四元数 -> 欧拉角
% Prototype: att = q2att(qnb)
% Input: qnb - attitude quaternion
% Output: att - Euler angles att=[pitch; roll; yaw] in radians
    q11 = qnb(1)*qnb(1); q12 = qnb(1)*qnb(2); q13 = qnb(1)*qnb(3); q14 = qnb(1)*qnb(4); 
    q22 = qnb(2)*qnb(2); q23 = qnb(2)*qnb(3); q24 = qnb(2)*qnb(4);     
    q33 = qnb(3)*qnb(3); q34 = qnb(3)*qnb(4);  
    q44 = qnb(4)*qnb(4);
    C12=2*(q23-q14);
    C22=q11-q22+q33-q44;
    C31=2*(q24-q13); C32=2*(q34+q12); C33=q11-q22-q33+q44;
    att = [ asin(C32); 
            atan2(-C31,C33); 
            atan2(-C12,C22) ];

AI解释：
这段代码是一个将四元数转换为欧拉角的函数。它接受一个四元数作为输入，并返回包含欧拉角（俯仰角、偏航角和翻滚角）的向量。代码的主要步骤如下：

1. 定义变量：首先，代码将输入的四元数分解为各个分量的平方，并存储在变量q11、q22、q33和q44中。这些变量用于后续的计算。
2. 定义旋转矩阵的元素：接下来，代码使用四元数的分量计算旋转矩阵的元素C12、C22、C31、C32和C33。这些元素将用于计算欧拉角。
3. 计算欧拉角：最后，代码使用旋转矩阵的元素计算欧拉角。首先，使用asin函数计算C32的值，得到俯仰角。然后，使用atan2函数计算C31和C33的值的正切，得到偏航角。最后，使用atan2函数计算C12和C22的值的正切，得到翻滚角。
4. 最终，函数返回包含欧拉角（俯仰角、偏航角和翻滚角）的向量att。这些欧拉角表示四元数对应的姿态在三维空间中的旋转角度。

7. 四元数 -> 等效旋转矢量：q2rv

function rv = q2rv(q) 
% Convert transformation quaternion to rotation vector.
%  四元数 -> 等效旋转矢量
% Prototype: rv = q2rv(q) 
% Input: q - transformation quaternion
% Output: rv - corresponding rotation vector, such that
	if(q(1)<0)
	    q = -q;
	end
    n2 = acos(q(1));
    if n2>1e-40
        k = 2*n2/sin(n2);
    else
        k = 2;
    end
    rv = k*q(2:4);   % q = [ cos(|rv|/2); sin(|rv|/2)/|rv|*rv ];

AI解释：
这段代码是将变换四元数转换为等效旋转矢量的函数。它接受一个四元数作为输入，并返回对应的旋转向量。代码的主要步骤如下：

1. 转换四元数的符号：代码首先检查四元数中的第一个元素是否为负数。如果是负数，则将四元数取负，以确保旋转向量的符号与期望的一致。
2. 计算旋转向量的大小：代码使用acos函数计算四元数的第一个元素的反余弦值，得到旋转向量的大小（也称为旋转角）。这里使用了一个条件语句，如果n2（即旋转角）大于一个小的阈值（1e-40），则通过一个公式计算旋转向量的大小；否则，将旋转向量的大小设置为2。
3. 计算旋转向量的方向：根据旋转向量的大小，代码使用sin和cos函数计算四元数的第二个到第四个元素对应的旋转向量的方向。具体来说，通过将旋转向量的大小与四元数的第二个到第四个元素的乘积相乘，得到了旋转向量的方向。
4. 最终，函数返回计算得到的旋转向量rv。这个旋转向量表示了四元数对应的姿态在三维空间中的旋转等效矢量。

注意，在转换过程中，代码对输入的四元数进行了符号的调整，以确保旋转向量的符号与期望的一致。

8. 等效旋转矢量 -> 四元数：rv2q

首先，将四元数转化为标量非负的四元数；其次，根据公式 $Q=q_0+{\mathbf{q}}_v=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}=$ $\cos \frac{\varphi }{2}+\frac{\varphi }{2}\cdot \frac{\sin (\varphi /2)}{\varphi /2}$, 先由四元数的标量关系 $q_0=\cos \frac{\varphi }{2}$ 求旋转矢量模值的一半 $\frac{\varphi }{2}=$ $\arccos \left(q_0\right)$, 再由矢量关系 ${\mathbf{q}}_v=\frac{\mathbf{\varphi }}{2}\cdot \frac{\sin (\varphi /2)}{\varphi /2}$ 求等效旋转矢量 $\mathbf{\varphi }=2\frac{(\varphi /2)}{\sin (\varphi /2)}{\mathbf{q}}_v$ 。
$$Q=q_0+{\mathbf{q}}_v=\cos \frac{\varphi }{2}+\mathbf{u}\sin \frac{\varphi }{2}$$

function q = rv2q(rv)
% Convert rotation vector to transformation quaternion.
%
% Prototype: q = rv2q(rv)
% Input: rv - rotation vector
% Output: q - corresponding transformation quaternion, such that
%             q = [ cos(|rv|/2); sin(|rv|/2)/|rv|*rv ]
    q = zeros(4,1);
    n2 = rv(1)*rv(1) + rv(2)*rv(2) + rv(3)*rv(3);
    if n2<1.0e-8  % cos(n/2)=1-n2/8+n4/384; sin(n/2)/n=1/2-n2/48+n4/3840
        q(1) = 1-n2*(1/8-n2/384); s = 1/2-n2*(1/48-n2/3840);
    else
        n = sqrt(n2); n_2 = n/2;
        q(1) = cos(n_2); s = sin(n_2)/n;
    end
    q(2) = s*rv(1); q(3) = s*rv(2); q(4) = s*rv(3);

这段代码是将旋转向量转换为变换四元数的函数。它接受一个旋转向量作为输入，并返回对应的变换四元数。代码的主要步骤如下：

1. 初始化四元数：代码首先创建一个大小为4的零向量q，用于存储变换四元数的各个元素。
2. 计算旋转向量的大小：代码使用旋转向量的第一个元素作为旋转向量的大小（也称为旋转角）。这里使用了一个条件语句，如果旋转向量的范数平方小于一个小的阈值（1.0e-8），则使用泰勒级数展开来计算旋转向量的大小；否则，通过标准公式计算旋转向量的大小。
3. 计算旋转向量的方向：根据旋转向量的大小，代码使用sin和cos函数计算旋转向量的方向。具体来说，通过将旋转向量的大小与旋转向量的第一个元素的符号相乘，并将结果与旋转向量的其他三个元素相乘，得到了旋转向量的方向。
4. 构建变换四元数：根据旋转向量的大小和方向，代码构建了对应的变换四元数。具体来说，将旋转向量的大小除以2，然后使用sin和cos函数计算变换四元数的各个元素。
5. 最终，函数返回计算得到的变换四元数q。这个变换四元数表示了旋转向量对应的姿态在四元数空间中的表示。

注意，在计算过程中，代码使用了不同的公式来处理旋转向量大小的不同情况，以提高计算精度。

9. 姿态阵 -> 等效旋转矢量：m2rv

function rv = m2rv(m)
% Convert transformation matrix to rotation vector.
%
% Prototype: rv = m2rv(m)
% Input: m - transformation matrix
% Output: rv - corresponding rotation vector, such that
        rv = [m(3,2)-m(2,3); m(1,3)-m(3,1); m(2,1)-m(1,2)];  % 11/10/2022
        phi = acos((m(1,1)+m(2,2)+m(3,3)-1)/2);
        if phi<1e-10, afa=1/2; else afa=phi/(2*sin(phi)); end
%         afa = phi/sqrt(rv'*rv);
        rv = afa*rv;
        return;
%     rv = iaskew(m-glv.I33);  % coarse init is ok when rv is small, otherwise may fail
%     rvx = askew(rv); % good! the following iteration due to the
    for k=1:2        % accuracy deduce of sqrt(.) in function m2qua
        xx = rv(1)*rv(1); xy = rv(1)*rv(2); xz = rv(1)*rv(3);
        yy = rv(2)*rv(2); yz = rv(2)*rv(3); zz = rv(3)*rv(3);
        n2 = xx+yy+zz;
%         n2 = rv'*rv; 
        if n2>1.0e-40
            n = sqrt(n2);
%             rvx = (m-eye(3)-(1-cos(n))/n2*rvx*rvx) * (n/sin(n));
            rvx = (m1-(1-cos(n))/n2*[-yy-zz,xy,xz; xy,-xx-zz,yz; xz,yz,-xx-yy]) * (n/sin(n));
            rv = [rvx(3,2); rvx(1,3); rvx(2,1)]; % rv = iaskew(rvx);
        else
            rv = zeros(3,1);
            break;
        end
    end

AI解释：
这段代码是一个将变换矩阵转换为旋转向量的函数。它的主要功能如下：

1. 计算旋转向量：通过从变换矩阵的元素中减去对角线上的元素，然后应用一定的计算步骤，得到旋转向量。
2. 计算旋转角度：使用acos函数计算旋转向量中的旋转角度。
3. 计算旋转向量归一化系数：根据旋转角度的值，计算旋转向量归一化系数。如果旋转角度非常小（小于1e-10），则将归一化系数设为1/2，否则根据公式计算。
4. 归一化旋转向量：使用归一化系数将旋转向量进行归一化。
5. 迭代计算旋转向量：通过一个循环来迭代计算旋转向量。首先，初始化旋转向量为一个与变换矩阵的差异较小的向量（使用coarse init）。然后，通过一系列的计算步骤来更新旋转向量。这个迭代过程会进行两次。

10. 等效旋转矢量 -> 姿态阵：rv2m

$${\mathbf{C}}_b^i=\mathbf{I}+\frac{\sin \varphi }{\varphi }(\mathbf{\varphi }\times )+\frac{1-\cos \varphi }{{\varphi }^2}(\mathbf{\varphi }\times {)}^2$$

function m = rv2m(rv)
% Convert rotation vector to transformation matrix.
% 等效旋转矢量 -> 姿态阵
% Prototype: m = rv2m(rv)
% Input: rv - rotation vector
% Output: m - corresponding DCM, such that

	xx = rv(1)*rv(1); yy = rv(2)*rv(2); zz = rv(3)*rv(3);
	n2 = xx+yy+zz;
    if n2<1.e-8
        a = 1-n2*(1/6-n2/120); b = 0.5-n2*(1/24-n2/720);  % a->1, b->0.5
    else
        n = sqrt(n2);
        a = sin(n)/n;  b = (1-cos(n))/n2;
    end
	arvx = a*rv(1);  arvy = a*rv(2);  arvz = a*rv(3);
	bxx = b*xx;  bxy = b*rv(1)*rv(2);  bxz = b*rv(1)*rv(3);
	byy = b*yy;  byz = b*rv(2)*rv(3);  bzz = b*zz;
	m = zeros(3,3);
	% m = I + a*(rvx) + b*(rvx)^2;
	m(1)=1     -byy-bzz; m(4)= -arvz+bxy;     m(7)=  arvy+bxz;
	m(2)=  arvz+bxy;     m(5)=1     -bxx-bzz; m(8)= -arvx+byz;
	m(3)= -arvy+bxz;     m(6)=  arvx+byz;     m(9)=1     -bxx-byy;

AI解释：
这个函数是将旋转矢量转换为姿态矩阵的函数。它接受一个旋转矢量作为输入，并返回对应的姿态矩阵作为输出。流程如下：

1. 首先，函数通过将输入的旋转矢量分别与单位矢量进行点积，计算出旋转矢量的三个分量（即xx、yy和zz）。
2. 接下来，函数计算了一个变量n2，它等于旋转矢量的三个分量的平方和。如果n2小于一个很小的阈值（1.e-8），那么说明旋转矢量的长度非常接近于零，这种情况下会使用近似解来计算姿态矩阵的元素。
3. 如果n2大于等于1.e-8，说明旋转矢量的长度不为零，这时候使用泰勒级数展开来计算姿态矩阵的元素。在这种情况下，函数首先计算出旋转矢量的长度n，然后根据长度n计算出sin(n)和(1-cos(n))的值，这些值将用于构建姿态矩阵的元素。
4. 接下来，函数根据上述计算得到的值，构建出姿态矩阵的各个元素。这些元素包括arv(1)、arv(2)和arv(3)，以及bxx、brv(1)rv(2)、brv(1)rv(3)、byy、brv(2)rv(3)和bzz。
5. 最后，函数将这些元素填充到一个3x3的矩阵中，并将该矩阵作为输出返回。

总结起来，这个函数的功能是将旋转矢量转换为姿态矩阵。它使用泰勒级数展开和近似解来处理旋转矢量的长度接近于零的情况，以避免数值不稳定性。

11. 反对称阵

askew：求三维向量的反对称阵

function m = askew(v)
    m = [ 0,     -v(3),   v(2); 
          v(3),   0,     -v(1); 
         -v(2),   v(1),   0     ];