Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

Nonlinear Observer Design for Visual-Inertial Odometry

Mouaad Boughellaba, Abdelhamid Tayebi, James R. Forbes, Soulaimane Berkane

TL;DR

This work addresses robust VIO in 3D by encoding the full extended state (pose, velocity, gravity, and landmarks) on a novel matrix Lie group SE_{3+n}(3). A nonlinear geometric observer with a cascaded structure decouples rotational and translational dynamics, yielding a Riccati-based globally exponentially stable translational estimator and an almost globally stable attitude estimator, ensuring almost global asymptotic convergence up to VIO’s unobservable directions. The method handles three measurement modalities (relative position, stereo bearing, monocular bearing) within a unified geometric framework and exhibits strong theoretical guarantees alongside empirical validation on simulations and EuRoC MAV data. This approach offers improved consistency over EKF- and gradient-based VIO methods and provides a principled path toward robust, real-time VIO in challenging real-world scenarios, with future work on IMU biases and embedded deployment.

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.

Nonlinear Observer Design for Visual-Inertial Odometry

TL;DR

This work addresses robust VIO in 3D by encoding the full extended state (pose, velocity, gravity, and landmarks) on a novel matrix Lie group SE_{3+n}(3). A nonlinear geometric observer with a cascaded structure decouples rotational and translational dynamics, yielding a Riccati-based globally exponentially stable translational estimator and an almost globally stable attitude estimator, ensuring almost global asymptotic convergence up to VIO’s unobservable directions. The method handles three measurement modalities (relative position, stereo bearing, monocular bearing) within a unified geometric framework and exhibits strong theoretical guarantees alongside empirical validation on simulations and EuRoC MAV data. This approach offers improved consistency over EKF- and gradient-based VIO methods and provides a principled path toward robust, real-time VIO in challenging real-world scenarios, with future work on IMU biases and embedded deployment.

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.
Paper Structure (22 sections, 5 theorems, 67 equations, 10 figures, 1 table)

This paper contains 22 sections, 5 theorems, 67 equations, 10 figures, 1 table.

Table of Contents

  1. Introduction
  2. RELATED WORK
  3. EKF- and optimization-based VIO
  4. Invariant and Equivariant Observers for VIO
  5. Preliminaries
  6. Notations
  7. Matrix Lie Group for VIO
  8. Uniform Observability and Continuous Riccati Equation
  9. Problem Statement
  10. System representation on a matrix Lie group
  11. Measurement models
  12. Relative position measurements
  13. Stereo-bearing measurements
  14. Monocular-bearing measurements
  15. Objective
  16. ...and 7 more sections

Key Result

Lemma 1

Suppose there exist constants $\delta > 0$, $\mu_v > 0$, and $\mu_q > 0$ such that for all $t \geq 0$: then the solution $P(t)$ of CRE, for all $t \geq 0$, satisfies the uniform bounds for some constants $0 < p_m \leq p_M < \infty$.

Figures (10)

  • Figure 1: Rigid body navigation in 3D with four unknown landmarks. The goal is to estimate the vehicle’s extended pose (position, velocity, and orientation) based on landmark position estimates and available measurements.
  • Figure 2: Structure of the proposed VIO observer.
  • Figure 3: The rigid body’s true trajectory with circular-horizontal and sinusoidal-vertical motion, along with the randomly distributed landmarks on the surrounding walls. The animation video can be found at https://youtu.be/ye6oyXOtPiM.
  • Figure 4: Comparison of root-mean-squared errors averaged over 17 Monte Carlo simulations for the proposed VIO scheme across the three measurement models \ref{['equ:measurements_3d']}, \ref{['equ:measurements_sb']}, and \ref{['equ:measurements_mb']}.
  • Figure 5: Time evolution of the rigid-body pose errors with $3\,\sigma$ bounds, obtained using the IEKF-based and the proposed VIO schemes under the measurement model defined in \ref{['equ:measurements_3d']}. The results are based on Monte Carlo simulations with 17 runs.
  • ...and 5 more figures

Theorems & Definitions (21)

  • Remark 1
  • Definition 1
  • Definition 2
  • Lemma 1: Bucy1967
  • Remark 2
  • Remark 3
  • Remark 4
  • Remark 5
  • Remark 6
  • Remark 7
  • ...and 11 more