Considering that the terrain-aided navigation (TAN) system based on iterated closest contour point (ICCP) algorithm diverges easily when the indicative track of inertial navigation system (INS) is large, Kalman filter is adopted in the traditional ICCP algorithm. The difference between matching result and INS output is used as the measurement of Kalman filter. In this way, the cumulative error of the INS can be corrected in time by filter feedback correction, and the indicative track used in ICCP is improved. The proper number of matching points is designated by comparing the simulation results of matching time and matching precision. Simulation experiments are carried out according to the ICCP algorithm and the mathematic model. And it is demonstrated that the integrated navigation system is effective in preventing the divergence of the indicative track for the underwater, long-term and high precision of the navigation system for autonomous underwater vehicles (AUV).