Improvement of TERCOM aided inertial navigation system by velocity correction

TERCOM is a terrain referenced navigation system using batch processing method which determines vehicle´s current position by comparing a series of terrain height measurements with database. Generally, it is known that TERCOM operates well over terrain with high roughness. However, if the attitude and velocity error of INS is not corrected, the navigation solution of conventional TERCOM aided INS can diverge. This paper describes the TERCOM aided INS algorithm and show the divergence problem. To solve the problem, we propose an improved TERCOM aided INS algorithm which corrects the velocity error of INS using linear Kalman filter, even though the navigation system does not have a speedometer. It is shown that the proposed TERCOM algorithm can prevent divergence of navigation solution based on computer simulations.