Aiming at the problem that the correlation extremum gravity matching algorithm has poor realtime performance and requires less initial error, a twostep correlation extremum gravity matching algorithm is proposed in this paper. Firstly, the fast mean square difference (MSD) algorithm is used for rough matching, then the appropriate rough matching result is extracted as the benchmark of the rigid transformation, and the inertial navigation system (INS) trajectory is transformed to the vicinity of the actual trajectory to generate a new trajectory to be matched. Finally, a precise searching is carried out near the new trajectory to be matched to find the least cost path. The effect of the twostep matching algorithm is verified through simulation experiments. Combining the advantages of the two methods, the gravity matching method can adapt to the case of the large initial error of INS, improve the efficiency and realtime performance, and maintain the high accuracy of the matching results.