[論文レビュー] $SE_2(3)$ based Extended Kalman Filtering and Smoothing Framework for Inertial-Integrated Navigation

Yarong Luo, Chi Guo+3 more|arXiv (Cornell University)|Feb 25, 2021

Inertial Sensor and Navigation参考文献 17被引用数 7

ひとこと要約

本稿では、状態を行列リー群で表現することで、姿勢、速度、位置の全状態に共通のフレーム誤差表現を可能にする $SE_2(3)$-ベースの拡張カルマンフィルタ（EKF）フレームワークを提案する。群アフィンの運動モデルと左/右不変誤差定義を活用することで、遷移行列から特定の加速度項を排除し、不変EKFと同等の性能を達成することで、大規模な誤差初期値条件下でも一貫性と耐障害性を向上させる。

ABSTRACT

This paper proposes an $SE_2(3)$ based extended Kalman filtering (EKF) framework for the inertial-integrated state estimation problem. The error representation using the straight difference of two vectors in the inertial navigation system may not be reasonable as it does not take the direction difference into consideration. Therefore, we choose to use the $SE_2(3)$ matrix Lie group to represent the state of the inertial-integrated navigation system which consequently leads to the common frame error representation. With the new velocity and position error definition, we leverage the group affine dynamics with the autonomous error properties and derive the error state differential equation for the inertial-integrated navigation on the north-east-down (NED) navigation frame and the earth-centered earth-fixed (ECEF) frame, respectively, the corresponding EKF, terms as $SE_2(3)$ based EKF has also been derived. It provides a new perspective on the geometric EKF with a more sophisticated formula for the inertial-integrated navigation system. Furthermore, we design two new modified error dynamics on the NED frame and the ECEF frame respectively by introducing new auxiliary vectors. Finally the equivalence of the left-invariant EKF and left $SE_2(3)$ based EKF have been shown in navigation frame and ECEF frame.

研究の動機と目的

• 従来のEKFにおけるインertシャル統合航法の状態誤差表現の不一致を是正すること。具体的には、フレームを考慮しないまま直線的なベクトル差で誤差を定義する点を改善する。
• 全状態（姿勢、速度、位置）を表すために $SE_2(3)$ 行列リー群を用いた幾何学的に一貫したEKFフレームワークを構築すること。
• NEDおよびECEF航行フレーム上で左・右不変誤差定式化を導出し、群アフィンの運動モデルと誤差の自己同性を保証すること。
• 左不変EKFと $SE_2(3)$-ベースのEKFの等価性を示し、提案フレームワークの理論的整合性を検証すること。
• 既存の不変スムージング手法とは異なり、単純で直感的な $SE_2(3)$-ベースのRTSスムージングアルゴリズムを用いて、フレームワークをスムージングに拡張すること。

提案手法

• 状態を $SE_2(3)$ 行列リー群に埋め込む。これにより回転、並進、速度が1つの多様体に統合され、幾何学的に一貫した誤差表現が可能になる。
• 誤差状態は群積 $\eta = \hat{\mathcal{X}} \mathcal{X}^{-1}$ で定義され、姿勢・速度・位置の誤差がすべて共通フレームで表現される。
• 群アフィン性を活用して誤差状態の運動モデルを導出し、右不変定式化では特定の加速度項が自然に遷移行列から消失する。
• NEDおよびECEFフレーム上で、数値安定性と誤差伝搬の改善を目的に補助速度ベクトルを用いた2種類の修正誤差ダイナミクスを導入する。
• NEDおよびECEFフレームそれぞれに独立してEKFを定式化し、左・右不変誤差定義を併用することで、異なる航行状況下でも耐障害性を確保する。
• 行列指数関数と群演算を用いて全状態更新を行う、$SE_2(3)$-ベースのスムージングアルゴリズムを提案する。

実験結果

リサーチクエスチョン

• RQ1 共通フレームを用いることで、$SE_2(3)$-ベースのEKFフレームワークがインエラー統合航法における誤差表現の一貫性を向上させられるか？
• RQ2 $SE_2(3)$-ベースのEKFは右不変定式化と同様に、遷移行列から特定の加速度項を排除できるか？その場合、数値的安定性にどのような影響を与えるか？
• RQ3 腕のずれ誤差を考慮したNEDおよびECEFフレーム上での左不変EKFと $SE_2(3)$-ベースのEKFが等価であるか？
• RQ4 NEDおよびECEFフレーム上での修正誤差ダイナミクスは、大規模な誤差初期値条件下でのEKF性能をどのように向上させるか？
• RQ5 $SE_2(3)$-ベースのフレームワークは、既存の不変スムージング手法よりも単純で直感的な定式化でスムージングに拡張可能か？

主な発見

• $SE_2(3)$-ベースのEKFフレームワークは、姿勢・速度・位置の誤差に対して一貫した共通フレーム誤差表現を実現し、従来のEKF手法におけるフレーム不一致問題を解消した。
• 右不変 $SE_2(3)$-ベースのEKF定式化では、遷移行列から特定の加速度項が明確に削除され、数値的耐障害性が向上した。
• 腕のずれ誤差を考慮したNEDおよびECEFフレーム上において、左不変EKFと $SE_2(3)$-ベースのEKFが等価であることが実証され、理論的整合性が裏付けられた。
• 補助速度ベクトルを用いたNEDおよびECEFフレーム上での修正誤差ダイナミクスは、大規模な誤差初期値条件下での安定性と収束性を向上させた。
• $SE_2(3)$-ベースのスムージングアルゴリズムは、不変RTSスムージングよりも単純で直感的であり、全状態更新には行列指数関数と群演算が使用された。
• 実験により、$SE_2(3)$-ベースのEKFが大規模な初期誤差角を伴う初期統合航法において、従来手法を上回る耐障害性を示した。