Improved Particle Filter-Based Matching Method with Gravity Sample Vector for Underwater Gravity-Aided Navigation

Gravity-matching algorithm is a key to the gravity-aided inertial navigation system (INS). The traditional particle filter-based matching algorithm with a gravity sample vector is efficient. However, the range of particle filter and the probability model of the actual location are not specified in the algorithm. An improved particle filter-based matching algorithm with a gravity sample vector is proposed. Because of the high short-term accuracy of INS, the error range of INS in a short period is analyzed in a polar coordinate system in this algorithm. First, the attitude error angle model of INS is established. Relative angle error is proposed to calculate latitudes and longitudes of particles in the fan area at any position. Then a particle filter embedded in a particle filter is proposed to calculate the error range of the real position and establish the probability model of this position. Finally, in order to reduce the matching error, the relative displacements of the positions of the particles and the upper matching positions are added to the weights of the particles. Simulation results show that the proposed method has higher accuracy and better robustness.