Real-Time Millimeter-Accurate Underwater Pose Estimation Via Tightly-Coupled Fusion of Vision and Optical Tracking

Abstract

Precise and high-frequency state estimation is required for advanced underwater robotic applications such as physical interaction and agile control, yet no single sensor can simultaneously provide both high accuracy and high update rates. Vision-based methods offer high-frequency updates but suffer from drift, while optical tracking systems are highly accurate but may not provide sufficiently high update rates for real-time control loops. This paper presents a tightly-coupled sensor fusion framework that combines a high-frequency (62 FPS) monocular vision-based pose estimator with a high-accuracy (millimeter-level) optical tracking system. Our approach uses a visual estimator for high-frequency state propagation—with a latent variable motion model to compensate for underwater disturbances—while the optical tracker provides periodic corrections. In a controlled underwater testbed, this achieves a position RMSE of 5.65 mm at 62 FPS, improving accuracy 1.6x compared to the best baseline method (EfficientPose + EKF: 9.20 mm) and 6.4x compared to vision-only estimation (36 mm). Our dataset and code are available upon request.