Symbolic Representation and Toolkit Development of Iterated Error-State Extended Kalman Filters on Manifolds

Error-state extended Kalman filter (ESEKF) is one of the extensively used filtering techniques in robot systems. There are many works that cast ESEKF on manifolds to improve consistency. However, most of these works are designed case by case, which makes it difficult to extend to new manifolds. In this article, we propose a generic method to formulate the iterated error-state extended Kalman filter (IESEKF) on manifolds, which aims to facilitate the deployment of IESEKF for on-manifold systems (e.g., lidar-inertial and visual-inertial systems). First, a canonical on-manifold representation of the robot system is proposed, based on which, an on-manifold IESEKF framework is formulated and solved by linearization at each estimation point. The proposed framework has two main advantages, one is that an equivalent error-state system is derived from linearization, which is minimally parameterized without any singularities in practice. And the other is that in each step of IESEKF, the manifold constraints are decoupled from the system behaviors, ultimately leading to a generic and symbolic IESEKF framework that naturally evolving on manifolds. Based on the separation of manifold constraints from the system behaviors, the on-manifold IESEKF is implemented as a toolkit in C++ packages, with which 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 manifold constraints. The existing implementation supports full iterated Kalman filtering for versatile systems on manifold $\mathcal {M} = \mathbb {R}^{m}\!\times SO(3)\!\times \!\cdots \!\times \!SO(3)\!\times \!SE_{N}(3)\!\times \!\cdots \!\times \!SE_{N}(3)\!\times \mathbb {S}^{2} \times \cdots \times \mathbb {S}^{2}$ or any of its submanifolds, and is extendable to other types of manifold when necessary. The proposed symbolic IESEKF and the developed toolkit are verified by implementing two filter-based tightly coupled lidar-inertial navigation systems. Results show that, while greatly facilitating the EKF deployment, the developed toolkit leads to estimation performances and computation efficiency comparable to hand-engineered counterparts. Finally, the toolkit is open-sourced at https://github.com/hku-mars/IKFoM . The aimed application is the real-time state estimation of dynamic systems (e.g., robots) whose states are evolving on manifolds.