[論文レビュー] Embedding manifold structures into Kalman filters.

Dongjiao He, Wei Xu+1 more|arXiv (Cornell University)|Feb 7, 2021

Target Tracking and Data Fusion in Sensor Networks被引用数 18

ひとこと要約

本論文は、SO(3) や S² などの多様体上に存在するロボットシステム向けに、誤差状態カルマンフィルタに多様体構造を埋め込む汎用的で記号的なカルマンフィルタフレームワークを提案する。⊕ および ⊟⊕ といった標準的な演算子を導入することで、多様体幾何学とシステムダイナミクスを分離し、自動的なフィルタ導出と、手作業で最適化されたソリューションと同等のパフォーマンスを示す C++ 実装を可能にする。ライダーインertialナビゲーションにおける性能を達成している。

ABSTRACT

Error-state Kalman filter is an elegant and effective filtering technique for robotic systems operating on manifolds. To avoid the tedious and repetitive derivations for implementing an error-state Kalman filter for a certain system, this paper proposes a generic symbolic representation for error-state Kalman filters on manifolds. Utilizing the $\boxplus\backslash\boxminus$ operations and further defining a $\oplus$ operation on the respective manifold, we propose a canonical representation of the robotic system, which enables us to separate the manifold structures from the system descriptions in each step of the Kalman filter, ultimately leading to a generic, symbolic and manifold-embedding Kalman filter framework. This proposed Kalman filter framework can be used by only casting the system model into the canonical form without going through the cumbersome hand-derivation of the on-manifold Kalman filter. This is particularly useful when the robotic system is of high dimension. Furthermore, the manifold-embedding Kalman filter is implemented as a toolkit in $C$++, with which an user needs only to define the system, and call the respective filter steps (e.g., propagation, update) according to the events (e.g., reception of input, reception of measurement). The existing implementation supports full iterated Kalman filtering for systems on manifold $\mathcal{S} = \mathbb{R}^m imes SO(3) imes \cdots imes SO(3) imes \mathbb{S}^2 imes \cdots imes \mathbb{S}^2 $ or any of its sub-manifolds, and is extendable to other types of manifold when necessary. The proposed symbolic Kalman filter and the developed toolkit are verified by implementing a tightly-coupled lidar-inertial navigation system. Results show superior filtering performances and computation efficiency comparable to hand-engineered counterparts. Finally, the toolkit is opened sourced at https://github.com/hku-mars/IKFoM.

研究の動機と目的

• ロボットシステムにおける多様体上での誤差状態カルマンフィルタの繰り返しで生じがちな、誤りを伴いやすい導出作業を排除すること。
• SO(3) や S² などの多様体上でのロボットシステムを、⊕ や ⊟⊕ といった記号的演算を用いて標準的表現で形式化すること。
• 多様体構造とシステムダイナミクスを分離する汎用的かつ拡張可能なカルマンフィルタフレームワークの開発。
• 高次元のロボットシステムにおいて、ヤコビ行列や接空間演算の手動導出なしに、効率的なフィルタリングを可能にすること。
• リアルタイムのロボットシステムへの即時統合を可能にする実用的でオープンソースの C++ ツールキットの提供。

提案手法

• 多様体上での状態および誤差伝播を表現するために、⊕ および ⊟⊕ 演算を用いた記号的フレームワークを導入。
• 多様体幾何学をシステムダイナミクスから抽象化する標準的システム表現を定義し、汎用的なフィルタ定式化を可能にする。
• SO(3) や S² などの多様体構造を、接空間上の演算を用いてその内在的幾何学を符号化することで、カルマンフィルタに埋め込む。
• システムモデルとフィルタアルゴリズムを分離し、高レベルのシステム定義から自動的にフィルタステップを生成可能にする。
• ユーザーがシステムモデルを定義し、イベントを介して標準的なフィルタステップ（伝搬、更新）を呼び出せる C++ ツールキットとして実装。
• ℝ^m × SO(3)^k × S²^l 及びその部分多様体を含む、複雑な多様体上での完全反復カルマンフィルタリングをサポート。他の多様体への拡張性も有する。

実験結果

リサーチクエスチョン

• RQ1 任意の多様体上での誤差状態カルマンフィルタの導出を自動化できる汎用的で記号的なフレームワークを設計できるか？
• RQ2 状態および誤差伝播における多様体固有の演算子 ⊕ や ⊟⊕ をどのように形式化すれば、一貫性と正しさを保証できるか？
• RQ3 記号的で多様体埋め込み型のカルマンフィルタが、手作業で最適化された実装と同等のパフォーマンスを達成できるか？
• RQ4 このようなフレームワークを C++ で効率的に実装し、リアルタイムのロボットアプリケーションをサポートできるか？
• RQ5 密結合型ライダーインertialナビゲーションのような高次元システムにおいて、このフレームワークのパフォーマンスと精度はどのようにスケーリングするか？

主な発見

• 提案された記号的カルマンフィルタフレームワークにより、ヤコビ行列の手動計算や接空間の導出なしに、誤差状態フィルタの自動導出が可能になった。
• C++ ツールキットは、SO(3) や S² を含む複雑な多様体上での完全反復カルマンフィルタリングをサポートし、システムモデリング用の即挿しインターフェースを備えている。
• 密結合型ライダーインエラルナビゲーションシステムにおいて、手作業で最適化された実装と同等のフィルタリングパフォーマンスを達成した。
• 実装はリアルタイムのロボットアプリケーションに適した計算効率を示し、システム定義の後は最小限のユーザー作業で済んだ。
• 他の多様体への拡張性を備えており、ℝ^m × SO(3)^k × S²^l の部分多様体は、標準でサポートされている。
• https://github.com/hku-mars/IKFoM に公開されたオープンソースのツールキットにより、フレームワークの再現性とコミュニティによる拡張が可能になった。