Logo
ScienceStack
Table of Contents
Fetching ...
Sign In

A Geometric Approach For Pose and Velocity Estimation Using IMU and Inertial/Body-Frame Measurements

Sifeddine Benahmed, Soulaimane Berkane, Tarek Hamel

TL;DR

This work tackles robust pose and velocity estimation for a rigid body using an IMU and generic inertial/body-frame measurements. It builds a nonlinear geometric observer on the higher-dimensional Lie group $\\mathrm{SE}_5(3)$ by introducing an auxiliary state to decouple attitude and translation, enabling fusion of diverse sensor outputs and Kalman-filter–like gain design via a Riccati equation. Under uniform observability, the approach guarantees almost global asymptotic stability of the estimation error, with a proof structure that separates attitude ISS and translation GES dynamics. Simulations on stereo- and GPS-aided INS demonstrate accurate convergence and effective noise rejection, highlighting the method’s generality and practical applicability to a wide range of sensing configurations.

Abstract

This paper addresses accurate pose estimation (position, velocity, and orientation) for a rigid body using a combination of generic inertial-frame and/or body-frame measurements along with an Inertial Measurement Unit (IMU). By embedding the original state space, $\so \times \R^3 \times \R^3$, within the higher-dimensional Lie group $\sefive$, we reformulate the vehicle dynamics and outputs within a structured, geometric framework. In particular, this embedding enables a decoupling of the resulting geometric error dynamics: the translational error dynamics follow a structure similar to the error dynamics of a continuous-time Kalman filter, which allows for a time-varying gain design using the Riccati equation. Under the condition of uniform observability, we establish that the proposed observer design on $\sefive$ guarantees almost global asymptotic stability. We validate the approach in simulations for two practical scenarios: stereo-aided inertial navigation systems (INS) and GPS-aided INS. The proposed method significantly simplifies the design of nonlinear geometric observers for INS, providing a generalized and robust approach to state estimation.

A Geometric Approach For Pose and Velocity Estimation Using IMU and Inertial/Body-Frame Measurements

TL;DR

This work tackles robust pose and velocity estimation for a rigid body using an IMU and generic inertial/body-frame measurements. It builds a nonlinear geometric observer on the higher-dimensional Lie group by introducing an auxiliary state to decouple attitude and translation, enabling fusion of diverse sensor outputs and Kalman-filter–like gain design via a Riccati equation. Under uniform observability, the approach guarantees almost global asymptotic stability of the estimation error, with a proof structure that separates attitude ISS and translation GES dynamics. Simulations on stereo- and GPS-aided INS demonstrate accurate convergence and effective noise rejection, highlighting the method’s generality and practical applicability to a wide range of sensing configurations.

Abstract

This paper addresses accurate pose estimation (position, velocity, and orientation) for a rigid body using a combination of generic inertial-frame and/or body-frame measurements along with an Inertial Measurement Unit (IMU). By embedding the original state space, , within the higher-dimensional Lie group , we reformulate the vehicle dynamics and outputs within a structured, geometric framework. In particular, this embedding enables a decoupling of the resulting geometric error dynamics: the translational error dynamics follow a structure similar to the error dynamics of a continuous-time Kalman filter, which allows for a time-varying gain design using the Riccati equation. Under the condition of uniform observability, we establish that the proposed observer design on guarantees almost global asymptotic stability. We validate the approach in simulations for two practical scenarios: stereo-aided inertial navigation systems (INS) and GPS-aided INS. The proposed method significantly simplifies the design of nonlinear geometric observers for INS, providing a generalized and robust approach to state estimation.

Paper Structure

This paper contains 16 sections, 4 theorems, 46 equations, 3 figures, 1 table.

Table of Contents

  1. Introduction
  2. Notation
  3. Problem Formulation
  4. Kinematic Model
  5. Objective
  6. Main Result
  7. Motivation
  8. Pre-Processing of Inertial- and Body-Frame Measurements: Unified Formulation
  9. Nonlinear Observer Design on $\mathrm{SE}_5(3)$
  10. Translational Error Dynamics and Gain Design
  11. Uniform Observability and Almost Global Asymptotic Stability
  12. Simulation Results
  13. Conclusion
  14. Proof of Proposition \ref{['proposition:reformuulation_of_the_measurements']}
  15. Proof of Lemma \ref{['lemma:dynamics_of_the_translaional_error_in_the_body_frame']}
  16. ...and 1 more sections

Key Result

Proposition 1

For any $i\in\mathcal{M}:=\{1,\cdots,m\}$, where $m=\mathbf{card}(y)/3$, there exists a reference vector (possibly time-varying) $r_i\in\mathds{R}^5$, such that the measurements in equation:vector_of_all_measurements can be reformulated as follows: where $\mathbf{y}_i=^{\top}$ and $\mathbf{r}_i=^{\top}$.

Figures (3)

  • Figure 1: Illustration of the proposed geometric estimation approach
  • Figure 2: Estimation errors and trajectories for Stereo-aided INS.
  • Figure 3: Estimation errors and trajectories for GPS-aided INS.

Theorems & Definitions (5)

  • Proposition 1
  • Lemma 1
  • Definition 1
  • Lemma 2
  • Theorem 1