UWB and IMU Fusion Positioning Based on ESKF with TOF Filtering

Abstract

Focusing on the problem that UWB and IMU fusion localization has a poor resistance to NLOS, we propose a UWB and IMU fusion algorithm based on the error state Kalman filter combined with TOF filtering. IMU errors are used as the system state variables. The error state is always close to 0, which makes the error state Kalman filter work away from singular values, and gimbal lock, and ensures the rationality and effectiveness of linearization. Then, the differences between the UWB ranging values and the pseudo ranges from the IMU solved position to the UWB anchors are used as the systematic observation to obtain the optimal estimation of the IMU errors. However, UWB measurements are constrained by NLOS, which leads to the reduction of positioning accuracy of the fusion positioning system in the NLOS environment. To solve this problem, this paper establishes a Kalman filter to process UWB measurements by using the property of changing UWB range values at adjacent moments during vehicle movement, which effectively reduces NLOS errors. The experimental results show that the error state Kalman filter combined with the TOF filtering has higher localization accuracy and better numerical stability compared with the error state Kalman filter, which enhances the robustness of the localization system under interference.