A particle filter-based matching algorithm with gravity sample vector for underwater gravity aided navigation

Gravity matching algorithm is a key technique of gravity aided navigation for underwater vehicles. The reliability of traditional single point matching algorithm can be easily affected by environmental disturbance, which results in mismatching and decrease of navigation accuracy. Therefore, a particle filter (PF)-based matching algorithm with gravity sample vector is proposed. The correlation between adjacent sample points of inertial navigation system is considered in the vector matching algorithm in order to solve the mismatching problem. The current sampling point matching result is rectified by the vectors composed by the selected sampling points and matching point. The amount of selected sampling points is determined by the gravity field distribution and the real-time performance of the algorithm. A PF-based on Bayesian estimation is introduced in the proposed method to overcome the divergence disadvantage of the traditional point matching algorithm in some matching areas with obvious gravity variation. Simulation results prove that compared with the traditional methods, the proposed method is robust to the changes of gravity anomaly in the matching areas, with more accurate and reliable matching results.