Indoor Fusion Positioning Based on "IMU-Ultrasonic-UWB" and Factor Graph Optimization Method
Fengyun Zhang, Jia Li, Xiaoqing Zhang, Shukai Duan, Shuang-Hua Yang
TL;DR
This work tackles indoor localization in GPS-denied environments where UWB alone suffers from NLOS biases and IMU-based navigation drifts. It proposes a multi-modal fusion framework that tightly couples UWB-TDoA, IMU, and ultrasonic measurements within a Lie-group $SE(3)$ factor graph, employing IMU pre-integration and a sliding-window optimization with NLOS mitigation. The approach introduces dynamic covariance weighting, ultrasonic-assisted initialization, and robust residuals, achieving a 12.3 cm RMS under dynamic motion and a 38% accuracy improvement over conventional Kalman-filter methods. Real-time performance is demonstrated on an embedded platform, with robustness against intermittent UWB signal and strong applicability to autonomous robotics, AR, and industrial automation in GPS-denied indoor spaces.
Abstract
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.