Underground precise positioning algorithm based on Kalman filter and weighted LM algorithm

In view of problem that positioning result of underground proximity detection device based on UWB precise positioning is susceptible to noise such as non-line of sight (NLOS) error, an underground precise positioning algorithm based on Kalman filter and weighted LM algorithm was proposed. Priori estimation value of tag card coordinates is obtained by Kalman filter prediction process; distance between the estimatied coordinates and each anchor node is calculated by using geometric relationship, the calculated distance is compared with direct measuring value of the detector, and ranging weight of each anchor node is allocated according to difference of the calculated distance and measured distance; weight matrix and ranging matrix are substituted into the weighted LM algorithm as the measured value to obtain intermediate result of the tag card coordinates; the intermediate result is substituted into Kalman filter update process to obtain final coordinates of the tag card. The test results show that compared with the multilateral positioning method, the underground precise positioning algorithm based on Kalman filter and weighted LM algorithm can improve positioning accuracy by more than one time without affecting positioning speed, and effectively reducing the interference of NLOS error and other noises.