Robust geomagnetic aided inertial navigation of underwater vehicles using the ICP algorithm

In this paper an algorithm is presented for positioning based on inertial navigation aided by using geomagnetic field intensity. The idea is based on matching noisy samples of geomagnetic intensities by the magnetometers and a priori geomagnetic map. The traditional Iterative Closest Point (ICP) algorithm is then applied in combination with the Inertial Navigation System (INS) to update its cumulative errors. Simulation experiments based on the actual geomagnetic reference data have also been performed for the validation of the proposed algorithm.