Indoor Fusion Positioning Based on "IMU-Ultrasonic-UWB" and Factor Graph Optimization Method

摘要

This paper presents a high-precision positioning system that integrates ultra-wideband (UWB) time difference of arrival (TDoA) measurements, inertial measurement unit (IMU) data, and ultrasonic sensors through factor graph optimization. To overcome the shortcomings of standalone UWB systems in non-line-of-sight (NLOS) scenarios and the inherent drift associated with inertial navigation, we developed a novel hybrid fusion framework. First, a dynamic covariance estimation mechanism is incorporated, which automatically adjusts measurement weights based on real-time channel conditions. Then, a tightly-coupled sensor fusion architecture is employed, utilizing IMU pre-integration theory for temporal synchronization. Finally, a sliding-window factor graph optimization backend is utilized, incorporating NLOS mitigation constraints. Experimental results in complex indoor environments show a 38\% improvement in positioning accuracy compared to conventional Kalman filter-based approaches, achieving a 12.3 cm root mean square (RMS) error under dynamic motion conditions. The system maintains robust performance even with intermittent UWB signal availability, down to a 40\% packet reception rate, effectively suppressing IMU drift through multi-modal constraint fusion. This work offers a practical solution for applications that require reliable indoor positioning in GPS-denied environments.