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)}?q_{n(k?1)}^{e(k?1)}?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)}?q_{n(k?1)}^{e(k?1)}?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)}?q_{b(k?1)}^{n(k?1)}?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{?}_k|\\ ?\frac{sin|0.5{?}_k|}{0.5{?}_k}0.5{?}_k\end{array}\right]$$b系的旋转向量计算方法为：${?}_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}?{\mathbf{X}}_{\mathbf{k}?1}\end{array}$$

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