028捷联惯导更新算法备忘

1、姿态更新

  对于$n$及$b$系，假定有如下四元数转换关系：

  那么：
$$r^{n(k+1)}=C_{b(k+1)}^{n(k+1)}{r}^{b(k+1)}=C_{n(k)}^{n(k+1)}{C}_{n(k)}^{b(k)}{C}_{b(k)}^{n(k)}{C}_{b(k+1)}^{b(k)}{r}^{b(k+1)}$$

  等价于：
rn(k+1)=Q(tk+1)⊗rb(k+1)⊗Q∗(tk+1)=p∗(h)⊗{Q(tk)⊗[q(h)⊗rb(k+1)⊗q∗(h)]⊗Q∗(tk)}⊗p(h)=[p∗(h)⊗Q(tk)⊗q(h)]⊗rb(k+1)⊗[q∗(h)⊗Q∗(tk)⊗p(h)] \begin{aligned} r^{n(k+1)} &= Q(t_{k+1}) \otimes r^{b(k+1)} \otimes Q^*(t_{k+1})\\ \\ &= p^*(h) \otimes \{ Q(t_k) \otimes [ q(h) \otimes r^{b(k+1)} \otimes q^*(h)] \otimes Q^*(t_k) \} \otimes p(h) \\ \\ &= [p^*(h) \otimes Q(t_k) \otimes q(h)] \otimes r^{b(k+1)} \otimes [q^*(h) \otimes Q^*(t_k) \otimes p(h)] \end{aligned} rn(k+1)​=Q(tk+1​)⊗rb(k+1)⊗Q∗(tk+1​)=p∗(h)⊗{Q(tk​)⊗[q(h)⊗rb(k+1)⊗q∗(h)]⊗Q∗(tk​)}⊗p(h)=[p∗(h)⊗Q(tk​)⊗q(h)]⊗rb(k+1)⊗[q∗(h)⊗Q∗(tk​)⊗p(h)]​

  所以：
$$Q(t_{k+1})=p^{\ast }(h)\otimes Q(t_k)\otimes q(h)$$

  其中：$p^{\ast }(h)$ 由$-{\omega }_{in}^n$确定，$q(h)$由圆锥误差补偿后的等效旋转矢量确定。

等效旋转矢量的求解，就是求解旋转矢量微分方程(Bortz方程)，该方程通过n子样算法求解。为了使旋转矢量准确，还要在子样算法求解旋转矢量中对其进行圆锥运动的不可交换误差补偿。

2、速度更新

  速度更新需要计算三部分的内容，即前一时刻速度、比力引起的速度补偿、有害加速度引起的速度补偿。计算的重点是比力引起的速度补偿。该部分包含加计输出的速度增量、速度的旋转效应补偿以及速度的划桨效应补偿。另外速度的划桨效应补偿还应对其进行优化，方法及系数同等效旋转矢量的不可交换误差补偿优化。最后还要把比力引起的速度补偿从b系转换到n系进行速度的更新。

秦书中的姿态和速度更新没有将$C_{n(k)}^{n(k+1)}$计算在内，而是事后进行了补偿，严讲义中将$C_{n(k)}^{n(k+1)}$直接计算在内。

3、位置更新

略