基本原理#
捷联式惯导系统和平台式惯导系统遵循共同的规律：惯导基本方程（比力方程）。
捷联式惯导系统导航坐标系也分为：指北方位、自由方位、游移方位。
平台式惯导依靠实体平台保持参考方向；捷联惯导取消实体平台，使用方向余弦矩阵或四元数构成“数学平台”。其计算主线是：
- 根据陀螺角增量更新姿态
- 将机体系比力转换到导航系并更新速度
- 对速度积分，更新位置
$C_b^p$ 表示从机体系到导航系的方向余弦矩阵（捷联矩阵$T$ ），则
$$ \mathbf r^p=C_b^p\mathbf r^b=Tr^b $$四元数的基本概念与运算#
定义与几种表示形式#
矢量式：$Q=q_0+\bold{q}$
复数式：$Q=q_0+q_1\boldsymbol i+q_2\boldsymbol j+q_3\boldsymbol k$ 其中 $q_0,q_1,q_2,q_3$ 是实数；$q_0$ 是标量部分，后三项组成矢量部分，$Q=q_0+\boldsymbol q,\qquad\boldsymbol q=q_1\boldsymbol i+q_2\boldsymbol j+q_3\boldsymbol k$
三角式（单位长度）：$Q=\cos\frac{\alpha}{2}+\boldsymbol u\sin\frac{\alpha}{2}$ 其中 $\boldsymbol u$ 为单位旋转轴，$\alpha$ 为旋转角
指数式(单位长度）：$Q=e^{\boldsymbol u\frac{\alpha}{2}}$
这里三角式和指数式用于描述单位旋转四元数，不是任意非单位四元数都能不带模长地写成这个形式。
共轭、范数与单位四元数#
共轭为：
$$ Q^*=q_0-q_1\boldsymbol i-q_2\boldsymbol j-q_3\boldsymbol k. $$范数为：
$$ \|Q\|=\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}. $$若 $\|Q\|=1$ ，称为单位四元数或规范化四元数。表示纯旋转时要求：
$$ \boxed{q_0^2+q_1^2+q_2^2+q_3^2=1}. $$相等、加减与标量乘法#
两个四元数相等，当且仅当四个对应分量分别相等。
设 $P=p_0+\boldsymbol p$ ，$Q=q_0+\boldsymbol q$ ，则：
$$ P\pm Q=(p_0\pm q_0)+(\boldsymbol p\pm\boldsymbol q). $$标量乘法按分量进行：
$$ aQ=aq_0+aq_1\boldsymbol i+aq_2\boldsymbol j+aq_3\boldsymbol k. $$加法满足交换律、结合律。
四元数乘法#
左乘、右乘矩阵：
$$ P\otimes Q=M(P)Q=M'(Q)P, $$ $$ M(P)= \begin{bmatrix} p_0&-p_1&-p_2&-p_3\\ p_1&p_0&-p_3&p_2\\ p_2&p_3&p_0&-p_1\\ p_3&-p_2&p_1&p_0 \end{bmatrix}, $$ $$ M'(Q)= \begin{bmatrix} 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{bmatrix}. $$乘法一般不满足交换律：
$$ P\otimes Q\ne Q\otimes P. $$但满足结合律、左右分配律：
$$ (P\otimes Q)\otimes S=P\otimes(Q\otimes S), $$ $$ P\otimes(Q+S)=P\otimes Q+P\otimes S. $$旋转复合时必须保持四元数的先后顺序。
四元数除法——求逆#
由 $Q\otimes Q^*=\|Q\|^2$ ，得到非零四元数的逆：
$$ \boxed{Q^{-1}=\frac{Q^*}{\|Q\|^2}}. $$对于单位四元数：
$$ \boxed{Q^{-1}=Q^*}. $$四元数与方向余弦矩阵#
将三维矢量写成标量为零的纯四元数：
$$ \mathcal R^b=\begin{bmatrix}0\\\boldsymbol r^b\end{bmatrix}. $$满足
$$ \mathcal R^R=Q\otimes\mathcal R^b\otimes Q^* $$ $$ \mathcal R^b=Q^*\otimes\mathcal R^R\otimes Q $$将四元数乘法展开，得到：
$$
C_b^R(Q)= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\ 2(q_1q_2+q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3-q_0q_1)\ 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} $$
选择 $R=n$ 时：
$$ T=C_b^n=C_b^R(Q), $$此时 $Q$ 称为姿态四元数。姿态更新可转化为先更新 $Q$ ，再由 $Q$ 计算捷联矩阵。
姿态表示#
捷联矩阵与姿态角#
姿态角为机体系相对平台系的转换角（本节）
以航向角 $\psi_G$ 、俯仰角 $\theta$ 和滚转角 $\gamma$ 描述姿态。，对应的捷联矩阵为
$$ T= \begin{bmatrix} \cos\gamma\cos\psi_G-\sin\gamma\sin\theta\sin\psi_G & -\cos\theta\sin\psi_G & \sin\gamma\cos\psi_G+\cos\gamma\sin\theta\sin\psi_G\\ \cos\gamma\sin\psi_G+\sin\gamma\sin\theta\cos\psi_G & \cos\theta\cos\psi_G & \sin\gamma\sin\psi_G-\cos\gamma\sin\theta\cos\psi_G\\ -\sin\gamma\cos\theta & \sin\theta & \cos\gamma\cos\theta \end{bmatrix}. $$姿态角可由矩阵元素求得：
$$ \theta_主=\arcsin T_{32}, \qquad \gamma_主=\operatorname{arctan}(\frac{-T_{31}}{T_{33}}), \qquad \psi_{G主}=\operatorname{arctan}(\frac{-T_{12}}{T_{22}}). $$根据定义域与主值判断真值：
- 俯仰角$\theta$ 的定义域为 $(-90°, +90°)$
- 倾斜角$\gamma$ 的定义域为 $(-180°, +180°)$
- 格网航向角$\psi_G$ 的定义域为 $(0°, 360°)$
捷联矩阵的作用：
- 用来实现比力坐标转换
- 根据捷联矩阵确定姿态角
注：欧拉角直观，但在俯仰角接近 $\pm90^\circ$ 时存在奇异性，因此姿态递推通常采用四元数。
捷联矩阵的更新方法#
欧拉角微分方程#
旋转顺序：
$$ x_p y_p z_p \xrightarrow[\psi_G]{\text{绕}z_p\text{轴}} x'_p y'_p z'_p \xrightarrow[\theta]{\text{绕}x'_p\text{轴}} x''_p y''_p z''_p \xrightarrow[\gamma]{\text{绕}y''_p\text{轴}} x_b y_b z_b $$$\psi$ 为航向角，$\theta$ 为横滚角，$\gamma$ 为俯仰角
投影至机体系 $$ \begin{bmatrix} \omega_{pbx}^b\ \omega_{pby}^b\ \omega_{pbz}^b \end{bmatrix}#
\dot\gamma \begin{bmatrix}0\1\0\end{bmatrix} + \dot\theta \begin{bmatrix}\cos\gamma\0\\sin\gamma\end{bmatrix} + \dot\psi_G \begin{bmatrix} -\sin\gamma\cos\theta\ \sin\theta\ \cos\gamma\cos\theta \end{bmatrix}. $$
矩阵表示 $$ \begin{bmatrix} \omega_{pbx}^b\ \omega_{pby}^b\ \omega_{pbz}^b \end{bmatrix}#
\begin{bmatrix} -\sin\gamma \cos\theta & \cos\gamma & 0 \ \sin\theta & 0 & 1 \ \cos\gamma \cos\theta & \sin\gamma & 0 \end{bmatrix} \begin{bmatrix} \dot{\psi}_G \ \dot{\theta} \ \dot{\gamma} \end{bmatrix} $$
反解微分方程
$$ \begin{bmatrix} \dot{\psi}_G \\ \dot{\theta} \\ \dot{\gamma} \end{bmatrix} = \frac{1}{\cos\theta} \begin{bmatrix} -\sin\gamma & 0 & \cos\gamma \\ \cos\gamma \cos\theta & 0 & \sin\gamma \cos\theta \\ \sin\gamma \sin\theta & \cos\theta & -\cos\gamma \sin\theta \end{bmatrix}\begin{bmatrix} \omega_{pbx}^b\\ \omega_{pby}^b\\ \omega_{pbz}^b \end{bmatrix} $$陀螺仪输出机体相对惯性系的角速度，需先处理为机体相对平台的角速度 $\boldsymbol\omega_{pb}^{b}=\boldsymbol\omega_{ib}^{b}-(C_b^p)^T\boldsymbol\omega_{ip}^{p}$ ，更新姿态角，然后更新捷联矩阵
注：求捷联矩阵当然采用平台系到机体系
方向余弦微分方程#
$$ \dot{C}_b^p=C_b^p(\omega_{pb}^b\times) $$$\boldsymbol\omega_{pb}^{b}=\boldsymbol\omega_{ib}^{b}-(C_b^p)^T\boldsymbol\omega_{ip}^{p}$ 求得此时刻角速度，$C_b^p$
四元数微分方程#
将机体相对参考系的角速度写成纯四元数：
$$ \mathcal W_{Rb}^b= \begin{bmatrix}0\\\boldsymbol\omega_{Rb}^b\end{bmatrix}. $$姿态四元数满足：
$$ \dot Q=\frac12 Q\otimes\mathcal W_{Rb}^b=\frac12 M'(\mathcal W_{Rb}^b)Q $$记 $\boldsymbol\omega_{Rb}^b=[\omega_x,\omega_y,\omega_z]^{\mathsf T}$
$$ M'(\mathcal W_{Rb}^b)= \begin{bmatrix} 0&-\omega_x&-\omega_y&-\omega_z\\ \omega_x&0&\omega_z&-\omega_y\\ \omega_y&-\omega_z&0&\omega_x\\ \omega_z&\omega_y&-\omega_x&0 \end{bmatrix}. $$于是：
$$ \begin{bmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \end{bmatrix} = \frac{1}{2} \begin{bmatrix} 0 & -\omega_x & -\omega_y & -\omega_z \\ \omega_x & 0 & \omega_z & -\omega_y \\ \omega_y & -\omega_z & 0 & \omega_x \\ \omega_z & \omega_y & -\omega_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} $$导航坐标系选平台坐标系时， $\boldsymbol\omega_{pb}^b =\boldsymbol\omega_{ib}^b-C_p^b \left(\boldsymbol\omega_{ie}^p+\boldsymbol\omega_{ep}^p\right) $
捷联惯导姿态更新：毕卡法、旋转矢量与圆锥补偿&&&&&&&——-&&&&&&&&&&#
先明确：这一部分要解决什么问题#
已知上一个时刻的姿态四元数 $Q_k$ ，以及本周期内陀螺提供的角运动，求下一个时刻的姿态 $Q_{k+1}$ 。
学习主线只有三步：
$$ \boxed{ \text{角增量子样} \longrightarrow\text{本周期的净旋转} \longrightarrow\text{增量四元数} \longrightarrow\text{新姿态} } $$- 毕卡法：从四元数微分方程出发，建立离散更新方法。
- 等效旋转矢量法：修正变轴转动时“角增量之和不等于净旋转”的问题。
- 多子样与圆锥优化：利用周期内部的信息，计算更准确的净旋转。
2. 毕卡法：从微分方程得到离散更新#
2.1 把微分方程改写成积分方程#
四元数运动学方程为：
$$ \dot Q=\frac12\Omega(\boldsymbol\omega)Q, $$ $$ \Omega(\boldsymbol\omega)= \begin{bmatrix} 0&-\omega_x&-\omega_y&-\omega_z\\ \omega_x&0&\omega_z&-\omega_y\\ \omega_y&-\omega_z&0&\omega_x\\ \omega_z&\omega_y&-\omega_x&0 \end{bmatrix}. $$令 $A(\tau)=\tfrac12\Omega(\boldsymbol\omega(t_k+\tau))$ ，则：
$$ Q(t_k+\tau)=Q_k+\int_0^\tau A(s)Q(t_k+s)\,ds. $$毕卡迭代从常值初猜开始，把上一次近似代入积分右侧：
$$ \boxed{ Q^{[0]}(\tau)=Q_k,\qquad Q^{[m+1]}(\tau)=Q_k+\int_0^\tau A(s)Q^{[m]}(s)\,ds } $$这里方括号上标表示迭代次数，不是时间导数。前两次迭代给出：
$$ Q^{[1]}(h)=\left[I_4+\int_0^h A(s)\,ds\right]Q_k, $$ $$ Q^{[2]}(h)= \left[I_4+\int_0^h A(s)\,ds +\int_0^h\int_0^s A(s)A(u)\,du\,ds\right]Q_k. $$要点：时间先后顺序保存在嵌套积分中。一般不能把这些项直接替换成总角增量的普通幂。
2.2 课件的角增量指数公式在什么条件下成立#
定义：
$$ \Delta\boldsymbol\theta=\int_0^h\boldsymbol\omega(t_k+\tau)\,d\tau, \quad d=\|\Delta\boldsymbol\theta\|, \quad \Delta\Theta=\Omega(\Delta\boldsymbol\theta). $$若周期内角速度方向固定，各时刻的 $A$ 彼此可交换，毕卡级数可合并为：
$$ Q_{k+1}=\exp\left(\frac12\Delta\Theta\right)Q_k. $$利用 $\Delta\Theta^2=-d^2I_4$ ，将奇偶次幂分别合并：
$$ \boxed{ Q_{k+1}= \left[I_4\cos\frac d2+\frac{\sin(d/2)}d\Delta\Theta\right]Q_k } $$固定转轴时上式精确；一般变轴转动时，把普通角增量代入上式属于近似。 课件后续引入旋转矢量，正是为了补偿这个近似遗漏的信息。
2.3 一至四阶近似，统一写成一张表#
把更新统一写成：
$$ Q_{k+1}\approx\left[a(d)I_4+b(d)\Delta\Theta\right]Q_k. $$| 对角增量的截断阶次 | $a(d)$ | $b(d)$ |
|---|---|---|
| 一阶 | $1$ | $1/2$ |
| 二阶 | $1-d^2/8$ | $1/2$ |
| 三阶 | $1-d^2/8$ | $1/2-d^2/48$ |
| 四阶 | $1-d^2/8+d^4/384$ | $1/2-d^2/48$ |
这张表代替课件中多次展开的矩阵幂。也可直接计算三角函数；当 $d$ 很小时，使用 $\sin(d/2)/d\approx1/2-d^2/48$ 避免零除。
提高表中的阶次，只减少指数展开的截断误差，不自动补偿转轴变化。
2.4 后面统一使用的“旋转矢量到四元数”工具#
对任意旋转矢量 $\boldsymbol v$ ，定义：
$$ \boxed{ \mathcal Q(\boldsymbol v)= \begin{bmatrix} \cos(\|\boldsymbol v\|/2)\\ \dfrac{\sin(\|\boldsymbol v\|/2)}{\|\boldsymbol v\|}\boldsymbol v \end{bmatrix}, \qquad \mathcal Q(\boldsymbol0)=\begin{bmatrix}1&0&0&0\end{bmatrix}^{\mathsf T} } $$于是，固定转轴时的更新就是：
$$ Q_{k+1}=Q_k\otimes\mathcal Q(\Delta\boldsymbol\theta). $$后续只需把 $\Delta\boldsymbol\theta$ 换成经过补偿的 $\boldsymbol\Phi$ ，同一个工具仍可使用。
3. 初值、规范化和姿态输出：只保留必需公式#
3.1 四元数与姿态矩阵的转换#
后面的初值计算和姿态输出均使用同一个矩阵：
$$ \boxed{ T=C(Q)= \begin{bmatrix} q_0^2+q_1^2-q_2^2-q_3^2&2(q_1q_2-q_0q_3)&2(q_1q_3+q_0q_2)\\ 2(q_1q_2+q_0q_3)&q_0^2-q_1^2+q_2^2-q_3^2&2(q_2q_3-q_0q_1)\\ 2(q_1q_3-q_0q_2)&2(q_2q_3+q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \end{bmatrix} } $$3.2 初值来自初始对准#
若初始对准给出 $T_0=C_b^n(0)$ ，且 $q_0$ 不接近零，可以取：
$$ q_0=\frac12\sqrt{1+T_{11}+T_{22}+T_{33}}, $$ $$ q_1=\frac{T_{32}-T_{23}}{4q_0},\quad q_2=\frac{T_{13}-T_{31}}{4q_0},\quad q_3=\frac{T_{21}-T_{12}}{4q_0}. $$$Q$ 与 $-Q$ 表示相同姿态，所以可选择整体符号。接近 $180^\circ$ 旋转时 $q_0\approx0$ ，应切换到最大分量分支，不能继续除以 $q_0$ 。
如果给出的是课件的 $3\! -\! 1\! -\! 2$ 欧拉角，也可直接组合初始四元数：
$$ Q_0=\mathcal Q(\chi\boldsymbol e_z) \otimes\mathcal Q(\theta\boldsymbol e_x) \otimes\mathcal Q(\gamma\boldsymbol e_y). $$在课件对应的东—北—天参考轴约定下，北偏西航向取 $\chi=\psi_W$ ，北偏东航向取 $\chi=-\psi_E$ 。这一个组合式代替两套重复的四分量三角展开。
3.3 更新后归一化，再输出姿态角#
$$ \boxed{Q\leftarrow\frac{Q}{\|Q\|}}. $$然后由 $T=C(Q)$ 求姿态角。按课件的轴顺序，在 $-\pi/2<\theta<\pi/2$ 内：
$$ \boxed{ \theta=\arcsin T_{32},\quad \gamma=\operatorname{atan2}(-T_{31},T_{33}),\quad \chi=\operatorname{atan2}(-T_{12},T_{22}) } $$输出航向时，再按定义取 $\psi_W=\chi$ 或 $\psi_E=-\chi$ 。当 $\theta=\pm\pi/2$ 时，欧拉角输出出现奇异性，四元数本身仍可用于姿态更新。
归一化恢复单位模约束；它不能修复角运动积分中遗漏的旋转信息。
4. 为什么需要等效旋转矢量#
4.1 总角增量没有完整记录转动顺序#
有限转动一般不交换：
$$ R_x(\alpha)R_y(\beta)\ne R_y(\beta)R_x(\alpha). $$因此，虽然知道：
$$ \Delta\boldsymbol\theta=\int_0^h\boldsymbol\omega(t_k+\tau)\,d\tau, $$一般也不能直接把它当成本周期的净旋转。用轴角表示净旋转：
$$ \boldsymbol\Phi=\phi\boldsymbol u,\qquad \|\boldsymbol u\|=1, $$就得到等效旋转矢量。它只描述起止姿态的关系，不必复现周期内每一个中间姿态。
$$ \boxed{\text{先准确求 }\boldsymbol\Phi,\quad \text{再用 }Q_{k+1}=Q_k\otimes\mathcal Q(\boldsymbol\Phi)\text{ 更新}} $$4.2 课件使用的补偿近似#
旋转矢量微分方程的小角度展开为：
$$ \dot{\boldsymbol\Phi} \approx\boldsymbol\omega +\frac12\boldsymbol\Phi\times\boldsymbol\omega +\frac1{12}\boldsymbol\Phi\times (\boldsymbol\Phi\times\boldsymbol\omega). $$课件先舍去二重叉乘项，再在一次叉乘内用累计角增量替代 $\boldsymbol\Phi$ 。令：
$$ \boldsymbol\alpha(\tau)=\int_0^\tau\boldsymbol\omega(t_k+s)\,ds, $$得到：
$$ \dot{\boldsymbol\Phi}(\tau) \approx\boldsymbol\omega(t_k+\tau) +\frac12\boldsymbol\alpha(\tau)\times\boldsymbol\omega(t_k+\tau). $$从 $\boldsymbol\Phi(0)=0$ 积分到周期末：
$$ \boxed{ \boldsymbol\Phi(h)\approx \underbrace{\boldsymbol\alpha(h)}_{\text{角增量之和}} +\underbrace{\frac12\int_0^h \boldsymbol\alpha(\tau)\times\boldsymbol\omega(t_k+\tau)\,d\tau}_{\text{主要的非交换性补偿}} } $$后面的子样公式都从这个近似积分式出发。 它适合短更新周期、小旋转增量，仍省略了更高阶旋转项。
5. 多子样：怎样从陀螺数据计算旋转矢量#
一个姿态更新周期可包含多个采样子区间。保留各段角增量，才能利用周期内部的变化信息。以下子区间均为等时长，编号按时间先后排列。
5.1 单子样：假定角速度为常值#
$$ \boxed{\boldsymbol\Phi=\Delta\boldsymbol\theta}. $$该模型中转轴固定，叉乘补偿为零。
5.2 双子样：一次角速度模型#
设：
$$ \boldsymbol\omega(t_k+\tau)=\boldsymbol a+2\boldsymbol b\tau. $$则累计角增量为 $\boldsymbol\alpha(\tau)=\boldsymbol a\tau+\boldsymbol b\tau^2$ 。由第 4.2 节的补偿积分：
$$ \frac12\int_0^h\boldsymbol\alpha\times\boldsymbol\omega\,d\tau =\frac16(\boldsymbol a\times\boldsymbol b)h^3. $$两个半周期的角增量满足：
$$ \Delta\boldsymbol\theta_1\times\Delta\boldsymbol\theta_2 =\frac14(\boldsymbol a\times\boldsymbol b)h^3. $$因此：
$$ \boxed{ \boldsymbol\Phi\approx \Delta\boldsymbol\theta_1+\Delta\boldsymbol\theta_2 +\frac23\Delta\boldsymbol\theta_1\times\Delta\boldsymbol\theta_2 } $$其中 $2/3$ 来自上述模型与等时长划分，不是任意两个有限转动复合的通用系数。
5.3 普通三子样：用积分取代逐阶求导#
假定：
$$ \boldsymbol\omega(t_k+\tau) =\boldsymbol a+2\boldsymbol b\tau+3\boldsymbol c\tau^2, \qquad \boldsymbol\alpha(\tau) =\boldsymbol a\tau+\boldsymbol b\tau^2+\boldsymbol c\tau^3. $$直接展开叉乘：
$$ \boldsymbol\alpha\times\boldsymbol\omega =(\boldsymbol a\times\boldsymbol b)\tau^2 +2(\boldsymbol a\times\boldsymbol c)\tau^3 +(\boldsymbol b\times\boldsymbol c)\tau^4. $$积分一次即可得到课件逐阶求导的结果：
$$ \boldsymbol\Phi\approx \boldsymbol a h+\boldsymbol b h^2+\boldsymbol c h^3 +\frac16(\boldsymbol a\times\boldsymbol b)h^3 +\frac14(\boldsymbol a\times\boldsymbol c)h^4 +\frac1{10}(\boldsymbol b\times\boldsymbol c)h^5. $$为了用陀螺数据代替模型参数，记：
$$ \boldsymbol A=\boldsymbol a h,\quad \boldsymbol B=\boldsymbol b h^2,\quad \boldsymbol D=\boldsymbol c h^3. $$对三个子区间积分，有：
$$ \begin{bmatrix} \Delta\boldsymbol\theta_1\ \Delta\boldsymbol\theta_2\ \Delta\boldsymbol\theta_3 \end{bmatrix}#
\begin{bmatrix} 1/3&1/9&1/27\ 1/3&3/9&7/27\ 1/3&5/9&19/27 \end{bmatrix} \begin{bmatrix}\boldsymbol A\\boldsymbol B\\boldsymbol D\end{bmatrix}. $$
上式可逐坐标分量求解，得到：
$$ \begin{aligned} \boldsymbol A&=\frac{11}{2}\Delta\boldsymbol\theta_1 -\frac72\Delta\boldsymbol\theta_2+\Delta\boldsymbol\theta_3,\\ \boldsymbol B&=\frac92\left(-2\Delta\boldsymbol\theta_1 +3\Delta\boldsymbol\theta_2-\Delta\boldsymbol\theta_3\right),\\ \boldsymbol D&=\frac92\left(\Delta\boldsymbol\theta_1 -2\Delta\boldsymbol\theta_2+\Delta\boldsymbol\theta_3\right). \end{aligned} $$代入：
$$ \boldsymbol\Phi\approx \boldsymbol A+\boldsymbol B+\boldsymbol D +\frac16\boldsymbol A\times\boldsymbol B +\frac14\boldsymbol A\times\boldsymbol D +\frac1{10}\boldsymbol B\times\boldsymbol D, $$合并同类叉乘项，得到普通三子样公式：
$$ \boxed{ \begin{aligned} \boldsymbol\Phi\approx{}&\sum_{j=1}^3\Delta\boldsymbol\theta_j\\ &+\frac{33}{80}\Delta\boldsymbol\theta_1\times\Delta\boldsymbol\theta_3\\ &+\frac{57}{80}\Delta\boldsymbol\theta_2\times (\Delta\boldsymbol\theta_3-\Delta\boldsymbol\theta_1). \end{aligned} } $$推导的目的，就是把不可直接测量的模型系数 $\boldsymbol a,\boldsymbol b,\boldsymbol c$ ，全部换成三个可获得的角增量子样。
6. 圆锥运动：为什么还要优化三子样系数#
6.1 多项式模型不一定适合实际振动#
普通三子样公式来自二次角速度模型。实际载体存在周期性振动时，角速度未必符合这个模型，所以还可针对典型振动工况调整补偿系数。
圆锥运动是检验不可交换性误差的重要工况：两个正交方向上的同频、具有适当相位差的角振动，使另一根轴绕平均方向形成圆锥面或近似圆锥面。
6.2 课件的圆锥运动模型#
以 $\beta$ 表示半锥角、$\Omega_c$ 表示圆锥运动角频率，课件模型可统一写为：
$$ Q_c(t)= \begin{bmatrix} \cos(\beta/2)\\ 0\\ \sin(\beta/2)\cos(\Omega_c t)\\ \sin(\beta/2)\sin(\Omega_c t) \end{bmatrix}. $$由 $\dot Q_c=\tfrac12Q_c\otimes\mathcal W$ ，得到：
$$ \mathcal W=2Q_c^*\otimes\dot Q_c, $$ $$ \boxed{ \boldsymbol\omega^b(t)= \begin{bmatrix} -2\Omega_c\sin^2(\beta/2)\\ -\Omega_c\sin\beta\sin(\Omega_c t)\\ \Omega_c\sin\beta\cos(\Omega_c t) \end{bmatrix} } $$一个完整圆锥周期后 $Q_c$ 回到原值，但逐轴积分角速度得到的矢量一般不为零。这正说明了“角速度分量积分”与“净姿态变化”的差别。
其中 $x$ 轴恒定分量是真实角速度的一部分；圆锥误差产生于计算中未正确处理各方向转动的复合。
6.3 普通算法与圆锥优化算法放在同一个结构中比较#
三子样统一写成：
$$ \boldsymbol\Phi\approx \sum_{j=1}^3\Delta\boldsymbol\theta_j +c_{13}\Delta\boldsymbol\theta_1\times\Delta\boldsymbol\theta_3 +c_s\Delta\boldsymbol\theta_2\times (\Delta\boldsymbol\theta_3-\Delta\boldsymbol\theta_1). $$| 公式类型 | 系数依据 | $c_{13}$ | $c_s$ |
|---|---|---|---|
| 普通三子样 | 二次角速度模型与近似补偿积分 | $33/80$ | $57/80$ |
| 圆锥优化三子样 | 课件给出的圆锥工况优化结果 | $9/20$ | $27/40$ |
课件的双子样优化结果仍为第 5.2 节的 $2/3$ 公式。
这里优化的是补偿系数，姿态更新的整体流程没有改变。 针对圆锥模型优化，不代表在所有机动与噪声条件下都全局最优。课件未展开优化系数的完整推导，应将它们作为给定算法结果使用。
7. 完整计算流程：把参考系转动放回去#
前面的相对角速度推导，适合说明离散更新与补偿原理。实际实现还必须明确采用哪一种参考系处理路线。
7.1 路线一：先修正为相对导航系的角运动#
按当前姿态与导航状态计算：
$$ \boldsymbol\omega_{nb}^b =\boldsymbol\omega_{ib}^b-C_n^b\boldsymbol\omega_{in}^n. $$由这一相对角运动形成子样，计算 $\boldsymbol\Phi_{nb}$ ，再更新：
$$ Q_{k+1}=Q_k\otimes\mathcal Q(\boldsymbol\Phi_{nb}). $$参考系修正的精度取决于周期内角速度和坐标变换的积分处理；不能把较长区间内的转动无条件按一个起点值相减。
7.2 路线二：分别计算机体与导航系的惯性旋转#
这对应课件的：
$$ Q_{k+1}=p^*(h)\otimes Q_k\otimes q(h). $$为避免小写 $p,q$ 与坐标系、分量符号混淆，改写为：
$$ \boxed{ Q_{k+1}=\delta Q_n^*\otimes Q_k\otimes\delta Q_b, \quad \delta Q_b=\mathcal Q(\boldsymbol\Phi_b), \quad \delta Q_n=\mathcal Q(\boldsymbol\Phi_n) } $$| 增量 | 输入 | 计算方式 |
|---|---|---|
| 机体侧 $\boldsymbol\Phi_b$ | 陀螺的惯性角增量子样，对应 $\boldsymbol\omega_{ib}^b$ | 使用适当的多子样补偿算法 |
| 导航侧 $\boldsymbol\Phi_n$ | 地球自转与导航系位置速率，对应 $\boldsymbol\omega_{in}^n$ | 短周期内常近似为 $\boldsymbol\omega_{in}^n(t_k)h$ |
同一个多子样结构可用于不同参考关系的角运动，但输入的参考系必须与待求旋转增量一致。双边路线的机体侧使用惯性角增量，不能再提前扣除一次导航系转动。
7.3 一次更新只需完成这些步骤#
- 初始化：由初始对准确定 $Q_0$ 。
- 获取输入：收集一个更新周期内的角增量子样，并确定采用上述哪条参考系处理路线。
- 求净旋转：选择单子样、双子样或三子样算法计算 $\boldsymbol\Phi$ 。
- 更新四元数：由 $\mathcal Q(\boldsymbol\Phi)$ 构造增量，按选定路线相乘。
- 规范化：恢复 $\|Q_{k+1}\|=1$ 。
- 输出：计算 $T=C(Q_{k+1})$ ；需要姿态角时，再按统一的顺序与正方向反算。