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

2025-04-02

AI Summary

This paper addresses the problem of high-precision pose and velocity estimation for rigid bodies. We propose a geometric unified observation framework based on the Lie group SE(5). By fusing IMU measurements with generic inertial-frame or body-frame measurements, we construct, for the first time on SE(5), a decoupled geometric error dynamics model—where translational error evolution mimics continuous-time Kalman filtering, enabling Riccati-equation-driven time-varying gain design and guaranteeing almost global asymptotic stability. Our approach overcomes limitations of conventional Euclidean-space modeling, achieving intrinsic decoupling of error dynamics, simplification of observer structure, and enhanced robustness. Extensive simulations—including stereo-camera-aided and GPS-aided inertial navigation systems—demonstrate its effectiveness. The method significantly improves the generality and engineering applicability of nonlinear geometric observers for high-accuracy state estimation.

Technology Category

• Intelligent Robots: State Estimation
• Knowledge Representation and Reasoning: Geometric, Spatial, and Temporal Reasoning
• Computer Vision: Motion & Tracking

Application Category

• Security and Privacy: Large-scale security measurements
• Systems and Infrastructure for Web, Mobile and WoT: Location- and context-aware Web and WoT applications and services
• Graph Algorithms and Modeling for the Web: Efficient manipulation of static and dynamic Web-related graphs

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 imes R^3 imes 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.

Problem

• Accurate pose estimation using IMU and inertial/body-frame measurements
• Reformulating vehicle dynamics within a geometric Lie group framework
• Simplifying nonlinear observer design for inertial navigation systems

Innovation

• Geometric framework for pose and velocity estimation
• Higher-dimensional Lie group embedding for decoupling
• Nonlinear observer with almost global asymptotic stability