INS 惯性推算

4 SINS编排

4.1 速度更新

$$v_k^n=v_{k-1}^n+△v_{f,k}^n+△v_{g/cor,k}^n$$

• 计算$△v_{f,k}^n$:
  $$△v_{f,k}^{b(k-1)}=△v_{f,k}^b+\frac{1}{2}△{\theta }_k\times △v_{f,k}^b+\frac{1}{12}(△{\theta }_{k-1}\times △v_{f,k}^b+△v_{f,k-1}^b\times △{\theta }_k)$$ $$△v_{f,k}^n=[I-(0.5{\zeta }_k\times )]C_{b(k-1)}^{n(k-1)}△v_{f,k}^{b(k-1)}$$ 使用中点时刻的速度和位置信息 更新${\omega }_{in}^n$，得到 0.5${\zeta }_k$:
  $${\zeta }_k=[{\omega }_{ie}^n+{\omega }_{en}^n{]}_{k-1/2}△t_k$$其中${\zeta }_k$为坐标框架$n(k)$相对$n(k-1)$的旋转向量；$k-1/2$表示区间$[t_{k-1},t_k]$中值；时刻$t_k$的位置和速度未知，为了计算中点时刻的速度和位置，对其进行外推：
  ​ 高程外推
  ：$h_{k-1/2}=h_{k-1}-\frac{v_{D,k-1}△t_k}{2}$
  经纬度外推
  可以通过如下方求得：$q_{n(k-1/2)}^{e(k-1/2)}=q_{e(k-1)}^{e(k-1/2)}\ast q_{n(k-1)}^{e(k-1)}\ast q_{n(k-1/2)}^{n(k-1)}$其中，
  $$\begin{gathered} q_{n(k-1/2)}^{n(k-1)}=\left[\begin{array}{c}cos|0.5{\zeta }_{k-1/2}|\\ -\frac{sin|0.5{\zeta }_{k-1/2}|}{0.5{\zeta }_{k-1/2}}0.5{\zeta }_{k-1/2}\end{array}\right] \\ {q}_{e(k-1)}^{e(k-1/2)}=\left[\begin{array}{c}cos|0.5{ϵ}_{k-1/2}|\\ -\frac{sin|0.5{ϵ}_{k-1/2}|}{0.5{ϵ}_{k-1/2}}0.5{ϵ}_{k-1/2}\end{array}\right] \end{gathered}$$ 则${\zeta }_{k-1/2}=[{\omega }_{ie}^n+{\omega }_{en}^n{]}_{k-1}\frac{△t_k}{2}$和${ϵ}_{k-1/2}={\omega }_{ie}^e\frac{△t_k}{2}$。最后单位化四元数$q_{n(k-1/2)}^{e(k-1/2)}$并得到对应的方向余弦矩阵，根据方向余弦矩阵求得此刻的经纬度值。
  速度外推
  $v_{k-1/2}^n=v_{k-1}^n+\frac{△v_{k-1}^n}{2}$
• 计算$△v_{g/cor,k}^n$:
  $△v_{g/cor,k}^n=[g^n-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n{]}_{k-1/2}△t_k$：

4.2 位置更新

n系到e系的四元数$q_n^e$包含经纬度信息,四元数更新如下：
$$q_{n(k)}^{e(k)}=q_{e(k-1)}^{e(k)}\cdot q_{n(k-1)}^{e(k-1)}\cdot q_{n(k)}^{n(k-1)}$$其中，$$\begin{gathered} q_{n(k)}^{n(k-1)}=\left[\begin{array}{c}cos|0.5{\zeta }_k|\\ -\frac{sin|0.5{\zeta }_k|}{0.5{\zeta }_k}0.5{\zeta }_k\end{array}\right] \\ {q}_{e(k-1)}^{e(k)}=\left[\begin{array}{c}cos|0.5{ϵ}_k|\\ -\frac{sin|0.5{ϵ}_k|}{0.5{ϵ}_k}0.5{ϵ}_k\end{array}\right] \end{gathered}$$则
${ϵ}_k={\omega }_{ie}^e△t_k$:表示e(k)相对于e(k-1)的旋转向量。
${\zeta }_{k-1/2}=[{\omega }_{ie}^n+{\omega }_{en}^n{]}_{k-1}\frac{△t_k}{2}$：这个量的计算需要位置和速度信息，首先根据更新后的速度，再次更新中点时刻的速度$v_{k-1/2}^n=\frac{1}{2}(v_k^n+v_{k-1}^n)$
经纬度外推
单位化四元数$q_{n(k)}^{e(k)}$并得到对应的方向余弦矩阵，根据方向余弦矩阵求得此刻的经纬度值。
高程外推
$h_k=h_{k-1}-v_{D,k-1/2}△t_k$

4.3 姿态更新

b系到n系的姿态$q_b^n$更新算法：
$$q_{b(k)}^{n(k)}=q_{n(k-1)}^{n(k)}\cdot q_{b(k-1)}^{n(k-1)}\cdot q_{b(k)}^{b(k-1)}$$
b系四元数$q_{b(k)}^{b(k-1)}$更新如下：
$$q_{b(k)}^{b(k-1)}=\left[\begin{array}{c}cos|0.5{\varphi }_k|\\ -\frac{sin|0.5{\varphi }_k|}{0.5{\varphi }_k}0.5{\varphi }_k\end{array}\right]$$b系的旋转向量计算方法为：${\varphi }_k=△{\theta }_k+\frac{1}{12}△{\theta }_{k-1}\times △{\theta }_k$
n系四元数$q_{n(k-1)}^{n(k)}$更新如下：$$q_{n(k-1)}^{n(k)}=\left[\begin{array}{c}cos|0.5{\zeta }_k|\\ -\frac{sin|0.5{\zeta }_k|}{0.5{\zeta }_k}0.5{\zeta }_k\end{array}\right]$$基于上边更新的位置信息，更新中点的位置，进一步更新${\zeta }_k$。
最后，单位化四元数$q_{b(k)}^{n(k)}$，并得到对应的方向余弦矩阵，再有方向余弦阵计算欧拉角

$$\begin{array}{r}{\mathbf{X}}_{k/k-1}={\mathbf{F}}_{k/k-1}\ast {\mathbf{X}}_{\mathbf{k}-1}\end{array}$$

$$\begin{array}{rl}{\mathbf{P}}_{k/k-1}& ={\mathbf{F}}_{k/k-1}\ast {\mathbf{P}}_{\mathbf{k}-1}\ast {\mathbf{F}}_{k/k-1}^{\mathbf{T}}+0.5\ast ({\mathbf{F}}_{k/k-1}\ast {\mathbf{G}}_{k/k-1}\ast {\mathbf{Q}}_{\mathbf{k}-1}{\mathbf{G}}_{k/k-1}^{\mathbf{T}}\\ & \ast {\mathbf{F}}_{k/k-1}^{\mathbf{T}}+{\mathbf{G}}_{k/k-1}\ast {\mathbf{Q}}_{\mathbf{k}-1}\ast {\mathbf{G}}_{k/k-1}^{\mathbf{T}})\ast \Delta t_k\end{array}$$