Probabilistic map matching for robust inertial navigation aiding

Robust aiding of inertial navigation systems in GNSS-denied environments is critical for the removal of accumulated navigation error caused by the drift and bias inherent in inertial sensors. One way to perform such an aiding uses matching of geophysical measurements, such as gravimetry, gravity gradiometry or magnetometry, with a known geo-referenced map. Although simple in concept, this map matching procedure is challenging: the measurements themselves are noisy; their associated spatial location is uncertain; and the measurements may match multiple points within the map (i.e. non-unique solution). In this paper, we propose a probabilistic multiple hypotheses tracker to solve the map matching problem and allow robust inertial navigation aiding. Our approach addresses the problem both locally, via probabilistic data association, and temporally by incorporating the underlying platform kinematic constraints into the tracker. The map matching output is then integrated into the navigation system using an unscented Kalman filter. Additionally, we present a statistical measure of local map information density -- the map feature variability -- and use it to weight the output covariance of the proposed algorithm. The effectiveness and robustness of the proposed algorithm are demonstrated using a navigation scenario involving gravitational map matching.