A Combinational Underwater Aided Navigation Algorithm Based on TERCOM/ICCP and Kalman Filter

In order to solve the problem that the iterated closest contour point(ICCP) algorithm diverges easily when the initial INS error is large, the terrain contour matching (TERCOM) algorithm is firstly used to reduce the initial INS error, then ICCP algorithm is used to obtain the best matching position. Two matching difference is used as the measurement of Kalman filter, INS error is corrected and the optimal estimate is obtained. The correlative analysis MSD is only introduced in the coarse matching stage, and the sliding window is used in the precise matching stage to improve the algorithm efficiency. Simulations are performed and the results show that the proposed combinational algorithm matching process is more stable and the precision is higher than traditional algorithm.