[論文レビュー] 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）の包括的で数学的に厳密な導出と比較分析を提供する。これら5つのフィルタをベイジアン最適化フレームワークの下で統一し、最大事後確率（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に比べて収束性と精度を向上させること。
• 5つのフィルタの体系的比較を行い、それらの類似点・相違点およびアルゴリズム的構造を明らかにすること。
• ロボット工学、SLAM、自律システム分野における非線形状態推定の実装や分析を行う研究者にとっての基盤的リファレンスを提供すること。

提案手法

• ベイジアン推論と最小平均二乗誤差（MMSE）推定を用いて、カルマンフィルタの予測と更新ステップを導出する。
• 非線形運動モデルおよび観測モデルを一次のテイラー展開で線形化することにより、拡張カルマンフィルタ（EKF）を適用する。
• 名目軌道からの状態誤差を伝搬することで、誤差状態カルマンフィルタ（ESKF）を導入し、非線形システムにおける数値的安定性を向上させる。
• 観測モデルを更新された推定値の周囲で繰り返し線形化することで、繰り返し拡張カルマンフィルタ（IEKF）を構築し、収束性を改善する。
• ガウス・ニュートン最適化を用いてイノベーション項の反復的改善を組み合わせたESKF構造を採用し、繰り返し誤差状態カルマンフィルタ（IESKF）を導出する。
• MAPに基づく導出によりEKFとIEKFを統一し、反復的更新が尤度関数上でのガウス・ニュートンステップに対応することを示す。

実験結果

リサーチクエスチョン

• RQ1 KF、EKF、ESKF、IEKF、IESKFは、それぞれの数学的定式化およびアルゴリズム的構造においてどのように異なるか？
• RQ2 EKFとMAP推定の関係は何か？ガウス・ニュートン最適化はEKFの性能をどのように向上させるか？
• 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が観測モデルに対して複数回のガウス・ニュートン反復を実行していることに相当することを確立し、非線形システムにおいてより高速な収束と優れた性能を達成していることが示された。