A Robust and Adaptive Complementary Kalman Filter Based on Mahalanobis Distance for Ultra Wideband/Inertial Measurement Unit Fusion Positioning

摘要

Ultra wideband (UWB) has been a popular technology for indoor positioning due to its high accuracy. However, in many indoor application scenarios UWB measurements are influenced by outliers under non-line of sight (NLOS) conditions. To detect and eliminate outlying UWB observations, we propose a UWB/Inertial Measurement Unit (UWB/IMU) fusion filter based on a Complementary Kalman Filter to track the errors of position, velocity and direction. By using the least squares method, the positioning residual of the UWB observation is calculated, the robustness factor of the observation is determined, and an observation weight is dynamically set. When the robustness factor does not exceed a pre-defined threshold, the observed value is considered trusted, and adaptive filtering is used to track the system state, while the abnormity of system state, which might be caused by IMU data exceptions or unreasonable noise settings, is detected by using Mahalanobis distance from the observation to the prior distribution. When the robustness factor exceeds the threshold, the observed value is considered abnormal, and robust filtering is used, whereby the impact of UWB data exceptions on the positioning results is reduced by exploiting Mahalanobis distance. Experimental results show that the observation error can be effectively estimated, and the proposed algorithm can achieve an improved positioning accuracy when affected by outlying system states of different quantity as well as outlying observations of different proportion.