A matching algorithm is a key technique in the gravity-aided inertial navigation system (INS). A matching algorithm combined with an iterated closest contour point (ICCP) algorithm and a point mass filter (PMF) is proposed. The algorithm involves a two-step matching process. First, the PMF based on vehicle position variable can obtain in real-time an instructional position given in a large initial position error. In particular, since the gravity anomaly database is tabulated in the form of a digital model, the parameter in the filter model is estimated by INS short-term displacement. Then, the ICCP algorithm with a sliding window can be employed for further matching. Simulation tests indicate that compared with the conventional ICCP algorithm, the proposed algorithm can achieve better results.