Terrain-aided strapdown inertial navigation system is a geophysical field positioning technique for long-endurance underwater vehicles, whose key technology is the matching algorithm. Aiming at transcending the limitations of current batch matching algorithms in accuracy due to local convergence and neglect of the SINS continuous trajectory information, we propose a two-stage combined matching algorithm. First, a coarse matching position is obtained based on the terrain mean factor search and the maximum likelihood estimation utilizing the multi-beam measured surface elevations. Then we implement the contour-based fine matching by introducing inertial constraint and proportionality factor, and propose a new numerical solution for the trajectory transformation matrix, where the proportionality factor and transformation matrix are both solvable independently. Finally, the simulations and field test results on real grid maps with 10 m resolution show that the proposed algorithm is superior to the previous algorithms and its feasibility in practical applications, and the matching accuracy reaches 1–2 grids.