[论文解读] Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)

Gyubeom Im|arXiv (Cornell University)|Jun 10, 2024

Inertial Sensor and Navigation被引用 4

一句话总结

本文提供了卡尔曼滤波器（KF）、扩展卡尔曼滤波器（EKF）、误差状态卡尔曼滤波器（ESKF）、迭代扩展卡尔曼滤波器（IEKF）以及迭代误差状态卡尔曼滤波器（IESKF）的全面、数学上严谨的推导与对比分析。该文将这些滤波器统一于贝叶斯优化框架下，通过最大后验概率（MAP）估计与高斯-牛顿优化方法推导出每种滤波器，明确推导了预测与校正步骤，通过迭代优化显著提升了收敛性与精度。

ABSTRACT

The Kalman Filter (KF) is a powerful mathematical tool widely used for state estimation in various domains, including Simultaneous Localization and Mapping (SLAM). This paper presents an in-depth introduction to the Kalman Filter and explores its several extensions: the Extended Kalman Filter (EKF), the Error-State Kalman Filter (ESKF), the Iterated Extended Kalman Filter (IEKF), and the Iterated Error-State Kalman Filter (IESKF). Each variant is meticulously examined, with detailed derivations of their mathematical formulations and discussions on their respective advantages and limitations. By providing a comprehensive overview of these techniques, this paper aims to offer valuable insights into their applications in SLAM and enhance the understanding of state estimation methodologies in complex environments.

研究动机与目标

• 从贝叶斯与优化的角度，提供KF、EKF、ESKF、IEKF与IESKF的统一、数学上严谨的推导。
• 阐明在非线性滤波背景下，MAP估计、MLE与高斯-牛顿优化之间的关系。
• 通过迭代优化方法推导IEKF与IESKF的更新步骤，相较于标准EKF与ESKF，显著提升收敛性与精度。
• 系统性地对比五种滤波器，突出其相似性、差异性及算法结构。
• 为机器人学、SLAM与自主系统中非线性状态估计的实现与分析研究者提供基础参考。

提出的方法

• 基于贝叶斯推断与最小均方误差（MMSE）估计，推导卡尔曼滤波器的预测与更新步骤。
• 通过一阶泰勒展开对非线性运动模型与观测模型进行线性化，应用扩展卡尔曼滤波器（EKF）。
• 通过在标称轨迹上传播状态误差，引入误差状态卡尔曼滤波器（ESKF），提升非线性系统中的数值稳定性。
• 通过在更新后的状态估计周围反复线性化观测模型，发展迭代扩展卡尔曼滤波器（IEKF），以改善收敛性。
• 通过将ESKF结构与基于高斯-牛顿优化的创新项迭代修正相结合，推导出迭代误差状态卡尔曼滤波器（IESKF）。
• 采用基于MAP的推导方法统一EKF与IEKF，表明迭代更新对应于似然函数上的高斯-牛顿步骤。

实验结果

研究问题

• RQ1 KF、EKF、ESKF、IEKF与IESKF在数学表达与算法结构上存在哪些差异？
• RQ2 EKF与MAP估计之间存在何种关系？高斯-牛顿优化如何提升其性能？
• RQ3 IEKF与IESKF中的迭代优化如何相较于标准EKF与ESKF提升估计精度？
• RQ4 为何误差状态形式（ESKF/IESKF）在非线性系统中比标准EKF更具数值稳定性？
• RQ5 黑塞矩阵与费舍尔信息在IESKF更新步骤推导中扮演何种角色？

主要发现

• IEKF与IESKF通过在更新后的状态估计周围反复线性化观测模型，显著降低线性化误差，从而实现更优的收敛性与精度。
• IESKF的更新步骤被证明为MAP目标函数上的高斯-牛顿步骤，表明迭代优化等价于通过牛顿型优化最小化负对数似然。
• 推导表明，ESKF与IESKF框架由于采用误差传播而非绝对状态传播，相比标准EKF在数值稳定性方面更具优势。
• 推导揭示，KF中的卡尔曼增益在高斯假设下为最优，而EKF与IEKF中的卡尔曼增益则源自线性化模型。
• IESKF的更新公式被显式推导为：δx̂_t|t,j = K_t,j(z_t - h( x̂_t|t,j,0,0) + H_t,j J_t,j^{-1}(x̂_t|t,j - x̂_t|t-1)) - J_t,j^{-1}(x̂_t|t,j - x̂_t|t-1)，明确展示了校正项对雅可比矩阵与创新项的依赖性。
• 本文建立证明，IESKF等价于对观测模型执行多次高斯-牛顿迭代，从而在非线性系统中实现更快收敛与更优性能。