我对 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. VIO 引用 ErKF
参考 [1] 进行此部分的推导。从上一篇博客 《Rotation Kinematics》 的 Quaternion 微分方程开始。
事先声明，我使用 Hamilton Quaternion。
未完待续。正在整理。