Research ArticleOriginal Article
Open Access

Probabilistic Map Matching for Robust Inertial Navigation Aiding

Xuezhi Wang, Christopher Gilliam, Allison Kealy, John Close, and Bill Moran
NAVIGATION: Journal of the Institute of Navigation June 2023, 70 (2) navi.583; DOI: https://doi.org/10.33012/navi.583

Abstract

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 estimated platform position from the output of map matching is then integrated into the navigation state 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.

Keywords

1 INTRODUCTION

In GNSS-denied (or contested) environments, platform navigation performance is dominated by the accuracy of onboard inertial sensors. Even with high-end inertial sensors, which exhibit extremely low bias and drift, it is not possible to avoid the build up of navigation errors over long-time frames (Titterton & Weston, 2004). Removing these accumulated navigation errors is crucial to retain the confidence of navigation accuracy (Groves, 2013). This removal, or correction, is achieved using one or more aiding sources that provide positional information, i.e., a position fix. Aiding sources can be categorized into three groups based on the technologies involved: 1) Radio-based aiding, which uses a transmitted ratio signal to obtain a position fix the canonical example is GNSS-based aiding. 2) Electromagnetic imaging, such as visual camera systems or synthetic aperture radar (SAR) imaging, which obtain a position fix by imaging the terrain around the platform and registering the corresponding image to known landmarks. 3) Geophysics-based aiding, which obtains a position fix by measuring geophysical quantities and matching these measurements to a known geo-referenced map process known as map matching. Examples of the geophysical quantities and their associated maps, include one or more of the elements of the gravitation vector and/or the gravity gradient tensor; the corresponding magnetic quantities; and bathymetry. In this paper, we focus on this geophysics-based aiding and present a novel map-matching algorithm.

Map-matching techniques are widely used in localization and navigation scenarios where GNSS is not readily available such as underwater, urban, or hostile environments. Based on how the measurements and corresponding maps are used, approaches to geophysical map-matching navigation can be split into two groups: implicit map matching and explicit map matching. Implicit map-matching techniques feed the geophysical measurements directly into statistical filters along with the inertial measurements. In this framework, the geo-referenced maps are used as lookup functions to compute the predicted geophysical measurements in the prediction step of the statistical filters. Due to the non-linear relationship between the estimation states and geophysical measurements, early approaches opted to use extended Kalman filters (EKF), to perform the estimation; examples include EKFs involving gravimetry (Affleck & Jircitano, 1990), gravity gradiometry (Jekeli, 2006), or both for submarine navigation Moryl et al. (1996). A performance analysis of a gravity gradiometry EKF was presented in (Lee et al., 2015). More recently, to avoid the linearisation present in the EKF, unscented Kalman filters (UKF), have been proposed for gravimetry (Wu et al., 2010) and gravity gradiometry (Gao et al., 2021). Finally, particle filters have been proposed for terrain-aided navigation using bathymetry data (Teixeira et al., 2017). A limitation of implicit map-matching techniques, however, is that the statistical filters need to be redesigned when either changing the type of geophysical measurements used or incorporating new geophysical quantities. A more flexible approach is found in explicit map matching.

Explicit map-matching techniques determine an estimate of the platform’s location by directly matching the geophysical measurement to a point in the map, i.e., the matching occurs explicitly in the map space. The resulting location estimate is then integrated into the navigation system in a similar way to a loosely coupled GNSS/INS system. Figure 1 shows a block diagram of a generic inertial navigation system (INS) with aiding from an explicit map-matching system. The key idea is to match the geophysical measurements s to a location in the map and then use this location to improve the INS position estimate x. The improved position estimate, xm, is then integrated into the full INS state vector, XINS. Although conceptually simple, this map-matching procedure is challenging for the following reasons. First, the geophysical measurements themselves are corrupted by sensor noise so the measurements will not match the map exactly. Second, the measurements may match multiple points within the map (i.e., non-unique solution). Finally, the locations where the measurements were acquired is of course uncertain. We term these challenges as the map measurement ambiguity problem and the development of a technique to resolve this problem is the main interest of this paper.

FIGURE 1
FIGURE 1

Illustration of a generic single recursion map-matching aided inertial navigation system (INS). The proposed map-matching block estimates the position of platform by filtering platform kinematic state and covariance Embedded Image taken from the INS with the map position identified by processing the geophysical measurement Sk. The estimated platform position and covariance Embedded Image are then feeded into the navigation integration filter for navigation position drifting correction.

In the literature, one approach to explicit map matching is to choose a position (or set of positions) in the map that minimize as standard error, such as mean square error or mean absolute error, between the geophysical measurement (or measurements) and the values in the map within a given region. However, as noted above, this is unlikely to yield a unique solution as multiple locations in the map may match the measurements. To solve this issue, the trajectory of the positions were constrained in (Wu et al., 2015 2017) using the observation that the relative INS position change is approximately equal to relative change between the true locations. In contrast, DeGregoria (2010) opted to constrain the problem by performing a joint minimization over all five of the independent elements of the gravity gradient tensor. Although straightforward, these approaches do not take account of the uncertainty in both the measurements and positional estimates, nor the structure of the map. An alternative set of approaches focuses on utilizing the non-uniqueness of a geophysical measurement in the map space. Specifically, a single scalar measurement belongs to an iso-contour of similar values in the geo-referenced map. Using this concept, Tuohy et al. (1996) proposed a generic map-matching technique for use with two or more maps; each measurement results in a different contour and the position of the platform is determined by the intersection of these contours. Measurement uncertainty was introduced by expanding the contours to a surface envelope. Building on this work, a single map approach based on iso-contours was proposed in (Kamgar-Parsi & Kamgar-Parsi, 1999). The authors posed the problem in terms of fitting a trajectory to a set of iso-contours based on initial position estimates and sensor measurements. To make the problem well posed, a stiffness regularization term was introduce to regularize the shape of the trajectory. However, linking the kinematic constraints of the platform’s motion to the regularization term is not straightforward. More recently, a map-matching method based on iterated closest contour point (ICCP) algorithm was proposed in (B. Wang et al., 2022). It essentially follows the same idea as described by Kamgar-Parsi & Kamgar-Parsi (1999) to fit the vehicle trajectory to a set of iso-contours, which are closest to the set of sensor measurements. The work uses gravitational contour maps generated from a gravity anomaly database. The same research group also proposed an improved particle-filter-based matching method in (B. Wang et al., 2021), where samples are drawn uniformly to cover range and yaw difference between two INS data points. While it shows an improved error performance compared with the standard gravity vector matching method, it is not clear how the particles are updated from gravimeter measurement in the matching process. As both the maps and the ICCP algorithm used by B. Wang et al. (2022) are not available, we implemented a standard ICCP algorithm (Han et al., 2018; Kamgar-Parsi & Kamgar-Parsi, 1999) to compare with the algorithm proposed in this work.

In this paper, we propose a probabilistic multiple-hypotheses tracking map-matching (PMHT-MM) algorithm to aid the onboard INS that performs platform localization by matching the onboard gravimetric sensor measurements with a geo-referenced data map. As shown in Figure 1, the estimated platform position is then integrated into the navigation state1 using an unscented Kalman filter (UKF) for navigation state compensation (Titterton & Weston, 2004). The INS compensation is treated as a recursive Bayesian estimation problem. At each epoch, the prior platform location distribution is obtained from the INS computed navigation state and updated by the gravimetric signal coordinates, estimated from the map matching via UKF. Map measurement ambiguity is addressed with the Expectation Maximization iterative approach, locally using a probabilistic data association, and temporally by considering the kinematic constraints of platform motion. Simulations using online data maps demonstrate that the proposed PMHT-MM aided INS can effectively eliminate long-term INS position errors caused by inertial sensor bias and drift in the GNSS denied environment. To the best of our knowledge, the use of probabilistic multiple-hypotheses tracking algorithm for map matching is novel and is a major contribution presented in this paper.

Following the introduction, the problem formulation is given in Section 2. We then present the PMHT-MM algorithm for INS aiding in Section 3. In Section 4, the performance of the proposed algorithm for aiding of INS using online maps is demonstrated in a realistic navigation scenario without GNSS. Results and discussions are presented, followed by conclusions in Section 5.

2 PROBLEM FORMULATION

Let s represent the sensed signal. This may reasonably be assumed a Gaussian distributed random variable Embedded Image, where s0 is the noiseless signal and σ the standard deviation of signal error. We assume, too, that the prior distribution of platform location is Gaussian with mean and covariance being xs and Σs, respectively. The signal location from the map, denoted by Embedded Image, based on the measurement s can be expressed as:

Embedded Image 1

Equation (1) is referred to as the map lookup function. Note that the prior location distribution of the platform, together with a threshold γ, defines an ellipsoidal area on the map centered at xs. Regardless of field measurement noise, the distribution of the map lookup function from a single measurement s can result in more than one likely location being compatible with the measurement. Figure 2 illustrates a one dimensional example of the map lookup process via Equation (1). We write Embedded Image for the collection of possible candidate locations of s from the measurement that also satisfy

Embedded Image 2

where γ is a constant probability threshold. Choice of the value of γ means that the ellipsoid area contains the signal location with a certain level of confidence. In this work, we refer to such an area as a search window. It will often be approximated by a rectangular area (rather than ellipsoidal); this provides significant computational efficiency with only a minor loss of accuracy. Figure 3 shows an example of a search window on a gravity map corresponding to the down component of the gravitational vector. It spreads over an area of 5 × 5 km2 with a collection Zm of 50 candidate locations of the measured signal with the mean of prior at the center. The data map is the Global Gravity Model Plus gravity field map obtained from Geodesy Group (2016).

FIGURE 2
FIGURE 2

Illustration of the map lookup process by a one dimensional example. The location of the sensed signal s with standard deviation of noise σ on the 1D map is found the search window centered at the prior of signal location xs with uncertain offset δx. In this example, the collected signal location candidates on the map are x1, x2, and x3.

FIGURE 3
FIGURE 3

Illustration of signal location search window with the collection of signal location candidates (red dots) obtained via the map lookup function Equation (1) with the field measurement s = 9.7974 m / s2 and the standard deviation of measurement noise σ = 0.9776 × 10−5 m / s2. The green dot is the true signal location and black dot signifies the probabilistic data association (PDA) solution.

The map-matching problem is to find the posterior density p(x | Zm) of the location x of the signal s on the map Embedded Image based on the candidate locations, Zm, and the prior.

In this work, we propose the PMHT-MM tracker to estimate this posterior density for INS aiding. The algorithm works with a sequence of sensor measurements that are correspond to a batch of platform locations over time. Each time, the algorithm runs iteratively, using an expectation-maximization technique (Dempster et al., 1977) to approximate the optimal estimator, taking into account data correlation locally and over time through the platform kinematic constraints.

3 PROBABILISTIC MULTIPLE-HYPOTHESES MAP MATCHING

In this section, we present our method to solve the map-matching problem. This combines a probabilistic data association (PDA) technique from Bar-Shalom & Fortmann (1988), used to resolve the map measurement ambiguity issue, with a probabilistic multiple-hypotheses tracker (Streit & Luginbuhl, 1994), to provide a robust map-matching solution in the context of INS aiding.

3.1 Map Access via Probability Data Association

PDA is a key method to determine where a gravimeter signal is originated in the position coordinates on the data map in the presence of non-unique, noisy data-location mapping. As part of integration filtering process shown in Figure 1, the position components of the navigation state XINS, denoted by xINS, serve as the filter predicted position vector and will be updated using a UKF if a position fix from an external source (e.g., GPS location, map-matching position, etc.) is present. Note that, if no external fix is present, the system shown in Figure 1 is simply a standard INS, with state estimator xINS and a covariance Σ describing a statistical uncertainty ellipse providing a restriction on where the signal s is measured on the map. Therefore, a finite set of potential locations for signal s on the map can be obtained via Equation (2). The above process is illustrated in Figure 4.

FIGURE 4
FIGURE 4

Illustration of the collection of candidate signal locations Embedded Image obtained via Equation (1) and Equation (2) based on knowledge of predicted vehicle position xINS from INS, and sensor noise level.

As we mentioned above, the location of a sensed signal s (e.g., obtained from the onboard INS) is assumed to follow a Gaussian distribution Embedded Image. The PDA solution of the map location Embedded Image of signal s, denoted by Embedded Image in Figure 4, is a probabilistic combination of the set of n candidate locations Embedded Image selected using the map lookup function Equation (1) and according to the criterion Equation (2). The probability weight of each candidate location zi is proportional to the geometric distance between zi and the window center xs. The probability weight can be calculated as:

Embedded Image 3

where Embedded Image, and Ri(σ) is the associated variance which is a function of the signal noise variance, or in other words, signal-to-noise ratio (SNR). Thus, the PDA solution, combining multiple locations to a single location, for the map location of sensed signal s over the area described by Equation (2) is the following weighted mean:

Embedded Image 4

and the associated weighted variance:

Embedded Image 5

The PDA solution described in this subsection is a popular technique for target tracking in clutter, which involves a gating process (X. Wang et al., 2002) and probabilistic data association (Bar-Shalom & Fortmann, 1988).

3.2 PMHT-MM Algorithm

The proposed PMHT-MM algorithm is derived directly from the PMHT algorithm, originally proposed by Streit & Luginbuhl (1994) for the application of multi-target tracking in clutter. We adopt this technique here for the map-matching to aid an INS, where only a single target—the platform—is involved. It provides an iterative multiple-hypotheses processing framework that handles measurement ambiguity locally, and system uncertainties over time under platform kinematic constraints. As pointed out by Davey (2007), it has good data association performance with a cost that is linear in time and the number of targets.

Let xt denote the kinematic state of the platform, which involves position and velocity. Its evolution over time is locally described by the state space model:

Embedded Image 6

and measurement model:

Embedded Image 7

where F, H, Q and R are known matrices.

The PMHT-MM algorithm works in a batch mode involving T > 1 data sampling periods, also known as scans; a scan is the duration between two consecutive measurement sampling points. Let:

Embedded Image

denote the kinematic states over a batch of T scans and

Embedded Image

be the set of measurements during the batch of scans, where Embedded Image signifies the set of n(t) measurements collected at scan t.

The PMHT-MM seeks to maximize the posterior probability density function p(X| Z) by performing the following expectation-maximization (EM) iteration

Embedded Image 8

where

Embedded Image 9

and Θ is the latent variable describing the hypothesis for data association. In the context of this work Z is the batch of T subsets of platform location candidates corresponding to the field measurements observed in the T scans, and Θ represents the set of gravitational field measurement association events.

At time k = t + T, the prior kinematic state of platform Embedded Image, as expressed in Equation (11) for i = 0, is obtained from the navigation state of the INS, and the set of signal candidate locations Zk which are obtained via the map lookup function in Equation (1) based on the set of gravimeter sensed signals Embedded Image. The PMHT-MM then runs the following two steps iteratively. At the i–th iteration:

  • Step 1: Calculate the PDA solution of map locations Embedded Image and their associated variances Embedded Image for the set of sensed signals Embedded Image based on knowledge of Embedded Image via Equations (3), (4), and (5). Note that the likelihood of the j–th candidate location p(zj | xs) in Equation (3) is replaced by

    Embedded Image 10

    where Embedded Image is the i–th iteration PMHT-MM predicted kinematic state of the platform at time k–t. As only one platform is involved, hypotheses are made only in the measurement origin; particularly, we assume that a measurement may originate from the platform or is a false alarm.

  • Step 2: State update and smoothing. The prior state estimates and the associated covariance matrices

    Embedded Image 11

    are updated via a fixed lag Kalman smoother in a forward (update) and backward (smoothing) recursion using the set of measurements obtained from Step 1.

    • Forward process:

      Embedded Image

      For t=1, ⋯, T – 1, we have (standard Kalman filtering)

      Embedded Image 12

    • Backward process:

      Embedded Image

      For t=T–1, ⋯, 2,1, (smoothing)

      Embedded Image 13

The iteration may be stopped if the criterion ||X(i+1) – X(i) || ≤ ε is met, or after a fixed number of iterations. In this work, we chose the number of iterations as 15 in all simulations as, in our context, almost no error difference between two consecutive iterations is observed after 15 iterations.

3.3 Map-Matching Aiding

The PMHT-MM algorithm is designed to work locally in coordinates consistent with the INS and map geo-reference, so that it deals with the (noisy) linear kinematics using standard Kalman filters. In this work, the kinematic components of the INS navigation state are taken as priors and an estimate is made of the current platform kinematic state based on a batch of sensed signals taken from a gravimetric sensor independent of INS. As illustrated in Figure 1, the posterior estimate Embedded Image of the platform is integrated into the INS via a loosely coupled unscented Kalman filter (UKF). Interested readers may refer to (Titterton & Weston,) and (Crassidis, 2006) for more information on the strapdown INS with UKF integration. We highlight several points below specifically regarding PMHT-MM aiding integration.

  • Platform kinematic behavior may be quite complex, but locally, within the batch length T, can be approximated by a linear system. A trade-off between aiding robustness and allowable platform maneuver capability is achieved by choosing a suitable batch length.

  • Two alternative approaches can be used to implement the PMHT-MM algorithm:

    • Standard: An update occurs after every batch time duration T × Δt; for example, T = 30, Δt = 10, then the aiding interval will be 300 s, where Δt is the gravimetric sensor sampling interval.

    • Retrodiction: An update occurs after every T × Δt, and the past navigation states involved in the current batch are also updated by retrodiction. This is equivalent to having an aiding interval Δt = 10 s.

    Our simulation suggests that the estimated platform trajectory is more smoother by using retrodiction, though more computational resources are required.

  • In view of the fact that the data variability (variation of features) of a map varies from place to place, it is desirable to define a measure to describe that variability, and to find a way to take this into account in the filter for map-matching aiding. In this work, such a measure, called map feature variability is defined. It is denoted by Embedded Image where i indicates the pixel around which the variability is quantified. The map feature variability at the ith pixel (location Embedded Image) within a fixed search window template centered at i is

    Embedded Image 14

    where n is the number of points in the search window. The map feature variability for a given map location provides a local measure on map data variation extent. In practice, this quantity is normalized over a fixed number of samples. If it is too small, it might be more effective to stop the aiding as, in these circumstances, the map-matching contributes little and might actually impair the performance of the INS integration filter. To achieve such a goal, the covariance of the estimated location by PMHT-MM algorithm (i.e., the covariance of external fix) is weighted by Embedded Image.

    Figure 5 shows an example of the map feature variability sequence computed along the platform travel path in the simulation scenario presented next. The magnitude of the map feature variability reflects the level of data variation on the map as indicated by the vertical gravity disturbance measurement sequence curve shown in the top figure along the platform trajectory taken by a noiseless sensor. Note that in practice, the vertical gravity disturbance measurement is the vertical part of the deflections from the normal gravity. In this work, we assume that the reading error of deflection angle is determined by the onboard gyroscope accuracy and is contained in the field sensor model.

FIGURE 5
FIGURE 5

Noiseless vertical gravity disturbance measurements and the map feature variability along the vehicle travel trajectory.

4 EXPERIMENT AND RESULTS

In this section, the proposed PMHT-MM aiding method is tested in a scenario of inertial navigation with aiding only from map-matching using gravimetric sensor measurements with associated data maps. The maps used in the experiment are the ultra-high resolution, non-parametric gravity maps, known as GGMplus (Hirt et al., 2013). We use two maps from GGMplus: a vertical gravity field data map and a vertical gravity disturbance map, both obtained online (Geodesy Group, 2016).

4.1 Geophysical Data

To exemplify our algorithm, we use the ultra-high resolution, non-parametric gravity maps presented by Hirt et al. (2013 ,2014). These maps, known as Global Gravity Model Plus (GGMplus), achieve a spatial resolution of ~ 250 m and covers all land and near-coastal areas for the Earth between ±60° latitude. This ultra-high resolution map is obtained by fusing the following three elements:

  1. GOCE/GRACE satellite gravity (spatial scales of ~ 10,000 km down to ~ 100 km).

  2. Global geopotential model EGM2008 (spatial scales of ~ 100 km to ~ 10 km).

  3. Topographic gravity due to terrain (spatial scales of ~ 10 km to ~ 250 m).

Note that the topographic gravity is obtained assuming a mass density of 2,670 kgm−3. For our simulations, we use gravitational acceleration in the down and the gravity disturbance (the radial derivatives of the disturbing gravity potential). These maps can be accessed at (Geodesy Group, 2016).

4.2 Simulation Scenario

The simulation scenario is a constant velocity vehicle traveling along the surface of the Earth at a fixed height of 100 m from [−38°, 144.5°] to [−35°, 150°] (i.e., from the Melbourne area to Sydney area) and at a ground speed of 22 m/s. The entire journey takes more than 3.6 hours and the PMHT-MM tracker is the only form of aiding to the onboard INS. Figure 6 shows the vehicle travel trajectory and the geo-referenced data map used for the test. The onboard INS computes the navigation state consisting of the platform geographical coordinates, navigation frame velocity, attitude and associated accelerometer and gyroscope biases at a sampling rate of 1 Hz, corresponding to a standard INS implementation as described in (Groves, 2013; Titterton & Weston, 2004). We assume that a low noise gravimeter is onboard the vehicle to take gravity field measurements at an interval of every Δ t = 10 seconds. As mentioned earlier, a UKF is used to incorporate the PMHT-MM output to update navigation state of the INS at an interval of T × Δt. For comparison, we choose the batch lengths of PMHT-MM T = 15 and T = 30, respectively.

FIGURE 6
FIGURE 6

The ultra-high resolution data map of gravity field obtained from (Geodesy Group, 2016): The platform travel trajectory (blue line) is superimposed on the map.

The primary simulation objectives is to compare the performance of the INS equipped high-end inertial sensors with that of the INS aided by PMHT-MM to see what level of bias correction can be acquired after the PMHT-MM aiding. The performance is measured using the metric of root-mean-squared (RMS) position error on the vehicle trajectories. In addition, track divergence rate in multiple Monte-Carlo runs is counted as an indication of the robustness of the PMHT-MM. A track in a single run is deemed to be divergent if the RMS position error becomes increasingly large over time without apparent bound. PMHT-MM tracker divergence is caused by a large uncertainty in a search window center estimation with a short batch length T; this results in repeated exclusion of true signal locations in the search windows.

100 Monte-Carlo runs are carried out for the INS with each of the following two sets of inertial sensors:

  1. Precision-grade accelerometer and gyroscope (PCAG);

  2. Quantum-grade accelerometer and precision grade gyroscope (QAPCG);

The noise characterization of the inertial sensors is given in Table 1.

View this table:
TABLE 1

Bias and Noise Ranges of Inertial Sensors in the Simulation According to Jekeli (2005).

In addition, 100 Monte-Carlo runs are carried out for each of the PMHT-MM aided INS cases:

  1. Batch length T = 15 and the standard deviation of gravimetric sensor noise is σ = 10−5 m / s2 or σ = 2 × 10−4 m / s2;

  2. Batch length T = 30 and the standard deviation of gravimetric sensor noise is σ = 10−5 m / s2 or σ = 2 × 10−4 m / s2.

where precision grade inertial sensors (PCAG) are used. Both low and high noise levels are chosen for the gravimetric sensor in the simulation. The low noise levels are chosen to model the best available high-end field sensors. On the other hand, the high noise levels are chosen to get a similar divergence rate for map-matching-aided inertial navigation using PMHT-MM algorithm with each of the two maps. In such a sensor noise setting, the comparison of the PMHT-MM performances between the two different data maps, e.g., the RMS position errors, is on a similar ground.

In the map-matching simulations, we observed that a search window centered at the map location of the measured signal s predicted by the onboard INS; a set of up to 20 locations (of data values closest to s) is collected via the map lookup function in Equation (1) and Equation (2). These candidate locations are then combined to the measured sensor location via the PDA method. At each PMHT-MM operation period, map-matching is performed between a batch of T measured sensor locations and a batch of predicted sensor locations iteratively. Both measured and predicted sensor locations are recalculated in each of EM iterations. The average size of search windows is about 5 × 5 km2 for using the gravity field map shown in Figure 6 and this number is slightly larger with the gravity disturbance map shown in Figure 7.

FIGURE 7
FIGURE 7

Ultra-high resolution data map of gravity disturbance (unit: m/s2) downloaded from (Geodesy Group, 2016): The platform trajectory (red line) is superimposed on the map.

The above mentioned simulations are also repeated with the map matching using the gravity disturbance map, which has a larger grid cell (thus lower resolution) than that of the gravity field map as shown in Figure 7. In that case, the gravity disturbance measurement at each epoch is obtained by processing the measurement of an onboard gravimetric sensor. Two sensor noise levels are considered: the standard deviations of the sensor noise are 10−6 m / s2 and 4 × 10−5 m / s2.

4.3 Results and Discussions

4.3.1 Overview Simulation Results

Before going to detail, we present a summary of the simulation results in Figure 8. The figure compares the RMS position errors of computed vehicle trajectories by the INS equipped with each of the two inertial sensor suites, PCAG and QAPCG, respectively. Along them, we plot the RMS position errors of the INS navigated trajectories with PCAG aided by the PMHT-MM algorithm with a batch length of T = 30 using the two GGMplus maps, respectively. Each result is averaged over 100 runs. In the navigation experiment aided by the PMHT-MM, a PCAG inertial sensor suite is used, and the aiding from PMHT-MM is integrated by an UKF whose predicted state is purely based on INS.

FIGURE 8
FIGURE 8

Comparison of RMS position errors of the INS 1) with PCAG; 2) with QAPCG; 3) with PCAG and PMHT-MM aiding using the vertical gravity field map (black curve); 4) with PCAG and PMHT-MM aiding using the vertical gravity disturbance map (pink curve).

Observations are made from Figure 8 as follows.

  • In the INS without aiding scenario, the RMS position error caused by the accumulative bias and drift grow unboundedly over time. The use of quantum-grade accelerometers reduces the RMS position error by a little over 6% after 3.5 hours compared with the PCAG. Nevertheless, the position error drift remains unbounded.

  • With the aiding by the proposed PMHT-MM algorithm using field measurements and the data map, the position drift is bounded and a stable RMS position error performance can be maintained. However, the error bound level of the PMHT-MM aiding depends on the field sensor precision and map resolution. For example, in the first half hour the aided position errors with the GGplus dgz map are higher because the map feature variability in the travel area is lower and the sensor signal is relatively flat as shown in Figure 5, or in other words, because the resolution of both sensor measurements and the map along that part of travel trajectory are poorer.

  • In the simulation, we assume that a sampling interval of the field sensors is 10 s, thus one aiding interval of PMHT-MM, which involves 30 scans, is 10 × 30 = 300 s. The INS sampling interval is 1 s.

4.3.2 Result with the Gravity Field Map

The gravimetric sensor measurement sequences along the platform travel trajectory with the standard deviations of noise σ = 0,10−5 and 2 × 10−4 m/ s2 are shown in Figure 9. The mean position error and track divergence rate under three levels of measurement noise, and averaged over 100 runs, are shown in Table 2. The results shown in Table 2 suggest that the position aiding output estimated by the PMHT-MM from gravimeter measurements matched against the GGMplus map yields an average position error in excess of 500 m at the measurement noise level σ = 10−5 m / s2 (SNR = 120 dB). This position error grows rapidly as the measurement noise level increases. Correspondingly, the tracker divergence rate also increases. The RMS position error comparison for T = 30 shown in Figure 10 confirms this observation. A similar situation for T = 15 is shown in Figure 11, though the error differences between the two levels of sensor noise become even larger. We see from Table 2 that with low sensor noise (σ = 10−5 m / s2) map matching is robust with zero divergence rate for both T = 15 and T = 30 and RMS error performance is dependent on the quality of the data map.

FIGURE 9
FIGURE 9

Gravity field measurements along the vehicle trajectory with noise σ = 0,10−5 and 2 × 10−4 m / s2, respectively

FIGURE 10
FIGURE 10

Comparison of RMS position error of the INS with PMHT-MM aiding for T = 30 at measurement noise levels σ = 10−5 m / s−2 (SNR = 120 dB) and 2 × 10−4 m / s−2 (SNR = 93 dB), respectively

FIGURE 11
FIGURE 11

RMS position error of the INS with PMHT-MM aiding for T = 15 at measurement noise levels σ = 10−5 m / s2 (SNR = 120 dB) and 2 × 10−4 m / s2 (SNR = 93 dB), respectively

View this table:
TABLE 2

Mean Error and Diverge Rate of the INS with PMHT-MM Aid Using the GGMplus Gravity Field Map

4.3.3 Result with Gravity Disturbance Map

In the case of PMHT-MM aiding using the gravity disturbance map, the mean position error and track divergence rate under three levels of measurement noise, and averaged over 100 runs, are shown in Table 3.

View this table:
TABLE 3

Mean Error and Divergence Rate with the GGMplus Gravity Disturbance Map.

The RMS position error performances of the PMHT-MM aided INS along the platform trajectory are shown in Figure 12 for T = 30 and Figure 13 for T = 15, respectively. These results obtained with the GGMplus gravity disturbance map show no significant difference compared with those using the GGMplus vertical gravity field map shown in Figure 10 and Figure 11. In addition, map-dependent PMHT-MM-aiding accuracy is clearly observed from the RMS position error of vehicle first hour trip: This is overwhelmingly large because of a small map feature variability in that area evidenced by the map feature variability along the trajectory shown in the bottom plot of Figure 5.

FIGURE 12
FIGURE 12

RMS position errors of the INS with PMHT-MM aiding at every 300 seconds using the GGMplus vertical gravity disturbance map shown in Figure 7, where the batch length of PMHT-MM is T = 30.

FIGURE 13
FIGURE 13

RMS position errors of the INS with PMHT-MM aiding at every 300 seconds using the GGMplus vertical gravity disturbance map shown in Figure 7, where the batch length of PMHT-MM is T = 15.

4.3.4 Discussion

  • The experimental results show that a robust map-matching performance is obtained by the PMHT-MM algorithm because it takes into account both local and spatial data correlation to provide an estimate of signal coordinates using an EM approach. While it works in a batch mode, only a relatively small amount of data samples is required to obtain a reasonable signal location estimate. This is in contrast to those map-matching approaches, i.e., (H. Wang et al., 2017; Wu et al., 2017) that require enormous measurements to carry out an area-based cross correlation.

  • A standard ICCP map-matching algorithm, denoted by ICCP-MM, is implemented in this work to compare its performance with that of the proposed PMHT-MM in the simulation using the GGMplus data map. In the overall simulation, we observed that the ICCP-MM experiences frequent track divergence. In fact, in all of our simulations, the ICCP-MM algorithm causes every track divergence when batch length T = 15. It can work when T = 30 in low sensor noise cases but with a high track divergence rate. Table 4 shows the simulation result for the batch length T = 30 and gravimeter noise level σ = 10−5 m / s2 with the GGMplus gravity field map. The RMS position error comparison is shown in Figure 15. While similar error performances of the two algorithms are observed, the ICCP-MM suffers from an 80% track divergence rate. A divergence rate of 64% is observed for the case of using GGMplus gravity disturbance map where the sensor noise level is 10−6 m / s2. The poor performance of ICCP algorithm is partially due to the lack of control of trajectory shape in the iteration regularized by line fitting.

  • We plot a snapshot of the PMHT-MM iterative localization process at a sampling epoch in Figure 14 in a run with aiding using the gravity field map. The plot shows the process that the algorithm drag the belief state (pink circle from INS) to the final estimated locations (black stars), i.e., to the state iteratively updated by the measurement location collection (blue plus symbols) with linear kinematic constraints.

  • Gravimetric sensor noise levels have a direct impact on the accuracy and robustness of map-matching aiding. The simulation results suggest that, using a low noise field sensor, the PMHT-MM algorithm is able to use a batch state of small length to yield a robust aided inertial navigation performance without track divergence.

  • The proposed algorithm may be used for map matching with other types of sensor measurements, such as the gravity gradient tensor (Jekeli, 2006), magnetometer measurements (Kim et al., 2019), or terrain-based navigation (Nygren & Jansson, 2004), etc.

View this table:
TABLE 4

Batch Length T = 30, σ = 10−5 m / s2, GGMplus Gravity Field Map.

FIGURE 14
FIGURE 14

A snapshot of the PMHT-MM iteration process with gravity field map at the Scan 4951: The PMHT-MM starts the iteration from the INS-predicted sensed locations (pink circles) and ends the iteration after 15 iterations at the final estimated locations (black stars) based on the sequence of T=15 subsets of candidate measurement locations (blue plus symbols).

FIGURE 15
FIGURE 15

Comparison of the RMS position errors between ICCP-MM and PMHT-MM aiding for T= 30 at gravimeter noise level σ = 10−5 m / s2 in the simulation as in Figure 10: The ICCP-MM aiding has an 80% track divergence rate, while no track divergence in the PMHT-MM aiding.

5 CONCLUSION

In this paper, we present a probabilistic multiple-hypotheses tracking map-matching algorithm for gravimetric data map-matching to aid an inertial navigation system in the absence of other aiding sources. The approach eliminates map measurement ambiguity by taking into account kinematic constraints of the platform and permits incorporation of data maps with a range of accuracy levels. Simulation results using online maps show the robustness and effectiveness of the algorithm for removing position drift that arises in INS over a long duration. Although the application shows an integrated gravimetric sensor map-matching inertial navigation scenario, the algorithm is applicable to other map-matching-based applications with measurements under a low data sampling regime.

The proposed PMHT-MM solves the map-matching localization problem via an iterative batch processing procedure that handles map measurement ambiguity with kinematic constraints. As suggested by the simulation results, it is capable of working at a low measurement rate with low resolution geophysical maps and giving a larger margin for trade-off between aiding robustness and ability of the tracker to handle vehicle maneuvers.

HOW TO CITE THIS ARTICLE

Wang, X., Gilliam, C., Kealy, A., Close, J., & Moran, B. (2023) Probabilistic map matching for robust inertial navigation aiding. NAVIGATION, 70(2). https://doi.org/10.33012/navi.583

ACKNOWLEDGMENTS

The authors wish to acknowledge Professor Andrew Greentree of Quantum Physics from the School of Science, RMIT university for his very helpful comments and suggestions in this work.

Footnotes

  • ↵1 The navigation state of INS consists of 15 components including vehicle longitude, latitude, height, velocity vector in navigation frame, pitch, roll, yaw, and inertial sensor bias components (three for accelerometer, three for gyroscope).

This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.

REFERENCES

  1. ↵
    1. Affleck, C., &
    2. Jircitano, A.
    (1990). Passive gravity gradiometer navigation system. In IEEE symposium on position location and navigation. a decade of excellence in the navigation sciences. IEEE. https://doi.org/10.1109/plans.1990.66158
  2. ↵
    1. Bar-Shalom, Y., &
    2. Fortmann, T. E.
    (1988). Tracking and Data Association. Academic Press.
  3. ↵
    1. Crassidis, J.
    (2006, apr). Sigma-point Kalman filtering for integrated GPS and inertial navigation. IEEE Transactions on Aerospace and Electronic Systems, 42(2), 750–756. https://doi.org/10.1109/taes.2006.1642588
    CrossRef
  4. ↵
    1. Davey, S. J.
    (2007, apr). Simultaneous localization and map building using the probabilistic multi-hypothesis tracker. IEEE Transactions on Robotics, 23(2), 271–280. https://doi.org/10.1109/tro.2007.892235
  5. ↵
    1. DeGregoria, A.
    (2010). Gravity gradiometry and map matching: an aid to aircraft inertial navigation systems (Unpublished master’s thesis). Air Force Institute of Technology, Wright-Patterson Air Force Base, Ohio.
  6. ↵
    1. Dempster, A. P.,
    2. Laird, N. M., &
    3. Rubin, D. B.
    (1977, sep). Maximum likelihood from incomplete data via the em algorithm. Journal of the Royal Statistical Society: Series B (Methodological), 39(1), 1–22. https://doi.org/10.1111/j.2517-6161.1977.tb01600.x
    CrossRef
  7. ↵
    1. Gao, D.,
    2. Hu, B.,
    3. Chang, L.,
    4. Qin, F., &
    5. Lyu, X.
    (2021). An aided navigation method based on strapdown gravity gradiometer. Sensors, 21(3). https://doi.org/10.3390/s21030829
  8. ↵
    1. Geodesy Group, K. U
    . (2016, June). Global gravity field model plus. http://geodesy.curtin.edu.au/research/resolution/.
  9. ↵
    1. Groves, P.
    (2013). Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems. Artech House.
  10. ↵
    1. Han, Y.,
    2. Wang, B.,
    3. Deng, Z., &
    4. Fu, M.
    (2018, February). A combined matching algorithm for underwater gravityaided navigation. IEEE/ASME Transactions on Mechatronics, 23(1), 233–241. https://doi.org/10.1109/tmech.2017.2774296
  11. ↵
    1. Hirt, C.,
    2. Claessens, S.,
    3. Fecher, T.,
    4. Kuhn, M.,
    5. Pail, R., &
    6. Rexer, M.
    (2013). New ultrahigh-resolution picture of Earth’s gravity field. Geophysical Research Letters, 40(16), 4279–4283. https://doi.org/10.1002/grl.50838
  12. ↵
    1. Hirt, C.,
    2. Kuhn, M.,
    3. Claessens, S.,
    4. Pail, R.,
    5. Seitz, K., &
    6. Gruber, T.
    (2014). Study of the Earth’s short-scale gravity field using the ERTM2160 gravity model. Computers & Geosciences, 73, 71–80.
  13. ↵
    1. Jekeli, C.
    (2005). Navigation error analysis of atom interferometer inertial sensor. NAVIGATION 52(1), 1–14. https://doi.org/10.1002/j.2161-4296.2005.tb01726.x
  14. ↵
    1. Jekeli, C.
    (2006, may). Precision free-inertial navigation with gravity compensation by an onboard gradiometer. Journal of Gaidance, Control and Dynamics, 29(3), 704–713. https://doi.org/10.2514/1.15368
  15. ↵
    1. Kamgar-Parsi, B., &
    2. Kamgar-Parsi, B.
    (1999, jul). Vehicle localization on gravity maps. In G. R. Gerhart, R. W. Gunderson, & C. M. Shoemaker (Eds.), Unmanned ground vehicle technology (Vol. 3693). SPIE. https://doi.org/10.1117/12.354447
  16. ↵
    1. Kim, Y. H.,
    2. Choi, M. J.,
    3. Kim, E. J., &
    4. Song, J. W.
    (2019, nov). Magnetic-map-matching-aided pedestrian navigation using outlier mitigation based on multiple sensors and roughness weighting. Sensors, 19(21), 4782. https://doi.org/10.3390/s19214782
  17. ↵
    1. Lee, J.,
    2. Kwon, J. H., &
    3. Yu, M.
    (2015). Performance evaluation and requirements assessment for gravity gradient referenced navigation. Sensors, 15(7), 16833–16847.
  18. ↵
    1. Moryl, J.,
    2. Rice, H., &
    3. Shinners, S.
    (1996). The universal gravity module for enhanced submarine navigation. In IEEE 1998 Position Location and Navigation Symposium (PLANS) (Cat. No.98CH36153). IEEE. https://doi.org/10.1109/plans.1998.670124
  19. ↵
    1. Nygren, I., &
    2. Jansson, M.
    (2004, July). Terrain navigation for underwater vehicles using the correlator method. IEEE Journal of Oceanic Engineering, 29(3), 906–915. https://doi.org/10.1109/joe.2004.833222
  20. ↵
    1. Streit, R. L., &
    2. Luginbuhl, T. E.
    (1994, jul). Maximum likelihood method for probabilistic multihypothesis tracking. In O. E. Drummond (Ed.), SPIE proceedings. SPIE. https://doi.org/10.1117/12.179066
  21. ↵
    1. Teixeira, F. C.,
    2. Quintas, J.,
    3. Maurya, P., &
    4. Pascoal, A.
    (2017). Robust particle filter formulations with application to terrain-aided navigation. International Journal of Adaptive Control and Signal Processing, 31(4), 608–651.
  22. ↵
    1. Titterton, D. H., &
    2. Weston, J. L.
    (2004). Strapdown Inertial Navigation Technology. 2nd. The Institution of Electrical Engineers.
  23. ↵
    1. Tuohy, S.,
    2. Patrikalakis, N.,
    3. Leonard, J.,
    4. Bellingham, J., &
    5. Chryssostomidis, C.
    (1996, 03). Map based navigation for autonomous underwater vehicles. International Journal of Offshore and Polar Engineering, 6(01).
  24. ↵
    1. Wang, B.,
    2. Ma, Z.,
    3. Huang, L.,
    4. Deng, Z., &
    5. Fu, M.
    (2022). A filtered-marine map-based matching method for gravity-aided navigation of underwater vehicles. IEEE/ASME Transactions on Mechatronics, 1–11. https://doi.org/10.1109/tmech.2022.3159596
  25. ↵
    1. Wang, B.,
    2. Zhu, J.,
    3. Ma, Z.,
    4. Deng, Z., &
    5. Fu, M.
    (2021, jun). Improved particle filter-based matching method with gravity sample vector for underwater gravity-aided navigation. IEEE Transactions on Industrial Electronics, 68(6), 5206–5216. https://doi.org/10.1109/tie.2020.2988227
  26. ↵
    1. Wang, H.,
    2. Wu, L.,
    3. Chai, H.,
    4. Bao, L., &
    5. Wang, Y.
    (2017, dec). Location accuracy of INS/gravity-integrated navigation system on the basis of ocean experiment and simulation. Sensors, 17(12), 2961. https://doi.org/10.3390/s17122961
  27. ↵
    1. Wang, X.,
    2. Challa, S., &
    3. Evans, R. J.
    (2002, July). Gating Techniques for Maneuvering Target Tracking in Clutter. IEEE Transactions on Communications AES, 38(3), 1087–1097.
  28. ↵
    1. Wu, L.,
    2. Ma, J., &
    3. Tian, J.
    (2010). A self-adaptive unscented Kalman filtering for underwater gravity aided navigation. In IEEE/ION Position, Location and Navigation Symposium 142–145. https://doi.org/10.1109/PLANS.2010.5507294
  29. ↵
    1. Wu, L.,
    2. Wang, H.,
    3. Chai, H.,
    4. Hsu, H., &
    5. Wang, Y.
    (2015, mar). Research on the relative positions-constrained pattern matching method for underwater gravity-aided inertial navigation. Journal of Navigation, 68(5), 937–950. https://doi.org/10.1017/s0373463315000235
  30. ↵
    1. Wu, L.,
    2. Wang, H.,
    3. Chai, H.,
    4. Zhang, L.,
    5. Hsu, H., &
    6. Wang, Y.
    (2017). Performance evaluation and analysis for gravity matching aided navigation. Sensors, 17(4), 769.
Back to top

In this issue

Print
Download PDF
Article Alerts
Email Article
Citation Tools
Probabilistic Map Matching for Robust Inertial Navigation Aiding
Xuezhi Wang, Christopher Gilliam, Allison Kealy, John Close,, Bill Moran
NAVIGATION: Journal of the Institute of Navigation Jun 2023, 70 (2) navi.583; DOI: 10.33012/navi.583
Twitter logo Facebook logo Mendeley logo
Bookmark this article

Jump to section

Keywords