Finite-time Stable Pose Estimation on TSE(3) using Point Cloud and Velocity Sensors

This work presents a finite-time stable pose estimator (FTS-PE) for rigid bodies undergoing rotational and translational motion in three dimensions, using measurements from onboard sensors that provide position vectors to inertially-fixed points and body velocities. The FTS-PE is a full-state observer for the pose (position and orientation) and velocities and is obtained through a Lyapunov analysis that shows its stability in finite time and its robustness to bounded measurement noise. Further, this observer is designed directly on the state space, the tangent bundle of the Lie group of rigid body motions, SE(3), without using local coordinates or (dual) quaternion representations. Therefore, it can estimate arbitrary rigid body motions without encountering singularities or the unwinding phenomenon and be readily applied to autonomous vehicles. A version of this observer that does not need translational velocity measurements and uses only point clouds and angular velocity measurements from rate gyros, is also obtained. It is discretized using the framework of geometric mechanics for numerical and experimental implementations. The numerical simulations compare the FTS-PE with a dual-quaternion extended Kalman filter and our previously developed variational pose estimator (VPE). The experimental results are obtained using point cloud images and rate gyro measurements obtained from a Zed 2i stereo depth camera sensor. These results validate the stability and robustness of the FTS-PE.

💡 Research Summary

The paper introduces a novel finite‑time stable pose estimator (FTS‑PE) that simultaneously estimates the full pose (orientation and position) and the body velocities of a rigid body moving in three‑dimensional space. The estimator relies solely on onboard measurements: a 3‑D point cloud providing relative position vectors of inertial landmarks and velocity measurements from an IMU (angular velocity, and optionally translational velocity). Unlike most existing approaches that operate in local coordinates or use unit quaternions/dual‑quaternions, the proposed observer is designed directly on the Lie group SE(3) and its tangent bundle TSE(3). This global representation eliminates singularities and the unwinding phenomenon.

The core of the design is a Morse‑Lyapunov function derived from a generalized Wahba cost with a symmetric weighting matrix. By proving that this cost is a Morse function on SO(3) under mild conditions, the authors construct a continuous feedback term that drives the estimation error dynamics to the identity in finite time. The error dynamics are expressed in terms of the attitude error Q = R ĤRᵀ and the position error χ = b – Qĥb, with associated angular and translational error velocities ω and υ. A Lyapunov analysis shows that, for appropriate gain choices and a non‑linear exponent α∈(0,1), the error converges to zero within a bounded settling time, and remains bounded in the presence of bounded measurement noise, thereby guaranteeing robustness.

Two observer variants are presented: (1) a full‑measurement version that uses both angular and translational velocity data together with the point cloud, and (2) a reduced version that requires only angular velocity measurements, estimating translational velocity by integrating the point‑cloud information over time. Both operate directly on SE(3), avoiding any need for coordinate transformations or quaternion normalization.

Simulation results demonstrate that, even with large initial errors, the FTS‑PE drives the pose error below 10⁻³ in less than 0.2 s, outperforming a previously developed variational pose estimator (VPE) and a dual‑quaternion extended Kalman filter (DQ‑EKF) in convergence speed and noise resilience. Experimental validation is performed with a ZED 2i stereo depth camera equipped with an IMU. Real‑time implementation at ~30 Hz shows stable convergence, with pose errors staying within a few centimeters and sub‑degree orientation errors, confirming the practical viability of the method.

In summary, the work delivers a globally defined, continuous, almost‑globally finite‑time stable pose observer that is model‑free, robust to bounded sensor noise, and suitable for autonomous vehicle applications where GPS is unavailable. Future directions suggested include extending the framework to handle external disturbances, bias estimation for inertial sensors, and multi‑sensor fusion for enhanced robustness.