我對 Error-State Kalman Filter 的理解。本文的主要參考文獻是 Joan Sola 的 《Quaternion kinematics for the error-state Kalman filter》[1]，當然是這本小冊子，做 VIO 的人都會熟讀這本冊子。
1. ErKF 與 EFK 的區別
關於 Error-State Kalman Filter (ErKF) 與 Extended Kalman Filter (EKF) 之間的聯系與區別可以參考論文 [2]，論文 [2] 可以幫助理解 Error-State Kalman Filter，現在引用其中的一些文字以說明 EKF 與 ErKF 之間的區別。
論文 [2] 的 Abstract：
EKFs operate by linearizing the nonlinear model around the current reference trajectory and then designing the Kalman filter gain for the linearized model. Recently, an alternative approach has emerged for a certain class of problems where the error in the states is estimated using a Kalman filter, rather than the state itself.
EKF 直接估計狀態值，而 ErKF 估計的是狀態值的誤差部分。
論文 [2] Remark II.6. 的最后一句：
Thus, this matrix is a linearized matrix,unlike in the ErKF case, which is a linear matrix.
上文中的 this matrix 是指 "the system matrix, \(\mathbf{F}(t)\), in EKF formulation"。ErKF 的 system matrix 在論文 [2] 中的
論文 [2] 中 EKF 與 ErKF 的 system matrix \(\mathbf{F}(t)\) 分別是公式 (11) 與 (18)。
1.1. EKF 的近似
EKF 的 \(\mathbf{F}(t)\) 用於對 States 的 Covariance 估計，可以將公式 (10) (11) (12) 結合一起看。
在 \(t\) 時刻 States 的 Covariance 是 \(\mathbf{P}(t) = \mathbf{E}[(\mathbf{x}(t) - \hat{\mathbf{x}}(t))(\mathbf{x}(t) - \hat{\mathbf{x}}(t))^T]\)，其中 \(\mathbf{x}(t)\) 是 States 真值，\(\hat{\mathbf{x}}(t)\) 是系統對 States 的的估計值。經過時間 \(\Delta t\) 到達下一個時刻 \(t + \Delta t\)，此時 Covariance 可以如下表示。
\[\begin{align} \mathbf{P}(t + \Delta t) = \mathbf{E}[(\mathbf{x}(t + \Delta t) - \hat{\mathbf{x}}(t + \Delta t))(\mathbf{x}(t + \Delta t) - \hat{\mathbf{x}}(t + \Delta t))^T] \end{align}\]
但是在沒有觀測之前，我們尚未獲得系統在 \(t + \Delta t\) 時刻的結果，即估計值 \(\hat{\mathbf{x}}(t + \Delta t)\)。考慮在 \(t + \Delta t\) 時刻沒有觀測之前的系統 Covariance \(\mathbf{P}^-(t + \Delta t)\)。需要將上面公式中的 \(\hat{\mathbf{x}}(t + \Delta t)\) 替換為 \(\hat{\mathbf{x}}^-(t + \Delta t)\)，表示是用 \(\hat{\mathbf{x}}(t)\) 經過運動方程計算得到的 \(t + \Delta t\) 時刻的 Predict 值（之后還需要經過 Update 過程修正）。
\[\begin{align} \hat{\mathbf{x}}^-(t + \Delta t) &= \hat{\mathbf{x}}(t) + \dot{\hat{\mathbf{x}}}(t) \Delta t \notag \\ &= \hat{\mathbf{x}}(t) + (\mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) + \mathbf{\Gamma}\mathbf{w}(t)) \Delta t \\ \mathbf{P}^-(t + \Delta t) &= \mathbf{E}[(\mathbf{x}(t + \Delta t) - \hat{\mathbf{x}}^-(t + \Delta t))(\mathbf{x}(t + \Delta t) - \hat{\mathbf{x}}^-(t + \Delta t))^T] \end{align}\]
對於 \(t + \Delta t\) 時刻的真值，系統是無從得知的，使用 \(t\) 時刻的真值與運動方程估計。
\[\begin{align} \mathbf{x}^-(t + \Delta t) &= \mathbf{x}(t) + \dot{\mathbf{x}}(t) \Delta t \notag \\ &= \mathbf{x}(t) + [\mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)) + \mathbf{\Gamma}\mathbf{w}(t)] \Delta t \notag \\ &\simeq \mathbf{x}(t) + \left[ \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) + {\partial \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \over \partial \mathbf{x}(t)}(\mathbf{x}(t) - \hat{\mathbf{x}}(t)) + \mathbf{\Gamma}\mathbf{w}(t)\right] \Delta t （泰勒一階展開近似）\notag \\ &= \mathbf{x}(t) + \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \Delta t + \mathbf{F}(t) (\mathbf{x}(t) - \hat{\mathbf{x}}(t)) \Delta t + \mathbf{\Gamma}\mathbf{w}(t) \Delta t \end{align}\]
計算 \(\mathbf{P}^-(t + \Delta t)\)。
\[\begin{align} \mathbf{P}^-(t + \Delta t) &= \mathbf{E}[(\mathbf{x}^-(t + \Delta t) - \hat{\mathbf{x}}^-(t + \Delta t))(\mathbf{x}^-(t + \Delta t) - \hat{\mathbf{x}}^-(t + \Delta t))^T] \notag \\ &\simeq \mathbf{E}[(\mathbf{x}(t) + \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \Delta t + \mathbf{F}(t) (\mathbf{x}(t) - \hat{\mathbf{x}}(t)) \Delta t + \mathbf{\Gamma}\mathbf{w}(t) \Delta t - \hat{\mathbf{x}}(t) - \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \Delta t) \notag \\ &\phantom{=}(\mathbf{x}(t) + \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \Delta t + \mathbf{F}(t) (\mathbf{x}(t) - \hat{\mathbf{x}}(t)) \Delta t + \mathbf{\Gamma}\mathbf{w}(t) \Delta t - \hat{\mathbf{x}}(t) - \mathbf{f}(\hat{\mathbf{x}}(t), \mathbf{u}(t)) \Delta t)^T ] \notag \\ &= \mathbf{E}[((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t))) ((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t)))^T] \notag \\ &\phantom{=} \mathbf{E}[\mathbf{\Gamma}\mathbf{w}(t)((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t)))^T] + \mathbf{E}[((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t)))\mathbf{w}(t)^T\mathbf{\Gamma}^T] \notag \\ &\phantom{=} + \mathbf{E}[\mathbf{\Gamma}\mathbf{w}(t)\mathbf{w}(t)^T\mathbf{\Gamma}^T]\notag \\ &= (\mathbf{I} + \mathbf{F}(t)\Delta t) \mathbf{E}[(\mathbf{x}(t) - \hat{\mathbf{x}}(t)) (\mathbf{x}(t) - \hat{\mathbf{x}}(t))^T] (\mathbf{I} + \mathbf{F}(t)\Delta t)^T \notag \\ &\phantom{=} \mathbf{\Gamma}\mathbf{E}[\mathbf{w}(t)]\mathbf{E}[((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t)))^T] + \mathbf{E}[((\mathbf{I} + \mathbf{F}(t)\Delta t)(\mathbf{x}(t) - \hat{\mathbf{x}}(t)))]\mathbf{E}[\mathbf{w}(t)^T]\mathbf{\Gamma}^T \notag \\ &\phantom{=} + \mathbf{\Gamma}\mathbf{E}[\mathbf{w}(t)\mathbf{w}(t)^T]\mathbf{\Gamma}^T \notag \\ &= (\mathbf{I} + \mathbf{F}(t)\Delta t) \mathbf{P}(t) (\mathbf{I} + \mathbf{F}(t)\Delta t)^T + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \phantom{=} (\text{We\ have\ } \mathbf{E}[\mathbf{w}(t)] = 0)\notag \\ &= \mathbf{P}(t) + \mathbf{P}(t)\mathbf{F}(t)^T\Delta t + \mathbf{F}(t) \mathbf{P}(t) \Delta t + \mathbf{F}(t) \mathbf{P}(t)\mathbf{F}(t)^T\Delta t^2 + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \notag \\ &{\simeq} \mathbf{P}(t) + \mathbf{P}(t)\mathbf{F}(t)^T\Delta t + \mathbf{F}(t) \mathbf{P}(t) \Delta t + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \phantom{=} (\Delta t^2 \text{\ too\ small, ignore second order and higher}) \end{align}\]
所以得到 \(\mathbf{P}(t)\) 的微分方程。
\[\begin{align} \dot{\mathbf{P}}(t) &= \mathbf{P}(t)\mathbf{F}(t)^T + \mathbf{F}(t) \mathbf{P}(t) + \mathbf{\Gamma}\mathbf{Q}\mathbf{\Gamma}^T \end{align}\]
與公式 (11) 一致。所以，得到關於 EKF 近似的結論，EKF 在 Predict 步驟中使用上一時刻對 States 的估計值 \(\hat{\mathbf{x}}(t)\)，對運動方程 \(\mathbf{f}\) 使用一階泰勒展開近似下一時刻 States 的真值 \(\mathbf{x}(t + \Delta t)\)。從而估計 Predict 步驟得到的 States Covariance。
1.2. ErKF 無近似
ErKF 估計的是 States 的誤差部分。
在 Predict 步驟的操作如下。
省略了控制值與誤差之后的微分方程如下。（論文中的符號混亂，所以我重新整理。）
\[\begin{align} \dot{\mathbf{x}}(t) &= \mathbf{f}(\mathbf{x}(t), \mathbf{u}(t)) + \mathbf{\Gamma}\mathbf{w}(t) \end{align}\]
將真值分解成為 nominal 和 error 兩個部分。nomianl 部分使用 \(\hat{\mathbf{x}}(t)\) 表示，error 部分使用 \(\delta \mathbf{x}(t)\) 表示。真值 \(\mathbf{x}(t) = \hat{\mathbf{x}}(t) + \delta \mathbf{x}(t)\) 。代入運動（微分）方程。
\[\begin{align} \dot{\hat{\mathbf{x}}}(t) + \delta\dot{\mathbf{x}}(t) &= \mathbf{f}(\hat{\mathbf{x}}(t)+\delta\mathbf{x}(t),\mathbf{u}(t)) + \mathbf{\Gamma}\mathbf{w}(t) \notag \\ &= \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) + {\partial \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) \over \partial \mathbf{x}(t)} \delta\mathbf{x}(t) \notag \\ &\phantom{=} + {1 \over 2!}{\partial^2 \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) \over \partial \mathbf{x}(t)^2} \delta\mathbf{x}(t)^2 + \dots + \mathbf{\Gamma}\mathbf{w}(t) \notag \\ &= \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) + \mathbf{F}(t) \delta\mathbf{x}(t) + O(\delta\mathbf{x}(t)^2) + \mathbf{\Gamma}\mathbf{w}(t) \\ \mathbf{F}(t) &= {\partial \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) \over \partial \mathbf{x}(t)} \\ \dot{\hat{\mathbf{x}}}(t) &= \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) \phantom{=}(定義) \\ \delta\dot{\mathbf{x}}(t) &= \mathbf{F}(t) \delta\mathbf{x}(t) + O(\delta\mathbf{x}(t)^2) + \mathbf{\Gamma}\mathbf{w}(t) \end{align}\]
由定義可以得到時間 \(\Delta t\) 之后時刻 \(t + \Delta t\) 的 \(\delta\mathbf{x}(t + \Delta t)\)。
\[\begin{align} \delta\mathbf{x}(t + \Delta t) &= \delta\mathbf{x}(t) + \delta\dot{\mathbf{x}}(t) \Delta t \notag \\ &= \delta\mathbf{x}(t) + \mathbf{F}(t) \delta\mathbf{x}(t)\Delta t \notag \\ &\phantom{=} + O(\delta\mathbf{x}(t)^2) \Delta t + \mathbf{\Gamma}\mathbf{w}(t)\Delta t \notag \\ &= (\mathbf{I} + \mathbf{F}(t)\Delta t)\delta\mathbf{x}(t) \notag \\ &\phantom{=} + O(\delta\mathbf{x}(t)^2) \Delta t + \mathbf{\Gamma}\mathbf{w}(t)\Delta t \notag \\ &= \mathbf{\Phi}(t)\delta\mathbf{x}(t) + O(\delta\mathbf{x}(t)^2) \Delta t + \mathbf{\Gamma}\mathbf{w}(t)\Delta t \end{align}\]
在 ErKF 中 \(\mathbf{\Phi}(t)\) 被稱作 State Transition Matrix。ErKF 的 Predict 步驟做近似 \(\delta\mathbf{x}(t + \Delta t) \simeq \mathbf{\Phi}(t)\delta\mathbf{x}(t)\)，剩下的 \(O(\delta\mathbf{x}(t)^2) \Delta t + \mathbf{\Gamma}\mathbf{w}(t)\Delta t\) 在 Update 步驟用觀測方程進行估計。
從 \(\mathbf{F}(t)\) 的定義中可以看到，其與 error \(\delta\mathbf{x}(t)\) 是無關的，使用上一步的估計值 \(\hat{\mathbf{x}}(t)\) 與這一步的控制值 \(\mathbf{u}(t)\) 即可計算得到。於是，在 Predict 步驟無需設計 error 部分 \(\delta\mathbf{x}(t)\) 的 error，對 \(\mathbf{\Phi}(t)\) 進行近似。
所以，可以得到結論，ErKF 的 Predict 步驟是一個線性的步驟。存在的近似 \(O(\delta\mathbf{x}(t)^2) \Delta t + \mathbf{\Gamma}\mathbf{w}(t)\Delta t\) 可以在 Update 步驟估計出來。
2. 應用 ErKF 至 VIO
參考 [1] 進行此部分的推導。從博客 《Rotation Kinematics》 的 Quaternion 微分方程開始。
事先聲明，我使用 Hamilton Quaternion。
先定義系統的狀態向量。
\[\begin{align} \mathbf{x} &= \begin{bmatrix} {}^G\mathbf{q}^T_I & \mathbf{b}_g^T & {}^G\mathbf{v}_I^T & \mathbf{b}_a^T & {}^G\mathbf{p}_I^T | {}^G\mathbf{f}_1^T & \dots & {}^G\mathbf{f}_N^T \end{bmatrix}^T \\ &= \begin{bmatrix} \mathbf{x}_s^T & | & \mathbf{x}_f^T \end{bmatrix}^T \end{align}\]
\(I\) 指 IMU 坐標系，在常規情況下 VIO 系統的 Body 坐標系與 IMU 坐標系重合，\(G\) 指 Global 坐標系，即 Inertial Frame。各個狀態值的含義如下：
- \({}^G\mathbf{q}_I\) 將點在 \(I\) 坐標系下的坐標轉換到 \(G\) 坐標系下的坐標所使用的四元數；
- \(\mathbf{b}_g\) 陀螺儀測量值的 bias；
- \({}^G\mathbf{v}_I\) 在 \(I\) 坐標系原點在 \(G\) 坐標系下坐標的速度，即 \({}^G\dot{\mathbf{p}}_I\)；
- \(\mathbf{b}_a\) 加速度計測量值的 bias；
- \({}^G\mathbf{p}_I\) 將點在 \(I\) 坐標系下的坐標轉換到 \(G\) 坐標系下的坐標所使用的偏移，即 \(I\) 坐標系原點在 \(G\) 坐標系下的坐標；
- \({}^G\mathbf{f}_1^T\) 是指地標點 1 在 \(G\) 坐標系下的坐標。
有 Transform 過程。
\[\begin{align} {}^G\mathbf{f}_i &= {}^G\mathbf{q}_I{}^I\mathbf{f}_i + {}^G\mathbf{p}_I, &i = 1, \dots, N \end{align}\]
系統的運動（微分）方程，可以看做是在 IMU 測量值的線加速度、角速度作用下，系統以上的狀態值發生變化。
博客 《Rotation Kinematics》 給出的 Quatenrion 微分方程如下。
\[\begin{align} {}^G\dot{\mathbf{q}}_I &= {1 \over 2} {}^G\mathbf{q}_I \otimes \mathbf{\omega}^{IG}_I(t) \end{align}\]
系統中所有狀態值（真值）的微分方程寫成如下形式。
\[\begin{align} {}^G\dot{\mathbf{q}}_I(t) &= {1 \over 2} {}^G\mathbf{q}_I(t) \otimes \mathbf{\omega}^{IG}_I(t) \\ \dot{\mathbf{b}}_g &= \mathbf{n}_{wg}(t) \\ {}^G\dot{\mathbf{v}}_I(t) &= {}^G\mathbf{a}(t) \\ \dot{\mathbf{b}}_a(t) &= \mathbf{n}_{wa}(t) \\ {}^G\dot{\mathbf{p}}_I(t) &= {}^G\mathbf{v}_I(t) \\ {}^G\dot{\mathbf{f}}_i(t) &= \mathbf{0}_{3 \times 1}, \phantom{\ } \phantom{\ } \phantom{\ } i = 1, \dots, N \end{align}\]
系統的輸入值角速度與加速的測量值存在誤差，會將誤差引入到運動方程中。依次將狀態值的真值划分成兩個部分—— nominal 與 error 。
2.1. 運動微分方程的 nominal 與 error
角速度與加速度的測量值與真值的關系如下。
\[\begin{align} \mathbf{\omega}_m(t) &= \mathbf{\omega}(t) + \mathbf{b}_g(t) + \mathbf{n}_g(t) \\ \mathbf{a}_m(t) &= \mathbf{C}({}^I\mathbf{q}_G(t))({}^G\mathbf{a}(t) - {}^G\mathbf{g}) + \mathbf{b}_a(t) + \mathbf{n}_a(t) \end{align}\]
將角速度與加速度的真值區分為 nominal 與 error 兩個部分。nominal 部分用上標 \(\hat{\cdot}\) 表示，error 用上標 \(\tilde{\cdot}\) 表示。
\[\begin{align} \mathbf{\omega}(t) &= (\mathbf{\omega}_m(t) - \mathbf{b}_g(t)) - \mathbf{n}_g(t) \notag \\ &= (\mathbf{\omega}_m(t) - \hat{\mathbf{b}}_g(t) + \tilde{\mathbf{b}}_g(t)) - \mathbf{n}_g(t) \notag \\ &= (\mathbf{\omega}_m(t) - \hat{\mathbf{b}}_g(t)) - (\mathbf{n}_g(t) - \tilde{\mathbf{b}}_g(t)) \notag \\ &= \hat{\mathbf{\omega}}(t) - \tilde{\mathbf{\omega}}(t) \\ {}^G\mathbf{a}(t) &= \mathbf{C}^T({}^G\mathbf{q}_I(t))\left[ (\mathbf{a}_m(t) - \hat{\mathbf{b}}_a(t)) - (\mathbf{n}_a(t) - \tilde{\mathbf{b}}_a(t)) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T({}^G\mathbf{q}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\tilde{\mathbf{q}}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t) \otimes \begin{bmatrix} 1 \\ {1 \over 2}\delta\mathbf{\theta}(t) \end{bmatrix}\right)\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T\left(\mathbf{C}^{-1}\left(\mathbf{C}\left({}^G\hat{\mathbf{q}}_I(t) \otimes \begin{bmatrix} 1 \\ {1 \over 2}\delta\mathbf{\theta}(t) \end{bmatrix}\right)\right)\right)\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T\left(\mathbf{C}^{-1}\left(\mathbf{C}[{}^G\hat{\mathbf{q}}_I(t)] \text{Exp}[\delta\mathbf{\theta}(t)]\right)\right)\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \{\mathbf{C}\left(\mathbf{C}^{-1}\left(\mathbf{C}[{}^G\hat{\mathbf{q}}_I(t)] \text{Exp}[\delta\mathbf{\theta}(t)]\right)\right)\}^T\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \{\mathbf{C}[{}^G\hat{\mathbf{q}}_I(t)] \text{Exp}[\delta\mathbf{\theta}(t)]\}^T\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \text{Exp}[-\delta\mathbf{\theta}(t)]\mathbf{C}^T[{}^G\hat{\mathbf{q}}_I(t)]\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= [\mathbf{I}-\delta\mathbf{\theta}(t)^{\wedge}]\mathbf{C}^T[{}^G\hat{\mathbf{q}}_I(t)]\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right]-\delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \end{align}\]
代入狀態值的真值中，將所有狀態值的 nominal 與 error 分離，分別寫出它們的微分方程。
先做較為復雜的姿態四元數部分。定義四元數真值與其 nominal、error 的關系如下。
\[\begin{align} {}^G\mathbf{q}_I(t) &= {}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) \\ {}^G\tilde{\mathbf{q}}_I(t) &= \begin{bmatrix} 1 \\ {1 \over 2}\delta\mathbf{\theta}(t) \end{bmatrix} \end{align}\]
即四元數的 error 部分對應的是一個小角度的旋轉，可以用軸角 \(\delta\mathbf{\theta}(t)\) 表示。
直接定義四元數 nominal 部分的微分方程為 \({}^G\dot{\hat{\mathbf{q}}}_I(t) = {1 \over 2} {}^G\hat{\mathbf{q}}_I(t) \otimes \hat{\mathbf{\omega}}(t)\)。
下面的這個公式推導參考論文 [1] 的公式 (259) 附近的推導。
\[\begin{align} {}^G\dot{\mathbf{q}}_I(t) &= {}^G\dot{\hat{\mathbf{q}}}_I(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) + {}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\dot{\tilde{\mathbf{q}}}_I(t) \notag \\ {1 \over 2} {}^G\mathbf{q}_I(t) \otimes \mathbf{\omega}(t) &= {}^G\dot{\hat{\mathbf{q}}}_I(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) + {}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\dot{\tilde{\mathbf{q}}}_I(t) \notag \\ {1 \over 2} {}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) \otimes \mathbf{\omega}(t) &= {1 \over 2} {}^G\hat{\mathbf{q}}_I(t) \otimes \hat{\mathbf{\omega}}(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) + {}^G\hat{\mathbf{q}}_I(t) \otimes {}^G\dot{\tilde{\mathbf{q}}}_I(t) \notag \\ {1 \over 2} {}^G\tilde{\mathbf{q}}_I(t) \otimes \mathbf{\omega}(t) &= {1 \over 2} \hat{\mathbf{\omega}}(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) + {}^G\dot{\tilde{\mathbf{q}}}_I(t) \notag \\ 2 {}^G\dot{\tilde{\mathbf{q}}}_I(t) &= {}^G\tilde{\mathbf{q}}_I(t) \otimes \mathbf{\omega}(t) - \hat{\mathbf{\omega}}(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) \notag \\ 2 \begin{bmatrix} 0 \\ {1 \over 2}\dot{\delta\mathbf{\theta}}(t) \end{bmatrix} &= {}^G\tilde{\mathbf{q}}_I(t) \otimes \mathbf{\omega}(t) - \hat{\mathbf{\omega}}(t) \otimes {}^G\tilde{\mathbf{q}}_I(t) \notag \\ \begin{bmatrix} 0 \\ \dot{\delta\mathbf{\theta}}(t) \end{bmatrix} &= [\mathbf{\omega}(t)]_R {}^G\tilde{\mathbf{q}}_I(t) - [\hat{\mathbf{\omega}}(t)]_L {}^G\tilde{\mathbf{q}}_I(t) \notag \\ &= \begin{bmatrix} 0 & -\mathbf{\omega}(t)^T \\ \mathbf{\omega}(t) & -\mathbf{\omega}(t)^{\wedge} \end{bmatrix} {}^G\tilde{\mathbf{q}}_I(t) - \begin{bmatrix} 0 & -\hat{\mathbf{\omega}}(t)^T \\ \hat{\mathbf{\omega}}(t) & \hat{\mathbf{\omega}}(t)^{\wedge} \end{bmatrix} {}^G\tilde{\mathbf{q}}_I(t) \notag \\ &= \begin{bmatrix} 0 & -(\mathbf{\omega}(t)-\hat{\mathbf{\omega}}(t))^T \\ \mathbf{\omega}(t)-\hat{\mathbf{\omega}}(t) & -(\mathbf{\omega}(t)+\hat{\mathbf{\omega}}(t))^{\wedge} \end{bmatrix} \begin{bmatrix} 1 \\ {1 \over 2}\delta\mathbf{\theta}(t) \end{bmatrix} \end{align}\]
所以可以得四元數 error 部分的微分方程。
\[\begin{align} \dot{\delta\mathbf{\theta}}(t) &= \mathbf{\omega}(t)-\hat{\mathbf{\omega}}(t) - {1 \over 2}(\mathbf{\omega}(t)+\hat{\mathbf{\omega}}(t))^{\wedge} \delta\mathbf{\theta}(t) \notag \\ &= -\tilde{\mathbf{\omega}}(t) - {1 \over 2}(2\hat{\mathbf{\omega}}(t) - \tilde{\mathbf{\omega}}(t))^{\wedge} \delta\mathbf{\theta}(t) \notag \\ &= -\tilde{\mathbf{\omega}}(t) - \hat{\mathbf{\omega}}(t)^{\wedge}\delta\mathbf{\theta}(t) + {1 \over 2}\tilde{\mathbf{\omega}}(t)^{\wedge}\delta\mathbf{\theta}(t) \notag \\ &\simeq -\hat{\mathbf{\omega}}(t)^{\wedge}\delta\mathbf{\theta}(t) - \tilde{\mathbf{\omega}}(t) \phantom{=}(二階小量近似 0) \end{align}\]
其他狀態值的 nominal 與 error 分離如下。
\[\begin{align} \dot{\mathbf{b}}_g(t) &= \mathbf{n}_{wg}(t) \notag \\ &= \mathbf{0}_3 - (-\mathbf{n}_{wg}(t)) \notag \\ &= \dot{\hat{\mathbf{b}}}_g(t) - \dot{\tilde{\mathbf{b}}}_g(t) \\ {}^G\dot{\mathbf{v}}_I(t) &= {}^G\mathbf{a}(t) \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right]-\delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\left[ \hat{\mathbf{a}}(t) - \tilde{\mathbf{a}}(t) \right] + {}^G\mathbf{g} \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) + {}^G\mathbf{g} - \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) - \delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) - \delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) \notag \\ &\simeq (\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) + {}^G\mathbf{g}) - \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) - \delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) \notag \\ &= (\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) + {}^G\mathbf{g}) - [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) - \delta\mathbf{\theta}(t)^{\wedge}\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)] \notag \\ &= (\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t) + {}^G\mathbf{g}) - \{\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) + [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)]^{\wedge}\delta\mathbf{\theta}(t)\} \notag \\ &= {}^G\dot{\hat{\mathbf{v}}}_I(t) - {}^G\dot{\tilde{\mathbf{v}}}_I(t) \\ \dot{\mathbf{b}}_a(t) &= \mathbf{n}_{wa}(t) \notag \\ &= \mathbf{0}_3 - (-\mathbf{n}_{wa}(t)) \notag \\ &= \dot{\hat{\mathbf{b}}}_a(t) - \dot{\tilde{\mathbf{b}}}_a(t) \\ {}^G\dot{\mathbf{p}}_I(t) &= {}^G\mathbf{v}_I(t) \notag \\ &= {}^G\hat{\mathbf{v}}_I(t) - {}^G\tilde{\mathbf{v}}_I(t) \notag \\ &= {}^G\dot{\hat{\mathbf{p}}}_I(t) - {}^G\dot{\tilde{\mathbf{p}}}_I(t) \\ {}^G\dot{\mathbf{f}}_i(t) &= \mathbf{0}_{3 \times 1}, \phantom{\ } \phantom{\ } \phantom{\ } i = 1, \dots, N \end{align}\]
整理。
nominal 部分有以下運動微分方程。
\[\begin{align} {}^G\dot{\hat{\mathbf{q}}}_I(t) &= {1 \over 2} {}^G\hat{\mathbf{q}}_I(t) \otimes \hat{\mathbf{\omega}}(t) \\ \dot{\hat{\mathbf{b}}}_g(t) &= \mathbf{0}_3 \\ {}^G\dot{\hat{\mathbf{v}}}_I(t) &= \mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t) + {}^G\mathbf{g} \\ \dot{\hat{\mathbf{b}}}_a(t) &= \mathbf{0}_3 \\ {}^G\dot{\hat{\mathbf{p}}}_I(t) &= {}^G\hat{\mathbf{v}}_I(t) \\ {}^G\dot{\hat{\mathbf{f}}}_i(t) &= \mathbf{0}_3, \phantom{\ } \phantom{\ } \phantom{\ } i = 1, \dots, N \end{align}\]
error 部分有以下運動微分方程。
\[\begin{align} \dot{\delta\mathbf{\theta}}(t) &= -\hat{\mathbf{\omega}}(t)^{\wedge}\delta\mathbf{\theta}(t) - \tilde{\mathbf{\omega}}(t) \\ &= -\hat{\mathbf{\omega}}(t)^{\wedge}\delta\mathbf{\theta}(t) - \mathbf{n}_g(t) + \tilde{\mathbf{b}}_g(t)\\ \dot{\tilde{\mathbf{b}}}_g(t) &= -\mathbf{n}_{wg}(t) \\ {}^G\dot{\tilde{\mathbf{v}}}_I(t) &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{a}}(t) + [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)]^{\wedge}\delta\mathbf{\theta}(t) \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))(\mathbf{n}_a(t) - \tilde{\mathbf{b}}_a(t)) + [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)]^{\wedge}\delta\mathbf{\theta}(t) \notag \\ &= \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\mathbf{n}_a(t) - \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{b}}_a(t) + [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)]^{\wedge}\delta\mathbf{\theta}(t) \notag \\ &= [\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\hat{\mathbf{a}}(t)]^{\wedge}\delta\mathbf{\theta}(t) - \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\tilde{\mathbf{b}}_a(t) + \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\mathbf{n}_a(t) \\ \dot{\tilde{\mathbf{b}}}_a(t) &= -\mathbf{n}_{wa}(t) \\ {}^G\dot{\tilde{\mathbf{p}}}_I(t) &= {}^G\tilde{\mathbf{v}}_I(t) \\ {}^G\dot{\tilde{\mathbf{f}}}_i(t) &= \mathbf{0}_3, \phantom{\ } \phantom{\ } \phantom{\ } i = 1, \dots, N \end{align}\]
2.2. State Transition Matrix
從以上的微分方程計算 State Transition Matrix。
\[\begin{align} \dot{\tilde{\mathbf{x}}}(t) &= \begin{bmatrix} \dot{\delta\mathbf{\theta}}(t) \\ \dot{\tilde{\mathbf{b}}}_g(t) \\ {}^G\dot{\tilde{\mathbf{v}}}_I(t) \\ \dot{\tilde{\mathbf{b}}}_a(t) \\ {}^G\dot{\tilde{\mathbf{p}}}_I(t) \\ {}^G\dot{\tilde{\mathbf{f}}}_1 \\ \vdots \\ {}^G\dot{\tilde{\mathbf{f}}}_N \end{bmatrix} \notag \\ &= \begin{bmatrix} -\hat{\mathbf{\omega}}(t)^{\wedge} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & -\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \begin{bmatrix} \delta\mathbf{\theta}(t) \\ \tilde{\mathbf{b}}_g(t) \\ {}^G\tilde{\mathbf{v}}_I(t) \\ \tilde{\mathbf{b}}_a(t) \\ {}^G\tilde{\mathbf{p}}_I(t) \\ {}^G\tilde{\mathbf{f}}_1 \\ \vdots \\ {}^G\tilde{\mathbf{f}}_N \end{bmatrix} \notag \\ &\phantom{=} + \begin{bmatrix} -\mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & -\mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3N\times12} \end{bmatrix} \begin{bmatrix} \mathbf{n}_g \\ \mathbf{n}_{wg} \\ \mathbf{n}_a \\ \mathbf{n}_{wa} \end{bmatrix} \notag \\ &= \mathbf{F} \begin{bmatrix} \delta\mathbf{\theta}(t) \\ \tilde{\mathbf{b}}_g(t) \\ {}^G\tilde{\mathbf{v}}_I(t) \\ \tilde{\mathbf{b}}_a(t) \\ {}^G\tilde{\mathbf{p}}_I(t) \\ {}^G\tilde{\mathbf{f}}_1 \\ \vdots \\ {}^G\tilde{\mathbf{f}}_N \end{bmatrix} + \mathbf{G} \begin{bmatrix} \mathbf{n}_g \\ \mathbf{n}_{wg} \\ \mathbf{n}_a \\ \mathbf{n}_{wa} \end{bmatrix} \end{align}\]
\[\begin{align} \mathbf{F}_s &= \begin{bmatrix} -\hat{\mathbf{\omega}}(t)^{\wedge} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & -\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \end{bmatrix} \\ \mathbf{G}_s &= \begin{bmatrix} -\mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & -\mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) & \mathbf{0}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} \end{bmatrix} \end{align}\]
從時刻 \(t\) 積分到時刻 \(t + \Delta t\)。
\[\begin{align} \dot{\tilde{\mathbf{x}}}(t) &= \mathbf{F}(t) \tilde{\mathbf{x}}(t) + \mathbf{G} \mathbf{n} \\ \ddot{\tilde{\mathbf{x}}}(t) &= \mathbf{F}(t) \dot{\tilde{\mathbf{x}}}(t) + \dot{\mathbf{F}}(t) \tilde{\mathbf{x}}(t) \notag \\ &= \mathbf{F}(t) (\mathbf{F}(t) \tilde{\mathbf{x}}(t) + \mathbf{G} \mathbf{n}) + \dot{\mathbf{F}}(t) \tilde{\mathbf{x}}(t) \notag \\ &= \mathbf{F}(t)^2 \tilde{\mathbf{x}}(t) + \mathbf{F}(t) \mathbf{G} \mathbf{n} + \dot{\mathbf{F}}(t) \tilde{\mathbf{x}}(t) \\ \tilde{\mathbf{x}}(t+\Delta t) &= \tilde{\mathbf{x}}(t) + \dot{\tilde{\mathbf{x}}}(t) \Delta t + {1 \over 2!}\ddot{\tilde{\mathbf{x}}}(t) \Delta t^2 + {1 \over 3!} \tilde{\mathbf{x}}(t)^{\cdot\cdot\cdot} \Delta t^3 + \cdots \notag \\ &= \tilde{\mathbf{x}}(t) + \mathbf{F}(t)\tilde{\mathbf{x}}(t) \Delta t + {1 \over 2!}\mathbf{F}(t)^2\tilde{\mathbf{x}}(t) \Delta t^2 + {1 \over 3!}\mathbf{F}(t)^3\tilde{\mathbf{x}}(t) \Delta t^3 + \cdots + \Omicron(\mathbf{n}) \notag \\ &= \left[ \mathbf{I} + \mathbf{F}(t)\Delta t + {1 \over 2!}\mathbf{F}(t)^2\Delta t^2 + {1 \over 3!}\mathbf{F}(t)^3\Delta t^3 + \dots \right] \tilde{\mathbf{x}}(t) + \Omicron(\mathbf{n}) \notag \\ &\simeq \mathbf{\Phi}(t) \tilde{\mathbf{x}}(t) \end{align}\]
目標是求 State Transition Matrix \(\mathbf{\Phi}(t)\) 。
令 \(\mathbf{A} = -\hat{\mathbf{\omega}}(t)^{\wedge}, \mathbf{B} = [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge}, \mathbf{C} = -\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t))\)，將 \(\mathbf{F}(t)\) 簡寫，並計算 \(\mathbf{F}(t)\) 的各個次方。
\[\begin{align} \mathbf{F} &= \begin{bmatrix} \mathbf{A} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \\ \mathbf{F}^2 &= \begin{bmatrix} \mathbf{A}^2 & \mathbf{A} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A} & \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \\ \mathbf{F}^3 &= \begin{bmatrix} \mathbf{A}^3 & \mathbf{A}^2 & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A}^2 & \mathbf{B}\mathbf{A} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A} & \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \\ \mathbf{F}^4 &= \begin{bmatrix} \mathbf{A}^4 & \mathbf{A}^3 & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A}^3 & \mathbf{B}\mathbf{A}^2 & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A}^2 & \mathbf{B}\mathbf{A} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \end{align}\]
可以總結出規律。
\[\begin{align} \mathbf{F} &= \begin{bmatrix} \mathbf{A} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \\ \mathbf{F}^2 &= \begin{bmatrix} \mathbf{A}^2 & \mathbf{A} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A} & \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{C} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \\ \mathbf{F}^k &= \begin{bmatrix} \mathbf{A}^k & \mathbf{A}^{k-1} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A}^{k-1} & \mathbf{B}\mathbf{A}^{k-2} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B}\mathbf{A}^{k-2} & \mathbf{B}\mathbf{A}^{k-3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix}, k \ge 3 \end{align}\]
於是可以計算 \(\mathbf{\Phi}(t)\)。
\[\begin{align} \mathbf{\Phi}(t) &= \mathbf{I} + \mathbf{F}\Delta t + {1 \over 2!}\mathbf{F}^2\Delta t^2 + {1 \over 3!}\mathbf{F}^3\Delta t^3 + \dots \notag \\ &= \begin{bmatrix} \mathbf{I} + \mathbf{A} \Delta t + {1 \over 2!}\mathbf{A}^2 \Delta t^2 + \cdots & \mathbf{I} \Delta t + {1 \over 2!} \mathbf{A} \Delta t^2 + {1 \over 3!} \mathbf{A}^2 \Delta t^3 + \cdots & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{B} \Delta t + {1 \over 2!}\mathbf{B}\mathbf{A} \Delta t^2 + {1 \over 3!}\mathbf{B}\mathbf{A}^2 \Delta t^3 + \cdots & {1 \over 2!}\mathbf{B} \Delta t^2 + {1 \over 3!}\mathbf{B}\mathbf{A} \Delta t^3 + {1 \over 4!}\mathbf{B}\mathbf{A}^2 \Delta t^4 + \cdots & \mathbf{I}_{3\times3} & \mathbf{C} \Delta t & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ {1 \over 2!}\mathbf{B} \Delta t^2 + {1 \over 3!}\mathbf{B}\mathbf{A} \Delta t^3 + {1 \over 4!}\mathbf{B}\mathbf{A}^2 \Delta t^4 + \cdots & {1 \over 3!}\mathbf{B} \Delta t^3 + {1 \over 4!}\mathbf{B}\mathbf{A} \Delta t^4 + {1 \over 5!}\mathbf{B}\mathbf{A}^2 \Delta t^5 + \cdots & \mathbf{I}_{3\times3} \Delta t & \mathbf{C} \Delta t^2 & \mathbf{I}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \notag \\ &= \begin{bmatrix} \mathbf{\Phi}(t)^{(1,1)} & \mathbf{\Phi}(t)^{(1,2)} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{\Phi}(t)^{(3,1)} & \mathbf{\Phi}(t)^{(3,2)} & \mathbf{I}_{3\times3} & \mathbf{\Phi}(t)^{(3,4)} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{0}_{3\times3} & \mathbf{I}_{3\times3} & \mathbf{0}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{\Phi}(t)^{(5,1)} & \mathbf{\Phi}(t)^{(5,2)} & \mathbf{I}_{3\times3} \Delta t & \mathbf{\Phi}(t)^{(5,4)} & \mathbf{I}_{3\times3} &|& \mathbf{0}_{3\times 3N} \\ \mathbf{0}_{3N\times15} & & & & &|& \mathbf{0}_{3N\times3N} \end{bmatrix} \end{align}\]
參考論文 [1] 的公式 (366) 整理以上矩陣塊。
\[\begin{align} \mathbf{\Phi}(t)^{(1,1)} &= \mathbf{I} + \mathbf{A} \Delta t + {1 \over 2!}\mathbf{A}^2 \Delta t^2 + \cdots \notag \\ &= \text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) \\ \mathbf{\Phi}(t)^{(1,2)} &= \mathbf{I} \Delta t + {1 \over 2!} \mathbf{A} \Delta t^2 + {1 \over 3!} \mathbf{A}^2 \Delta t^3 + \cdots \notag \\ &= \mathbf{I} \Delta t + {1 \over 2!} {-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} \Delta t^2 + {1 \over 3!} {-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} \mathbf{A} \Delta t^3 + \cdots \notag \\ &= \mathbf{I} \Delta t + {-\mathbf{A} \over ||\hat{\mathbf{\omega}}(t)||^2} \left({1 \over 2!} \mathbf{A}^2 \Delta t^2 + {1 \over 3!} \mathbf{A}^3 \Delta t^3 + \cdots \right) \notag \\ &= \mathbf{I} \Delta t + {-\mathbf{A} \over ||\hat{\mathbf{\omega}}(t)||^2} (\text{exp}(\mathbf{A}\Delta t) - \mathbf{I} - \mathbf{A}\Delta t) \notag \\ &= \mathbf{I} \Delta t + {\hat{\mathbf{\omega}}(t)^{\wedge} \over ||\hat{\mathbf{\omega}}(t)||^2} (\text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) \\ \mathbf{\Phi}(t)^{(3,1)} &= \mathbf{B} \Delta t + {1 \over 2!}\mathbf{B}\mathbf{A} \Delta t^2 + {1 \over 3!}\mathbf{B}\mathbf{A}^2 \Delta t^3 + \cdots \notag \\ &= \mathbf{B} \left(\mathbf{I}\Delta t + {1 \over 2!}\mathbf{A} \Delta t^2 + {1 \over 3!}\mathbf{A}^2 \Delta t^3 + \cdots\right) \notag \\ &= \mathbf{B} \mathbf{\Phi}(t)^{(1,2)} \notag \\ &= \mathbf{B}\left(\mathbf{I} \Delta t + {\hat{\mathbf{\omega}}(t)^{\wedge} \over ||\hat{\mathbf{\omega}}(t)||^2} (\text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge}\Delta t)\right) \notag \\ &= [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge} \left(\mathbf{I} \Delta t + {\hat{\mathbf{\omega}}(t)^{\wedge} \over ||\hat{\mathbf{\omega}}(t)||^2} (\text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge}\Delta t)\right) \\ \mathbf{\Phi}(t)^{(3,2)} &= \mathbf{\Phi}(t)^{(5,1)} = {1 \over 2!}\mathbf{B} \Delta t^2 + {1 \over 3!}\mathbf{B}\mathbf{A} \Delta t^3 + {1 \over 4!}\mathbf{B}\mathbf{A}^2 \Delta t^4 + \cdots \notag \\ &= \mathbf{B} \left( {1 \over 2!}\mathbf{I} \Delta t^2 + {1 \over 3!}\mathbf{A} \Delta t^3 + {1 \over 4!}\mathbf{A}^2 \Delta t^4 + \cdots \right) \notag \\ &= \mathbf{B} \left( {1 \over 2!}\mathbf{I} \Delta t^2 + {1 \over 3!}{-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} \Delta t^3 + {1 \over 4!}{-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2}\mathbf{A} \Delta t^4 + \cdots \right) \notag \\ &= \mathbf{B} \left[ {1 \over 2!}\mathbf{I} \Delta t^2 + {-1 \over ||\hat{\mathbf{\omega}}(t)||^2} \left({1 \over 3!}\mathbf{A}^3 \Delta t^3 + {1 \over 4!}\mathbf{A}^4 \Delta t^4 + \cdots \right) \right] \notag \\ &= \mathbf{B} \left[ {1 \over 2!}\mathbf{I} \Delta t^2 + {-1 \over ||\hat{\mathbf{\omega}}(t)||^2} \left( \text{exp}(\mathbf{A}\Delta t) - \mathbf{I} - \mathbf{A}\Delta t - {1 \over 2!}\mathbf{A}^2\Delta t^2 \right) \right] \notag \\ &= \mathbf{B} \left[ {1 \over 2!}\mathbf{I} \Delta t^2 + {-1 \over ||\hat{\mathbf{\omega}}(t)||^2} \left( \text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge}\Delta t - {1 \over 2!}(\hat{\mathbf{\omega}}(t)^{\wedge})^2\Delta t^2 \right) \right] \notag \\ &= [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge} \left[ {1 \over 2!}\mathbf{I} \Delta t^2 + {-1 \over ||\hat{\mathbf{\omega}}(t)||^2} \left( \text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge}\Delta t - {1 \over 2!}(\hat{\mathbf{\omega}}(t)^{\wedge})^2\Delta t^2 \right) \right] \\ \mathbf{\Phi}(t)^{(3,4)} &= \mathbf{C} \Delta t \notag \\ &= -\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) \Delta t \\ \mathbf{\Phi}(t)^{(5,4)} &= \mathbf{C} \Delta t^2 \notag \\ &= -\mathbf{C}^T({}^G\hat{\mathbf{q}}_I(t)) \Delta t^2 \end{align}\]
\[\begin{align} \mathbf{\Phi}(t)^{(5,2)} &= {1 \over 3!}\mathbf{B} \Delta t^3 + {1 \over 4!}\mathbf{B}\mathbf{A} \Delta t^4 + {1 \over 5!}\mathbf{B}\mathbf{A}^2 \Delta t^5 + \cdots \notag \\ &= \mathbf{B} \left( {1 \over 3!}\mathbf{I}\Delta t^3 + {1 \over 4!}\mathbf{A} \Delta t^4 + {1 \over 5!}\mathbf{A}^2 \Delta t^5 + \cdots \right) \notag \\ &= \mathbf{B} \left( {1 \over 3!}\mathbf{I}\Delta t^3 + {1 \over 4!}{-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} \Delta t^4 + {1 \over 5!}{-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} \mathbf{A} \Delta t^5 + \cdots \right) \notag \\ &= \mathbf{B} \left( {1 \over 3!}\mathbf{I}\Delta t^3 + {1 \over 4!} \mathbf{A} {-\mathbf{A}^2 \over ||\hat{\mathbf{\omega}}(t)||^2} \Delta t^4 + {1 \over 5!} \mathbf{A} {-\mathbf{A}^2 \over ||\hat{\mathbf{\omega}}(t)||^2} \mathbf{A} \Delta t^5 + \cdots \right) \notag \\ &= \mathbf{B} \left( {1 \over 3!}\mathbf{I}\Delta t^3 + {1 \over 4!} {-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} {-\mathbf{A}^2 \over ||\hat{\mathbf{\omega}}(t)||^2} \Delta t^4 + {1 \over 5!} {-\mathbf{A}^3 \over ||\hat{\mathbf{\omega}}(t)||^2} {-\mathbf{A}^2 \over ||\hat{\mathbf{\omega}}(t)||^2} \mathbf{A} \Delta t^5 + \cdots \right) \notag \\ &= \mathbf{B} \left[ {1 \over 3!}\mathbf{I}\Delta t^3 + {\mathbf{A} \over ||\hat{\mathbf{\omega}}(t)||^4} \left( {1 \over 4!} \mathbf{A}^4 \Delta t^4 + {1 \over 5!} \mathbf{A}^5 \Delta t^5 + \cdots \right) \right] \notag \\ &= \mathbf{B} \left[ {1 \over 3!}\mathbf{I}\Delta t^3 + {\mathbf{A} \over ||\hat{\mathbf{\omega}}(t)||^4} \left( \text{exp}(\mathbf{A}\Delta t) - \mathbf{I} - \mathbf{A} \Delta t - {1 \over 2!} \mathbf{A}^2 \Delta t^2 - {1 \over 3!} \mathbf{A}^3 \Delta t^3 \right) \right] \notag \\ &= \mathbf{B} \left[ {1 \over 3!}\mathbf{I}\Delta t^3 + {-\hat{\mathbf{\omega}}(t)^{\wedge} \over ||\hat{\mathbf{\omega}}(t)||^4} \left( \text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge} \Delta t - {1 \over 2!} (\hat{\mathbf{\omega}}(t)^{\wedge})^2 \Delta t^2 + {1 \over 3!} (\hat{\mathbf{\omega}}(t)^{\wedge})^3 \Delta t^3 \right) \right] \notag \\ &= [\mathbf{C}^T\left({}^G\hat{\mathbf{q}}_I(t)\right)\hat{\mathbf{a}}(t)]^{\wedge} \left[ {1 \over 3!}\mathbf{I}\Delta t^3 + {-\hat{\mathbf{\omega}}(t)^{\wedge} \over ||\hat{\mathbf{\omega}}(t)||^4} \left( \text{exp}(-\hat{\mathbf{\omega}}(t)^{\wedge}\Delta t) - \mathbf{I} + \hat{\mathbf{\omega}}(t)^{\wedge} \Delta t - {1 \over 2!} (\hat{\mathbf{\omega}}(t)^{\wedge})^2 \Delta t^2 - {1 \over 3!} ||\hat{\mathbf{\omega}}(t)||^2 \hat{\mathbf{\omega}}(t)^{\wedge} \Delta t^3 \right) \right] \\ \end{align}\]