Robust adaptive collaborative navigation algorithm for UWB Non line of sight and Multipath in underground spaces

This article addresses the Non Line of Sight (NLOS) and multipath issues in ultra wide band (UWB) range sensors for collaborative navigation of multiple unmanned vehicles in underground spaces. An anomaly detection algorithm Robust Random Cut Forest (RRCF) is used. Based on this, an improved robust adaptive Square Root Cubature Kalman filter (SRCKF) collaborative navigation algorithm based on Huber correction and Myers Tapley adaptation is proposed. Huber correction is used to correct outliers after RRCF detection, and Myers Tapley method adaptively processes continuous time-varying anomaly ranging errors caused by possible long-term non line of sight and multipath. Finally, This article compares the performance of the SRCKF algorithm, robust algorithm without RRCF detection, and robust algorithm with RRCF detection when there are outliers through simulation. The average positioning error of the SRCKF algorithm is 0.34 m, and the average positioning error of the RRCF Huber collaborative navigation algorithm is 0.30 m, which improves performance by 11.8% measured by the average positioning error. It also eliminates the abnormal error caused by ranging outliers, verifying the performance of the RRCF Huber algorithm. In addition, the performance of the RRCF Huber algorithm with and without Myers Tapley adaptation is compared when there is continuous anomalous ranging error. It is found that the RRCF Huber algorithm performs poorly in the presence of continuous anomalous ranging error, with RMSE value of 0.89 m, after adding Myers Tapley adaptation, the RMSE value reduces to 0.42 m, the algorithm's positioning performance improved by 52.8% measured by RMSE values, verifying the performance of the Myers Tapley adaptation.