[Paper Review] Kalman Filters on Differentiable Manifolds

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

Inertial Sensor and Navigation26 references27 citations

TL;DR

The paper develops a generic, symbolic Kalman filter framework for systems evolving on differentiable manifolds, introducing canonical manifold representations and an on-manifold Kalman filter toolkit in C++ for easy deployment.

ABSTRACT

Kalman filter is presumably one of the most important and extensively used filtering techniques in modern control systems. Yet, nearly all current variants of Kalman filters are formulated in the Euclidean space $\mathbb{R}^n$, while many real-world systems (e.g., robotic systems) are really evolving on manifolds. In this paper, we propose a method to develop Kalman filters for such on-manifold systems. Utilizing $\boxplus$, $\boxminus$ operations and further defining an oplus operation on the respective manifold, we propose a canonical representation of the on-manifold system. Such a canonical form enables us to separate the manifold constraints from the system behaviors in each step of the Kalman filter, ultimately leading to a generic and symbolic Kalman filter framework that are naturally evolving on the manifold. Furthermore, the on-manifold Kalman filter is implemented as a toolkit in $C$++ packages which enables users to implement an on-manifold Kalman filter just like the normal one in $\mathbb{R}^n$: the user needs only to provide the system-specific descriptions, and then call the respective filter steps (e.g., predict, update) without dealing with any of the manifold constraints. The existing implementation supports full iterated Kalman filtering for systems on any manifold composed of $\mathbb{R}^n$, $SO(3)$ and $\mathbb{S}^2$, 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 that the developed toolkit leads to superior filtering performances and computation efficiency comparable to hand-engineered counterparts. Finally, the toolkit is opened sourced at https://github.com/hku-mars/IKFoM to assist practitioners to quickly deploy an on-manifold Kalman filter.

Motivation & Objective

• Motivate filtering for systems whose states evolve on manifolds rather than Euclidean space.
• Provide a canonical, discrete-time representation that separates manifold constraints from system dynamics.
• Derive a fully iterated, symbolic error-state Kalman filter on manifolds (IKFoM).
• Develop an open-source C++ toolkit that hides manifold constraint handling from the user.
• Validate the framework with a lidar-inertial navigation system and real-world datasets.

Proposed method

• Define boxplus, boxminus, and a boxplus-like operation on manifolds to represent perturbations and state updates.
• Propose a canonical discrete-time form x_{k+1} = x_k ⊕_M (Δt f(x_k, u_k, w_k)).
• Derive the symbolic, iterated Kalman filter in the error-state form using the canonical representation.
• Provide differentiations and Jacobians for primitive manifolds (e.g., R^n, SO(3), S^2) and extendable to compound manifolds.
• Implement a C++ toolkit that automatically applies predict/update steps with manifold constraints.
• Demonstrate applicability to systems like lidar-inertial navigation and compare performance to hand-engineered counterparts.

Experimental results

Research questions

• RQ1 How can Kalman filtering be reformulated to operate directly on differentiable manifolds while preserving manifold constraints?
• RQ2 Can a canonical, generic representation enable a symbolic, iterated Kalman filter on manifolds without case-by-case derivations?
• RQ3 What is the impact of integrating manifold constraints into the Kalman filter on accuracy and efficiency for robotic systems?
• RQ4 Can an open-source toolkit enable practitioners to deploy on-manifold Kalman filters with minimal system-specific coding?

Key findings

• Introduces a canonical discrete-time representation and an on-manifold error-state Kalman filter (IKFoM).
• Shows that manifold constraints can be separated from system dynamics at each Kalman filter step within the canonical form.
• Develops a C++ toolkit that hides manifold treatment from users; users provide system descriptions and call predict/update.
• Verifies the framework by implementing a tightly-coupled lidar-inertial navigation system and real-world datasets.
• Reports superior filtering performance and computation efficiency comparable to hand-engineered counterparts (qualitative).
• Toolkit is open-sourced at https://github.com/hku-mars/IKFoM for broader adoption.