Relative Pose-Velocity Estimation Using Dual IMU Measurements and Relative Position Sensing

TL;DR

This paper develops a deterministic Riccati observer and nonlinear filter for estimating relative pose and velocity between a vehicle and a moving target using dual IMUs and relative position or bearing measurements, with proven convergence conditions.

Contribution

It formulates the relative dynamics on SE_2(3), designs a Riccati observer and a nonlinear filter, and analyzes observability conditions for global exponential convergence.

Findings

The Riccati observer guarantees convergence under persistence-of-excitation of target acceleration.

The nonlinear filter on SO(3) achieves almost global asymptotic stability.

Simulation results validate the effectiveness of the proposed estimation methods.

Abstract

This paper addresses the problem of estimating the relative pose (position and orientation) and velocity of a vehicle with respect to a moving target, where both are equipped with Inertial Measurement Units (IMUs), assuming the availability of relative position or bearing measurements. The body-target relative dynamics are formulated on $\mathbf{SE}_2(3)$ and recast into a linear time-varying (LTV) model in the ambient space $\mathbb{R}^{15}$, on which a deterministic Riccati observer is designed. We analyze the uniform observability (UO) conditions required to guarantee global exponential convergence of the estimation error in the ambient space for both measurement cases. In the case of relative position measurements, UO requires only a persistence-of-excitation condition on the target acceleration, whereas for bearing measurements, additional conditions are required. Building on this, a nonlinear complementary filter on $\mathbf{SO}(3)$ is designed to provide a smooth estimate of the orientation component of the state with almost global asymptotic stability. Finally, simulation results are provided to validate the proposed solution.