Geometric State Fusion for Autonomous Agents: A Comparative Analysis of Dual Quaternion Observer and Kalman Filters

Abstract

Autonomous agents operating in dynamic 3D environments require robust pose estimation that preserves the geometric structure of rigid-body motion. Traditional filtering approaches often decouple rotation and translation, leading to kinematic inconsistencies. This paper presents a comprehensive benchmark of three geometrically-aware sensor fusion architectures: a Dual Quaternion Geometric Observer (GeoDQ) using Screw Linear Interpolation (SCLERP), a manifold-aware Unscented Kalman Filter (UKF-M), and an Error-State Kalman Filter (ESKF). We reformulated the 6DoF sensor fusion problem on the SE(3) manifold to enable coordinate-free uncertainty propagation and evaluated these approaches on the entire RoNIN dataset (35 trajectories) using optimized JIT-compiled implementations. Extensive experimental results demonstrate that the proposed GeoDQ method outperforms filtering baselines by more than an order of magnitude in accuracy, reducing the Root Mean Square Error (RMSE) to 0.0043 m compared to ESKF (0.141 m) and UKF-M (0.231 m). Furthermore, robustness analysis reveals that the geometric observer maintains superior tracking stability under sparse position updates, significantly mitigating drift where filtering methods degrade. Despite the mathematical complexity of dual quaternion algebra, the optimized observer exhibits superior computational efficiency, executing faster than both the standard ESKF and UKF-M implementations. This performance confirms that dual quaternion-based observers provide a mathematically rigorous yet computationally lightweight solution suitable for high-frequency real-time estimation on resource-constrained embedded systems.

Keywords: Geometric State Fusion, Dual Quaternion Interpolation, Error-State Kalman Filter (ESKF), Unscented Kalman Filter (UKF), SE(3) Pose Estimation, SCLERP, Robust Navigation, Computational Efficiency.

Benchmark Summary (RoNIN Dataset)

We evaluated the algorithms on 35 distinct trajectories. The table below summarizes the aggregated performance metrics, comparing standard Python implementations against Numba-optimized (JIT) versions.

Key Finding: The proposed GeoDQ (JIT) architecture achieves the lowest tracking error while maintaining the fastest execution time. Notably, the standard deviation for GeoDQ is significantly lower (± 0.0009) compared to ESKF (± 0.0213) and UKF-M (± 0.0378), indicating superior consistency.

Global Robustness Analysis

Global Robustness Analysis averaged over all 35 RoNIN trajectories. The plot illustrates the degradation of RMSE (Log Scale) as the position update interval increases (simulating VO/GPS outages). The GeoDQ observer (Red/Dashed) demonstrates exceptional stability, maintaining a smooth error curve throughout the entire test range. Unlike Kalman Filter variants (ESKF, UKF), which show rapid error accumulation as updates become sparse, the geometric observer effectively mitigates drift even under severe data decimation, confirming its superior structural preservation of the SE(3) state.

Qualitative Tracking Performance

Example of trajectory tracking on sequence a000_11. The 3D trajectory (top-left) and Top-Down projection (top-right) illustrate the tracking path of ESKF (Purple), UKF-M (Blue), and the proposed GeoDQ (Red/Green dashed) against Ground Truth (Black).

Example of trajectory tracking on sequence a025_2. Comparison of estimated paths against ground truth, highlighting drift reduction by the Geometric Observer.

Example of trajectory tracking on sequence a043_3. The GeoDQ observer maintains tighter convergence to the ground truth path compared to filter-based approaches.

Citation

If you find this project useful, please consider citing: