An improved autonomous underwater vehicle federal integrated navigation method based on two-stage position matching technology is proposed in this study to solve the problem of positioning divergence caused by the defects of error accumulation in an inertial navigation system. The improved two-stage position matching method includes the acquisition and preprocessing of the strap-down inertial navigation system and indicates track sequences and elevation measurement sequences. The first-stage position matching is based on iterative closest contour point, while the second-stage position matching is based on an affine algorithm. Simulation results indicate that the proposed algorithm solves the problem of the limitation of the rigid transformation of traditional iterative closest contour point and effectively resolves the matching failure caused by the large initial position error of long-time navigation.