Dual Preintegration for Relative State Estimation
Ruican Xia, Hailong Pei
TL;DR
The paper tackles relative state estimation for two 6-DoF agents by addressing nonlinear rotation-induced linearization errors through a dual IMU preintegration approach. It formulates RSE as a MAP smoothing problem and introduces a dual preintegration factor that fuses IMU measurements from both platforms to constrain consecutive relative states, enabling efficient relinearization. The authors provide rigorous observability analysis showing bias observability under relative rotational excitation and characterize unobservable subspaces under degenerate motions, supported by simulations and VR field tests. The results demonstrate improved accuracy and robustness over state-of-the-art methods, with practical impact for VR controller tracking and multi-robot relative navigation where precise, drift-free relative poses are critical.
Abstract
Relative State Estimation perform mutually localization between two mobile agents undergoing six-degree-of-freedom motion. Based on the principle of circular motion, the estimation accuracy is sensitive to nonlinear rotations of the reference platform, particularly under large inter-platform distances. This phenomenon is even obvious for linearized kinematics, because cumulative linearization errors significantly degrade precision. In virtual reality (VR) applications, this manifests as substantial positional errors in 6-DoF controller tracking during rapid rotations of the head-mounted display. The linearization errors introduce drift in the estimate and render the estimator inconsistent. In the field of odometry, IMU preintegration is proposed as a kinematic observation to enable efficient relinearization, thus mitigate linearized error. Building on this theory, we propose dual preintegration, a novel observation integrating IMU preintegration from both platforms. This method serves as kinematic constraints for consecutive relative state and supports efficient relinearization. We also perform observability analysis of the state and analytically formulate the accordingly null space. Algorithm evaluation encompasses both simulations and real-world experiments. Multiple nonlinear rotations on the reference platform are simulated to compare the precision of the proposed method with that of other state-of-the-art (SOTA) algorithms. The field test compares the proposed method and SOTA algorithms in the application of VR controller tracking from the perspectives of bias observability, nonlinear rotation, and background texture. The results demonstrate that the proposed method is more precise and robust than the SOTA algorithms.