1. 程式人生 > >028捷聯慣導更新演算法備忘

028捷聯慣導更新演算法備忘

1、姿態更新

  對於 n n b 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 ) 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)}

  等價於:
r n ( k + 1 ) = Q ( t k + 1 ) r b ( k + 1 ) Q ( t k + 1 ) = p ( h ) { Q ( t k ) [ q ( h ) r b ( k + 1 ) q ( h ) ] Q ( t k ) } p ( h ) = [ p ( h ) Q ( t k ) q ( h ) ] r b ( k + 1 ) [ q ( h ) Q ( t k ) 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}

  所以:
Q ( t k + 1 ) = p ( h ) Q ( t k ) q ( h ) Q(t_{k+1}) = p^*(h) \otimes Q(t_k) \otimes q(h)

  其中: p ( h ) p^*(h) ω i n n -\omega_{in}^{n} 確定, q ( h ) q(h) 圓錐誤差補償後的等效旋轉向量確定。

等效旋轉向量的求解,就是求解旋轉向量微分方程(Bortz方程),該方程通過n子樣演算法求解。為了使旋轉向量準確,還要在子樣演算法求解旋轉向量中對其進行圓錐運動的不可交換誤差補償。

2、速度更新

  速度更新需要計算三部分的內容,即前一時刻速度、比力引起的速度補償、有害加速度引起的速度補償。計算的重點是比力引起的速度補償。該部分包含加計輸出的速度增量、速度的旋轉效應補償以及速度的划槳效應補償。另外速度的划槳效應補償還應對其進行優化,方法及係數同等效旋轉向量的不可交換誤差補償優化。最後還要把比力引起的速度補償從b系轉換到n系進行速度的更新。

秦書中的姿態和速度更新沒有將 C n ( k ) n ( k + 1 ) C_{n(k)}^{n(k+1)} 計算在內,而是事後進行了補償,嚴講義中將 C n ( k ) n ( k + 1 ) C_{n(k)}^{n(k+1)} 直接計算在內。

3、位置更新