Kalman Filters on Differentiable Manifolds (2102.03804v3)

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.

• The paper introduces a novel Kalman filtering approach using error-state techniques tailored for differentiable manifolds.
• It presents a canonical framework with manifold-specific operations that decouples constraints from system dynamics to improve accuracy.
• It provides a C++ open-source toolkit, demonstrating enhanced performance in lidar-inertial navigation and robust state estimation.

Overview of "Kalman Filters on Differentiable Manifolds"

The paper presents a novel approach to implementing Kalman filters for systems operating on differentiable manifolds. Traditional Kalman filters are predominantly designed for Euclidean spaces, limiting their applicability to real-world systems that often evolve on more complex manifolds such as $SO(3)$ or $\mathbb{S}^2$. This research proposes a systematic way to adapt Kalman filtering to these scenarios by leveraging manifold-specific operations.

Key Concepts and Methodology

1. System Representation on Manifolds:
   • The paper introduces a canonical representation for systems operating on manifolds using $\boxplus\backslash\boxminus$ operations and an additional $\oplus$ operation. This framework allows the state and measurement models of a system to be expressed naturally on the manifold, separating manifold constraints from system dynamics.
2. Error-State Kalman Filtering:
   • The research details a symbolic error-state iterated Kalman filter (IKFoM) framework that manages the nonlinearity and constraints of manifold spaces. By parameterizing the state trajectory in a minimal error state space, the filter provides improved linearization and numerical stability.
3. Toolkit Development:
   • A $C$++ package is developed to facilitate the implementation of these manifold-based Kalman filters. This open-source toolkit allows users to integrate manifold-specific constraints without manually deriving the filter equations, streamlining deployment.

Numerical Results and Claims

The paper substantiates the proposed framework through the implementation of a tightly-coupled lidar-inertial navigation system. The results demonstrate superior filtering performance and computational efficiency, evidencing the framework's capability to match hand-engineered solutions. Specifically, the symbolic Kalman filter provides enhanced accuracy in state estimation and exhibits robustness in scenarios with significant rotational dynamics.

Practical and Theoretical Implications

Practically, the proposed framework can substantially ease the integration of advanced filtering techniques in robotic systems, vehicular navigation, and sensor fusion tasks that inherently involve dynamic manifolds. Theoretical implications include the potential expansion of manifold-agnostic filtering strategies and the development of more generalized nonlinear system estimators.

Future Developments

Future research may explore extending this framework to encompass a wider variety of manifolds and applications. Further advancements could include the refinement of computational techniques to reduce overhead and enhance real-time processing capabilities in embedded systems.

In summary, this research presents a comprehensive and adaptable Kalman filtering approach tailored for differentiable manifolds, offering significant potential for a broad range of applications in control systems and beyond.