码迷,mamicode.com
首页 > 其他好文 > 详细

对 Error-State Kalman Filter 的理解

时间:2019-10-02 00:29:38      阅读:431      评论:0      收藏:0      [点我收藏+]

标签:conf   公式   min   博客   理解   参考文献   nal   text   error   

我对 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。

未完待续。正在整理。

参考文献

[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.

对 Error-State Kalman Filter 的理解

标签:conf   公式   min   博客   理解   参考文献   nal   text   error   

原文地址:https://www.cnblogs.com/JingeTU/p/11616469.html

(0)
(0)
   
举报
评论 一句话评论(0
登录后才能评论!
© 2014 mamicode.com 版权所有  联系我们:gaon5@hotmail.com
迷上了代码!