对 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}} &= \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) + \mathbf{\Gamma}\mathbf{w}(t) \notag \\ &= \mathbf{f}(\hat{\mathbf{x}}(t),\mathbf{u}(t)) + \mathbf{F}(t) \delta\mathbf{x}(t) + \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) + \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 + \mathbf{\Gamma}\mathbf{w}(t)\Delta t \notag \\ &= (\mathbf{I} + \mathbf{F}(t)\Delta t)\delta\mathbf{x}(t) + \mathbf{\Gamma}\mathbf{w}(t)\Delta t \notag \\ &= \mathbf{\Phi}(t)\delta\mathbf{x}(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)\)，剩下的 \(\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 步骤是一个线性的步骤，不存在隐藏的近似。存在的近似 \(\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。各个状态值的含义如下：

1. \({}^G\mathbf{q}_I\) 将点在 \(I\) 坐标系下的坐标转换到 \(G\) 坐标系下的坐标所使用的四元数；
2. \(\mathbf{b}_g\) 陀螺仪测量值的 bias；
3. \({}^G\mathbf{v}_I\) 在 \(I\) 坐标系原点在 \(G\) 坐标系下坐标的速度，即 \({}^G\dot{\mathbf{p}}_I\)；
4. \(\mathbf{b}_a\) 加速度计测量值的 bias；
5. \({}^G\mathbf{p}_I\) 将点在 \(I\) 坐标系下的坐标转换到 \(G\) 坐标系下的坐标所使用的偏移，即 \(I\) 坐标系原点在 \(G\) 坐标系下的坐标；
6. \({}^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} \\ \tilde{\mathbf{x}}(t+2)) &= \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}\]

参考文献

[1] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).

[2] Madyastha, Venkatesh, et al. "Extended Kalman filter vs. error state Kalman filter for aircraft attitude estimation." AIAA Guidance, Navigation, and Control Conference. 2011.

转载于:https://www.cnblogs.com/JingeTU/p/11616469.html