Nonlinear Observer Design for Visual-Inertial Odometry

TL;DR

This paper introduces a geometric nonlinear observer framework on a novel Lie group structure for visual-inertial odometry, achieving almost global stability and robust state estimation in 3D space.

Contribution

It proposes a new Lie group-based geometric observer for VIO that outperforms traditional EKF methods in stability and robustness.

Findings

Achieves almost global asymptotic stability.

Demonstrates robustness through simulations and real-world experiments.

Provides consistent estimation of pose, velocity, gravity, and landmarks.

Abstract

This paper addresses the problem of Visual-Inertial Odometry (VIO) for rigid body systems evolving in three-dimensional space. We introduce a novel matrix Lie group structure, denoted SE_{3+n}(3), that unifies the pose, gravity, linear velocity, and landmark positions within a consistent geometric framework tailored to the VIO problem. Building upon this formulation, we design an almost globally asymptotically stable nonlinear geometric observer that tightly integrates data from an Inertial Measurement Unit (IMU) and visual sensors. Unlike conventional Extended Kalman Filter (EKF)-based estimators that rely on local linearization and thus ensure only local convergence, the proposed observer achieves almost global stability through the decoupling of the rotational and translational dynamics. A globally exponentially stable Riccati-based translational observer along with an almost global input-to-state stable attitude observer are designed such that the overall cascaded observer enjoys almost global asymptotic stability. This cascaded architecture guarantees robust and consistent estimation of the extended state, including orientation, position, velocity, gravity, and landmark positions, up to the VIO unobservable directions (i.e., a global translation and rotation about gravity). The effectiveness of the proposed scheme is demonstrated through numerical simulations as well as experimental validation on the EuRoC MAV dataset, highlighting its robustness and suitability for real-world VIO applications.