前言
上周在西安惯性技术学会上完成了关于超高精度SINS算法的报告，由于研究内容比较小众，所以报告的一半时间用来介绍算法研究的意义、超高精度算法和常规算法的关系等内容。
准备ppt的时候，花了不少时间总结传统的SINS算法，毕竟这个发展了50多年的算法目前依然应用在各型捷联惯导系统中。但是也发现了传统算法的一些问题，所以在这里总结一下。倒不是说传统算法有错误或者不好用，而是其良好的工程应用效果，掩盖了一些原理性问题。有的是定义不准确，有的是推导过程细节不严谨等等。
传统SINS姿态算法发展过程
目前使用的捷联惯导姿态算法是以等效旋转矢量微分方程为基础设计的，1971年Bortz提出了等效旋转矢量微分方程，这也是之后多子样算法的数学基础。
算法发展过程如下图：
目前使用的成熟算法就是图中1998年以前的算法，包括圆锥误差、划桨误差、涡卷误差补偿，这一套算法已经可以适应现阶段各型捷联惯导系统，满足导航场景的要求。
目前大家接触的教材和网上的资料都会按照姿态数学表达进行算法分类，常见的是四元数算法和等效旋转矢量算法，这会给人留下，等效旋转矢量更加适合进行姿态更新，且精度更高的印象。
欧拉角更新算法
欧拉角是一种比较直观地表示载体姿态的方法，它将旋转矩阵分解为三个旋转分量，分别对应于绕三个轴的旋转。
\left\{ \begin{aligned}
\dot{\theta} &= \cos\gamma \omega_x + \sin\gamma \omega_z \\
\dot{\gamma} &= \tan\theta\sin\gamma \omega_x + \omega_y - \tan\theta\cos\gamma \omega_z \\
\dot{\psi} &= -\frac{\sin\gamma}{\cos\theta} \omega_x + \frac{\cos\gamma}{\cos\theta} \omega_z
\end{aligned} \right.
欧拉角应当是满足加性更新条件的，所以直接使用Rung-Kutta算法即可得到准确的数值解算结果。
四级四阶Rung-Kutta算法
更新公式：
\left\{\begin{aligned}
k_1 &= f(t_{k-1}, x_{k-1})\\
k_2 &= f(t_{k-1}+\frac{h}{2}, x_{k-1} + k_1\frac{h}{2})\\
k_3 &= f(t_{k-1}+\frac{h}{2}, x_{k-1} + k_2\frac{h}{2})\\
k_4 &= f(t_{k-1}+h, x_{k-1} + k_3h)\\
x_k &= x_{k-1} + \frac{h}{6}\left( k_1+2k_2+2k_3+k_4 \right)
\end{aligned}\right.
用\boldsymbol{\mathcal{E}}=\begin{pmatrix}
\theta \\ \gamma \\ \psi
\end{pmatrix} 表示欧拉角矢量，则欧拉角微分方程可以写成：
\dot{\boldsymbol{\mathcal{E}}} = \boldsymbol{f}(\boldsymbol{\mathcal{E}},t)=\boldsymbol{F}(\boldsymbol{\mathcal{E}}) \boldsymbol{\omega}(t)
在$[0,T]$内的二子样欧拉角RK4数值更新公式为：
\left\{\begin{aligned}
\boldsymbol{k}_1 &= \boldsymbol{F}(\boldsymbol{\mathcal{E}}) \boldsymbol{\omega}(0)\\
\boldsymbol{k}_2 &= \boldsymbol{F}(\boldsymbol{\mathcal{E}}+k_1t_s) \boldsymbol{\omega}(t_s)\\
\boldsymbol{k}_3 &= \boldsymbol{F}(\boldsymbol{\mathcal{E}}+k_2t_s) \boldsymbol{\omega}(t_s)\\
\boldsymbol{k}_4 &= \boldsymbol{F}(\boldsymbol{\mathcal{E}}+k_3\cdot2t_s) \boldsymbol{\omega}(2t_s)\\
\boldsymbol{\mathcal{E}}_k &= \boldsymbol{\mathcal{E}}_{k-1} + \frac{2t_s}{6}\left( \boldsymbol{k}_1+2\boldsymbol{k}_2+2\boldsymbol{k}_3+\boldsymbol{k}_4 \right)
\end{aligned}\right.
式中，
\boldsymbol{\omega}(0) = \frac{3\Delta\boldsymbol{\theta}_1-\Delta\boldsymbol{\theta}_2}{T}
\boldsymbol{\omega}(t_s) = \frac{\Delta\boldsymbol{\theta}_1+\Delta\boldsymbol{\theta}_2}{T}
\boldsymbol{\omega}(2t_s) = \frac{3\Delta\boldsymbol{\theta}_2-\Delta\boldsymbol{\theta}_1}{T}
在圆锥运动下进行数值解算，得到的圆锥轴姿态变化和姿态误差变化如图：
显然采用RK4算法的姿态解算精度高于简单的单步累加精度。精度差异主要来自二子样算法可以得到更加精确的角速度信息。
四元数算法毕卡积分算法
\boldsymbol{\dot{q}} = \frac{1}{2} \boldsymbol{q} \circ \boldsymbol{\omega}
