对 Error-State Kalman Filter 的理解
我对 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}\]
