[Paper Review] 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 Networks18 citations

TL;DR

This paper proposes a generic, symbolic Kalman filter framework that embeds manifold structures into error-state Kalman filters for robotic systems on manifolds such as SO(3) and S². By introducing canonical operations like ⊕ and ⊟⊕, the method decouples manifold geometry from system dynamics, enabling automatic filter derivation and efficient C++ implementation with performance matching hand-engineered solutions in lidar-inertial navigation.

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.

Motivation & Objective

• To eliminate repetitive, error-prone derivations of on-manifold error-state Kalman filters for robotic systems.
• To formalize a canonical representation of robotic systems on manifolds using symbolic operations like ⊕ and ⊟⊕.
• To develop a generic, extensible Kalman filter framework that separates manifold structure from system dynamics.
• To enable high-dimensional robotic systems to be filtered efficiently without manual derivation of Jacobians and tangent space operations.
• To provide a practical, open-source C++ toolkit for plug-and-play integration into real-time robotic systems.

Proposed method

• Introduces a symbolic framework using ⊟⊕ and ⊕ operations to represent state and error propagation on manifolds.
• Defines a canonical system representation that abstracts manifold geometry from system dynamics, enabling generic filter formulation.
• Embeds manifold structures (e.g., SO(3), S²) into the Kalman filter by encoding their intrinsic geometry via operations on tangent spaces.
• Decouples the system model from the filter algorithm, allowing automatic generation of filter steps from a high-level system definition.
• Implements the framework as a C++ toolkit where users define the system model and invoke standard filter steps (propagate, update) via events.
• Supports full iterated Kalman filtering on manifolds of the form ℝ^m × SO(3)^k × S²^l and their sub-manifolds, with extensibility to other manifolds.

Experimental results

Research questions

• RQ1 Can a generic symbolic framework be designed to automate the derivation of error-state Kalman filters on arbitrary manifolds?
• RQ2 How can manifold-specific operations like ⊕ and ⊟⊕ be formalized to ensure consistency and correctness in state and error propagation?
• RQ3 To what extent can a symbolic, manifold-embedded Kalman filter achieve performance comparable to hand-optimized implementations?
• RQ4 Can such a framework be efficiently implemented in C++ to support real-time robotic applications?
• RQ5 How does the framework scale in performance and accuracy for high-dimensional systems like tightly-coupled lidar-inertial navigation?

Key findings

• The proposed symbolic Kalman filter framework enables automatic derivation of error-state filters without manual Jacobian computation or tangent space derivation.
• The C++ toolkit supports full iterated Kalman filtering on complex manifolds including SO(3) and S², with plug-and-play interface for system modeling.
• In a tightly-coupled lidar-inertial navigation system, the framework achieved filtering performance comparable to hand-engineered counterparts.
• The implementation demonstrated computation efficiency suitable for real-time robotic applications, with minimal user effort beyond system definition.
• The framework is extensible to other manifolds, with support for sub-manifolds of ℝ^m × SO(3)^k × S²^l out of the box.
• The open-source toolkit at https://github.com/hku-mars/IKFoM enables reproducibility and community extension of the framework.