A Matching Algorithm Based on the Nonlinear Filter and Similarity Transformation for Gravity-Aided Underwater Navigation

In the application of gravity-aided navigation, the matching algorithm is the key technique. A matching algorithm combined with filter recursive process and trajectory similarity transformation is proposed, which deals with a gravity anomaly database tabulated in the form of a digital model. Due to the difficulty of establishing a specific observation model and the low observability of system models derived from inertial navigation system (INS) error equations, a filter model based on vehicle position is established and one of the parameters is estimated by INS short-term output. The matching algorithm is a two-stage matching process. The first stage is based on point mass filter to achieve a preliminary matching result. The second stage implements precise matching via similarity transformation. Simulation and experimental tests show that compared to traditional algorithms, the proposed algorithm can achieve a more precise and reliable location result.