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

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.