An algorithm for terrain-aided inertial navigation based on nonlinear optimal filtering

When the initial position error or the altimeter measurement noise is large, the BUAA Inertial Terrain-Aided Navigation (BITAN) algorithm based on extended Kalman filtering can not be located accurately. To solve this problem, we propose a modified BITAN algorithm based on nonlinear optimal filtering. The posterior probability density correction is obtained by using the prior probability density of the system's state transition model and the most recent observations. Hence, the local unobservable system caused by the measurement equation through terrain linearization is avoided. This algorithm is tested by using the digital elevation model and flight data, and is compared with BITAN. Results show that the accuracy of the proposed algorithm is higher than BITAN, and the robustness of the system is improved.