对 Error-State Kalman Filter 的理解

Posted 2022-12-11 jingetu

我对 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, \\(\\mathbfF(t)\\), in EKF formulation"。ErKF 的 system matrix 在论文 [2] 中的

论文 [2] 中 EKF 与 ErKF 的 system matrix \\(\\mathbfF(t)\\) 分别是公式 (11) 与 (18)。

1.1. EKF 的近似

EKF 的 \\(\\mathbfF(t)\\) 用于对 States 的 Covariance 估计，可以将公式 (10) (11) (12) 结合一起看。

在 \\(t\\) 时刻 States 的 Covariance 是 \\(\\mathbfP(t) = \\mathbfE[(\\mathbfx(t) - \\hat\\mathbfx(t))(\\mathbfx(t) - \\hat\\mathbfx(t))^T]\\)，其中 \\(\\mathbfx(t)\\) 是 States 真值，\\(\\hat\\mathbfx(t)\\) 是系统对 States 的的估计值。经过时间 \\(\\Delta t\\) 到达下一个时刻 \\(t + \\Delta t\\)，此时 Covariance 可以如下表示。

\\[\\beginalign \\mathbfP(t + \\Delta t) = \\mathbfE[(\\mathbfx(t + \\Delta t) - \\hat\\mathbfx(t + \\Delta t))(\\mathbfx(t + \\Delta t) - \\hat\\mathbfx(t + \\Delta t))^T] \\endalign\\]

但是在没有观测之前，我们尚未获得系统在 \\(t + \\Delta t\\) 时刻的结果，即估计值 \\(\\hat\\mathbfx(t + \\Delta t)\\)。考虑在 \\(t + \\Delta t\\) 时刻没有观测之前的系统 Covariance \\(\\mathbfP^-(t + \\Delta t)\\)。需要将上面公式中的 \\(\\hat\\mathbfx(t + \\Delta t)\\) 替换为 \\(\\hat\\mathbfx^-(t + \\Delta t)\\)，表示是用 \\(\\hat\\mathbfx(t)\\) 经过运动方程计算得到的 \\(t + \\Delta t\\) 时刻的 Predict 值（之后还需要经过 Update 过程修正）。

\\[\\beginalign \\hat\\mathbfx^-(t + \\Delta t) &= \\hat\\mathbfx(t) + \\dot\\hat\\mathbfx(t) \\Delta t \\notag \\ &= \\hat\\mathbfx(t) + (\\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) + \\mathbf\\Gamma\\mathbfw(t)) \\Delta t \\ \\mathbfP^-(t + \\Delta t) &= \\mathbfE[(\\mathbfx(t + \\Delta t) - \\hat\\mathbfx^-(t + \\Delta t))(\\mathbfx(t + \\Delta t) - \\hat\\mathbfx^-(t + \\Delta t))^T] \\endalign\\]

对于 \\(t + \\Delta t\\) 时刻的真值，系统是无从得知的，使用 \\(t\\) 时刻的真值与运动方程估计。

\\[\\beginalign \\mathbfx^-(t + \\Delta t) &= \\mathbfx(t) + \\dot\\mathbfx(t) \\Delta t \\notag \\ &= \\mathbfx(t) + [\\mathbff(\\mathbfx(t), \\mathbfu(t)) + \\mathbf\\Gamma\\mathbfw(t)] \\Delta t \\notag \\ &\\simeq \\mathbfx(t) + \\left[ \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) + \\partial \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\over \\partial \\mathbfx(t)(\\mathbfx(t) - \\hat\\mathbfx(t)) + \\mathbf\\Gamma\\mathbfw(t)\\right] \\Delta t （泰勒一阶展开近似）\\notag \\ &= \\mathbfx(t) + \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\Delta t + \\mathbfF(t) (\\mathbfx(t) - \\hat\\mathbfx(t)) \\Delta t + \\mathbf\\Gamma\\mathbfw(t) \\Delta t \\endalign\\]

计算 \\(\\mathbfP^-(t + \\Delta t)\\)。

\\[\\beginalign \\mathbfP^-(t + \\Delta t) &= \\mathbfE[(\\mathbfx^-(t + \\Delta t) - \\hat\\mathbfx^-(t + \\Delta t))(\\mathbfx^-(t + \\Delta t) - \\hat\\mathbfx^-(t + \\Delta t))^T] \\notag \\ &\\simeq \\mathbfE[(\\mathbfx(t) + \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\Delta t + \\mathbfF(t) (\\mathbfx(t) - \\hat\\mathbfx(t)) \\Delta t + \\mathbf\\Gamma\\mathbfw(t) \\Delta t - \\hat\\mathbfx(t) - \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\Delta t) \\notag \\ &\\phantom=(\\mathbfx(t) + \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\Delta t + \\mathbfF(t) (\\mathbfx(t) - \\hat\\mathbfx(t)) \\Delta t + \\mathbf\\Gamma\\mathbfw(t) \\Delta t - \\hat\\mathbfx(t) - \\mathbff(\\hat\\mathbfx(t), \\mathbfu(t)) \\Delta t)^T ] \\notag \\ &= \\mathbfE[((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t))) ((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t)))^T] \\notag \\ &\\phantom= \\mathbfE[\\mathbf\\Gamma\\mathbfw(t)((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t)))^T] + \\mathbfE[((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t)))\\mathbfw(t)^T\\mathbf\\Gamma^T] \\notag \\ &\\phantom= + \\mathbfE[\\mathbf\\Gamma\\mathbfw(t)\\mathbfw(t)^T\\mathbf\\Gamma^T]\\notag \\ &= (\\mathbfI + \\mathbfF(t)\\Delta t) \\mathbfE[(\\mathbfx(t) - \\hat\\mathbfx(t)) (\\mathbfx(t) - \\hat\\mathbfx(t))^T] (\\mathbfI + \\mathbfF(t)\\Delta t)^T \\notag \\ &\\phantom= \\mathbf\\Gamma\\mathbfE[\\mathbfw(t)]\\mathbfE[((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t)))^T] + \\mathbfE[((\\mathbfI + \\mathbfF(t)\\Delta t)(\\mathbfx(t) - \\hat\\mathbfx(t)))]\\mathbfE[\\mathbfw(t)^T]\\mathbf\\Gamma^T \\notag \\ &\\phantom= + \\mathbf\\Gamma\\mathbfE[\\mathbfw(t)\\mathbfw(t)^T]\\mathbf\\Gamma^T \\notag \\ &= (\\mathbfI + \\mathbfF(t)\\Delta t) \\mathbfP(t) (\\mathbfI + \\mathbfF(t)\\Delta t)^T + \\mathbf\\Gamma\\mathbfQ\\mathbf\\Gamma^T \\phantom= (\\textWe\\ have\\ \\mathbfE[\\mathbfw(t)] = 0)\\notag \\ &= \\mathbfP(t) + \\mathbfP(t)\\mathbfF(t)^T\\Delta t + \\mathbfF(t) \\mathbfP(t) \\Delta t + \\mathbfF(t) \\mathbfP(t)\\mathbfF(t)^T\\Delta t^2 + \\mathbf\\Gamma\\mathbfQ\\mathbf\\Gamma^T \\notag \\ &\\simeq \\mathbfP(t) + \\mathbfP(t)\\mathbfF(t)^T\\Delta t + \\mathbfF(t) \\mathbfP(t) \\Delta t + \\mathbf\\Gamma\\mathbfQ\\mathbf\\Gamma^T \\phantom= (\\Delta t^2 \\text\\ too\\ small, ignore second order and higher) \\endalign\\]

所以得到 \\(\\mathbfP(t)\\) 的微分方程。

\\[\\beginalign \\dot\\mathbfP(t) &= \\mathbfP(t)\\mathbfF(t)^T + \\mathbfF(t) \\mathbfP(t) + \\mathbf\\Gamma\\mathbfQ\\mathbf\\Gamma^T \\endalign\\]

与公式 (11) 一致。所以，得到关于 EKF 近似的结论，EKF 在 Predict 步骤中使用上一时刻对 States 的估计值 \\(\\hat\\mathbfx(t)\\)，对运动方程 \\(\\mathbff\\) 使用一阶泰勒展开近似下一时刻 States 的真值 \\(\\mathbfx(t + \\Delta t)\\)。从而估计 Predict 步骤得到的 States Covariance。

1.2. ErKF 无近似

ErKF 估计的是 States 的误差部分。

在 Predict 步骤的操作如下。

省略了控制值与误差之后的微分方程如下。（论文中的符号混乱，所以我重新整理。）

\\[\\beginalign \\dot\\mathbfx(t) &= \\mathbff(\\mathbfx(t), \\mathbfu(t)) + \\mathbf\\Gamma\\mathbfw(t) \\endalign\\]

将真值分解成为 nominal 和 error 两个部分。nomianl 部分使用 \\(\\hat\\mathbfx(t)\\) 表示，error 部分使用 \\(\\delta \\mathbfx(t)\\) 表示。真值 \\(\\mathbfx(t) = \\hat\\mathbfx(t) + \\delta \\mathbfx(t)\\) 。代入运动（微分）方程。

\\[\\beginalign \\dot\\hat\\mathbfx(t) + \\delta\\dot\\mathbfx &= \\mathbff(\\hat\\mathbfx(t)+\\delta\\mathbfx(t),\\mathbfu(t)) + \\mathbf\\Gamma\\mathbfw(t) \\notag \\ &= \\mathbff(\\hat\\mathbfx(t),\\mathbfu(t)) + \\partial \\mathbff(\\hat\\mathbfx(t),\\mathbfu(t)) \\over \\partial \\mathbfx(t) \\delta\\mathbfx(t) + \\mathbf\\Gamma\\mathbfw(t) \\notag \\ &= \\mathbff(\\hat\\mathbfx(t),\\mathbfu(t)) + \\mathbfF(t) \\delta\\mathbfx(t) + \\mathbf\\Gamma\\mathbfw(t) \\ \\mathbfF(t) &= \\partial \\mathbff(\\hat\\mathbfx(t),\\mathbfu(t)) \\over \\partial \\mathbfx(t) \\ \\dot\\hat\\mathbfx(t) &= \\mathbff(\\hat\\mathbfx(t),\\mathbfu(t)) \\phantom=(定义) \\ \\delta\\dot\\mathbfx(t) &= \\mathbfF(t) \\delta\\mathbfx(t) + \\mathbf\\Gamma\\mathbfw(t) \\endalign\\]

由定义可以得到时间 \\(\\Delta t\\) 之后时刻 \\(t + \\Delta t\\) 的 \\(\\delta\\mathbfx(t + \\Delta t)\\)。

\\[\\beginalign \\delta\\mathbfx(t + \\Delta t) &= \\delta\\mathbfx(t) + \\delta\\dot\\mathbfx(t) \\Delta t \\notag \\ &= \\delta\\mathbfx(t) + \\mathbfF(t) \\delta\\mathbfx(t)\\Delta t + \\mathbf\\Gamma\\mathbfw(t)\\Delta t \\notag \\ &= (\\mathbfI + \\mathbfF(t)\\Delta t)\\delta\\mathbfx(t) + \\mathbf\\Gamma\\mathbfw(t)\\Delta t \\notag \\ &= \\mathbf\\Phi(t)\\delta\\mathbfx(t) + \\mathbf\\Gamma\\mathbfw(t)\\Delta t \\endalign\\]

在 ErKF 中 \\(\\mathbf\\Phi(t)\\) 被称作 State Transition Matrix。ErKF 的 Predict 步骤做近似 \\(\\delta\\mathbfx(t + \\Delta t) \\simeq \\mathbf\\Phi(t)\\delta\\mathbfx(t)\\)，剩下的 \\(\\mathbf\\Gamma\\mathbfw(t)\\Delta t\\) 在 Update 步骤用观测方程进行估计。

从 \\(\\mathbfF(t)\\) 的定义中可以看到，其与 error \\(\\delta\\mathbfx(t)\\) 是无关的，使用上一步的估计值 \\(\\hat\\mathbfx(t)\\) 与这一步的控制值 \\(\\mathbfu(t)\\) 即可计算得到。于是，在 Predict 步骤无需设计 error \\(\\delta\\mathbfx(t)\\) 的 error，对 \\(\\mathbf\\Phi(t)\\) 进行近似。

所以，可以得到结论，ErKF 的 Predict 步骤是一个线性的步骤，不存在隐藏的近似。存在的近似 \\(\\mathbf\\Gamma\\mathbfw(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.