Keywords

1 Introduction

In GNSS-denied 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 and Weston 2004). Removing these accumulated navigation errors is therefore crucial for 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.

Geophysical map matching is an effective method for localisation and navigation where GNSS is not available; such as underwater, urban, or hostile environments (Tyren 1982; Tuohy et al. 1996; Kamgar-Parsi and Kamgar-Parsi 1999; Goldenberg 2006; Wang et al. 2022a,b; Li et al. 2022). Although conceptually simple, map matching with geophysical maps suffers from map measurement ambiguity issues. First, the geophysical measurements themselves are degraded by sensor noise so the measurements will not match the map exactly. Second, the measurements may match multiple points within the map as the map-lookup process is a scalar to vector mapping. Third, the location where the measurement was acquired is of course uncertain. Finally, the map itself suffers from finite spatial and signal resolution.

Here we consider magnetometry aided inertial navigation with total magnetic intensity (TMI) maps. The method include a probabilistic data association (PDA) approach to address the measurement ambiguity problem, and a probabilistic multiple hypothesis tracker (PMHT) for the map matching localisation using geophysical data maps. We show that the PDA method provides an effective way to map a field measurement into geolocation, but also enables a quantitative analysis of localisation error with respect to the magnetometer noise levels for a given TMI reference map. Furthermore, we implement a magnetometry aided INS using this method to determine the relationship between magnetometer noise levels and navigation performance.

Diamond magnetometry is a rapidly developing field, with potential applications for navigation Frontera et al. (2018). Sensitivity of diamond sensors is rapidly increasing, with additional techniques poised to transition from research to practical systems, meaning that it is timely to explore the potential of existing and future diamond sensors. Techniques designed to improve sensitivity include isotopic enrichment (Balasubramanian et al. 2009), portability through embedding in optical fibers (Ruan et al. 2018; Bai et al. 2020; Filipkowski et al. 2022), and laser threshold magnetometry (Jeske et al. 2016; Dumeige et al. 2019; Hahl et al. 2022).

2 INS Aiding via Map Matching

Aided INS can be described by a recursive Bayesian filtering system, where the system prediction is given by the onboard INS, and system update from measurements from external aiding sources. Figure 1 illustrates a generic aided INS with aiding from a map matching system. The INS is initialised from known parameters and at time k propagates the navigation state \({\mathbf {X}}_{\mathrm {INS},k\mid k-1}\) based on the earth surface motion model and inertial measurements \(({\mathbf {f}}_b\,\boldsymbol {\omega }_b)\). The global position measurements, estimated from map matching, are assumed to be Gaussian distributed with mean \(\hat {\boldsymbol {x}}^s\) – the estimated sensor location where s is taken and covariance \(\varSigma ^s\), and are incorporated into the system via an integration filter to update the navigation state \({\mathbf {X}}_{\mathrm {INS},k\mid k}\). For simplicity, we denote the navigation state at time k as \({\mathbf {X}}_k \in \mathbb {R}^n\): this comprises the components of vehicle kinematic state (position and velocity) expressed in the geographical coordinates, vehicle attitude (roll, pitch and yaw), and inertial sensor bias terms. Based on inertial sensor measurements, navigation state is

$$\displaystyle \begin{aligned}{} {\mathbf{X}}_{k} = {\mathbf{F}}_{\mathrm{INS}}({\mathbf{X}}_{k-1},\boldsymbol{\omega}_b,{\mathbf{f}}_b) + {\mathbf{w}}_k, \end{aligned} $$
(1)

where the function \({\mathbf {F}}_{\mathrm {INS}}(\cdot )\) signifies the mechanization of INS which involves the prior navigation state \({\mathbf {X}}_{k-1}\), the measurements of accelerometer \({\mathbf {f}}_b\) and gyroscope \(\boldsymbol {\omega }_b\) at k, respectively (Titterton and Weston 2004), where \(\mathbf {w} \sim {\mathcal {N}}(0,\boldsymbol {Q})\) accounts for system process noise including the errors from accelerometer and gyroscope \({\mathcal {N}}(\mathbf {a},\boldsymbol {B})\) signifies a Gaussian distribution with mean vector \(\mathbf {a}\) and covariance matrix \(\boldsymbol {B}\).

Fig. 1
figure 1

Generic single recursion map matching aided inertial navigation system

Full size image

At each aiding update time k, the aiding position measurement is coupled into the navigation state via

$$\displaystyle \begin{aligned}{} \boldsymbol{y}_k = \mathbf{H}{\mathbf{X}}_k + {\mathbf{v}}_k. \end{aligned} $$
(2)

where \(\mathbf {H}\) is a constant matrix and \(\mathbf {v} \sim {\mathcal {N}}(0,\boldsymbol {R})\) is a Gaussian zero-mean noise term modeling the measurement errors.

The INS aiding problem is to find the posterior density \(p({\mathbf {X}}_k\mid \boldsymbol {y}_{1:k})\) based on the sequence of measurements \(\boldsymbol {y}_{1:k}\) from aiding sources.

3 Probabilistic Multiple Hypothesis Map Matching

The probabilistic multiple hypothesis map matching involves probabilistic data association (PDA) for data mapping from TMI signal domain to vehicle position domain, and a batch based multiple hypothesis tracking algorithm to iteratively optimise the estimated vehicle trajectory.

For magnetometer measurement \(s_k\) at time k, the measurement model is

$$\displaystyle \begin{aligned}{} s_k = s_k^o + \nu_k, \end{aligned} $$
(3)

where \(s_k^o\) is the ground truth value and \(\nu _k\) a noise term covering imperfect sensor measurements, assumed to be Gaussian distributed i.e., \( \nu \sim {\mathcal {N}}(0, \sigma ^2) \).

Following (3), we consider a set of candidate measurements from a single measurement \(s_k\), one of which is the true sensor measurement. Let \(Z_m=\{\boldsymbol {z}_i, \, i = 1,\cdots , n\}\) denote the set of possible map locations corresponding to \(s_k\). We assume that at time k, the location of magnetometer, which takes the magnetic intensity measurement \(s_k\), is a Gaussian random variable with mean \(\boldsymbol {x}_k^s\) and covariance matrix \(\Sigma ^s_k\). Then, the location of true magnetic intensity measurement \(\boldsymbol {z}_i, \, i = 1,\cdots , n\) should satisfy (Chi-Square Test)

$$\displaystyle \begin{aligned}{} (\boldsymbol{z}_i-\boldsymbol{x}^s)(\boldsymbol{\Sigma}^s)^{-1}(\boldsymbol{z}_i-\boldsymbol{x}^s)' \leq \gamma, \end{aligned} $$
(4)

where \(\gamma \) is a probability threshold. This determines an ellipsoid on the data map containing the magnetometer location with a certain level of confidence. We refer to this area as a search window. Figure 2 illustrates the data PDA mapping process. A finite set of potential locations for signal s on the map can be obtained via (4).

Fig. 2
figure 2

Collection of candidate signal locations \(\{ \boldsymbol {z}^{s}_i,\,i = 1,2,\cdots ,n\}\) obtained via (4) based on knowledge of predicted vehicle position \( \boldsymbol {x}_{INS}\) from INS, and sensor noise level

Full size image

The probability weight of each candidate location \(\boldsymbol {z}_i\) is proportional to the geometric distance between \(\boldsymbol {z}_i\) and the window centre \(\boldsymbol {x}^s\) (i.e., \(\boldsymbol {x}_{INS}\)). The probability weight can be found as

$$\displaystyle \begin{aligned}{} w_i = p(\boldsymbol{z}_i\mid\boldsymbol{x}^s)\left[\sum_{j=1}^n p(\boldsymbol{z}_j\mid\boldsymbol{x}^s)\right]^{-1}, \end{aligned} $$
(5)

where \(p(\boldsymbol {z}_i\mid \boldsymbol {x}^s) \sim {\mathcal {N}}\bigl (\boldsymbol {z}_i-\boldsymbol {x}^s,\,\boldsymbol {R}_i(\sigma )\bigr )\), and \(\boldsymbol {R}_i(\sigma )\) is the associated variance which is a function of the signal noise variance, or in other words, signal-to-noise ratio (SNR). Thus, the mean \(\bar {\boldsymbol {z}}\) and variance \( \bar {\boldsymbol {R}}\) of PDA solution for the map location on magnetic intensity measurement \(s_k\) are given by

$$\displaystyle \begin{aligned}{} \bar{\boldsymbol{z}} = \sum_{i=1}^n w_i \boldsymbol{z}_i. \quad \quad \bar{\boldsymbol{R}} = \sum_{i = 1}^{n} w_i \left[\boldsymbol{R}_i(\sigma) + (\boldsymbol{z}_i-\bar{\boldsymbol{z}})(\boldsymbol{z}_i-\bar{\boldsymbol{z}})'\right]. \end{aligned} $$
(6)

Using PDA, the map matching quality can be characterised the PDA error distance \(\varepsilon _{\mathrm {PDA}} \), defined as the Euclidian distance between the true magnetometer location and the location estimated via PDA, i.e., \(\varepsilon _{\mathrm {PDA}} = \| \hat {\mathbf {x}}_{PDA}- \boldsymbol {x}^s\|.\)

Our simulations demonstrate that for a fixed resolution map, the measurement taken from a high sensitivity/low noise magnetometer will result in small PDA error distance. The TMI map used in the simulation is downloaded from Australia (2023). As shown in Fig. 3a, the actual data grid size is \(85 \times 85\) metres. The simulation is carried out in the area surrounded by the green solid line rectangle. For every sensor noise level, 1000 samples are drawn randomly in the area, which are treated as the mean of sensor locations. The values of sensor location covariance \(\Sigma ^s\) and probability threshold \(\gamma \) are chosen such that a search window approximately \(6.8 \,\mbox{km}^2\) is formed for collecting candidate measurement locations. PDA error distances are then calculated as a function of sensor noise level \(\sigma \) and map grid size.

Fig. 3
figure 3

(a) The total magnetic intensity map used in the simulation from Australia (2023) with superimposed platform travel trajectory (yellow line). (b) Comparison of PDA error distances vs. sensor noise levels in the TMI map at original, 5 and 10 times downsampled data grids

Full size image

Figure 3b shows the plot of PDA error distances versus sensor noise levels with original TMI map and the 5 and 10 times downsampled TMI maps. The plots show that localisation error decreases with improving magnetometer sensitivity until a localisation floor is reached. This floor is a function of the map resolution, with higher resolution providing a lower floor. The implication is that for a finite resolution map there is a magnetometer sensitivity below which no improvement is expected.

We use Map Feature Variability (MFV) as a measure of data variation sparsity of the geophysical data map. The MFV at a data point i on a data map is defined as \(\mathcal {C}_i = \frac {1}{n}\sum _{j}^n(s_{x_i} - s_{x_j})^2, \ \ \forall \, \boldsymbol {x}_j \in \mbox{search window},\, \boldsymbol {x}_j \neq \,\boldsymbol {x}_i.\) In a map matching based INS aiding, the value of \(C_i^{-1}\) may be used to weight the estimated sensor location covariance to provide additional parameter that locally describes quality of the data map used.

Figure 4c shows an example of the normalised map feature variability over the map area (Fig. 4a). For reference, we also show the original TMI map in Fig. 3a, and the PDA error distance maps for sensor noise levels \(\sigma = 0.015\) nT and \(0.15\) nT in Fig. 3b and d, respectively.

Fig. 4
figure 4

TMI map quality analysis in the red rectangle area shown in Fig. 3. (a) TMI map shown inside the red rectangle in Fig. 3a. (c) Map feature variability. (b) PDA error distance for \(\sigma = 0.015\) nT. (d) PDA error distance for \(\sigma = 0.15\) nT

Full size image

The Map matching localisation problem, shown in Fig. 1, is solved using the probabilistic multiple hypothesis tracker based map matching (PMHT-MM) proposed in (Wang et al. 2023). It iteratively estimates the current vehicle location from a batch of measurements processed by the probabilistic multiple hypothesis map matching method introduced in Sect. 3 under the vehicle dynamic constraints.

4 Navigation Experiment

The simulation scenario is a constant velocity vehicle traveling along the surface of the earth at a fixed height of 100 m from \([-38^\circ , 144.5^\circ ]\) to \([-35^\circ , 150^\circ ]\) (i.e., from Melbourne to Sydney) and at a ground speed of 22 m/s. The entire journey takes more than 3.6 hours and navigation is conducted by an onboard INS in GNSS denied environment. The inertial sensors (both accelerometer and gyroscope) used in the INS are precision grade with errors specified according to Jekeli (2005), with measurement frequency of 1 Hz and are assumed to be well calibrated before the journey starts. We assume that a low noise magnetometer is onboard to take magnetic intensity measurement at an interval of every 10 seconds. The PMHT-MM algorithm works with a batch of 30 magnetic intensity measurements at a time in an aiding interval 300 seconds.

In this experiment, two noise levels for the magnetometer are considered: (1) \(\sigma = 0.0015\) nT, which is to model a magnetometer of very high precision; (2) \(\sigma = 0.15\) nT, which represents the level of sensitivity of magnetometers that are commercially available. We plot the vehicle root-mean-squared (RMS) position errors in Fig. 5 along with case of INS-only without aiding. The results were averaged from 100 Monte Carlo runs for each of cases.

Fig. 5
figure 5

Comparison of RMS position errors of the aided INS for magnetometer measurement noise levels 0.015 nT (red) and 0.15 nT (blue), along with INS-only case (black)

Full size image

The simulation results in Fig. 5 show that:

  • when the noise level of magnetometer is 0.0015 nT, the INS with magnetometry aiding can achieve an average of RMS position error of 250 m; the RMS position error doubles if the noise level is 10 times larger at 0.15 nT.

  • magnetometry INS aiding is robust with 100% success rate with a batch length (i.e., the number of magnetometer measurements to be processed in a batch) 30 at each time in this simulation. If a lower batch length or a high noise level magnetometer is used, the RMS position error increases and the magnetometry INS aiding will not be completely reliable.

  • magnetometry INS aiding is able to remove position drift, as indicated by the INS-only case, which is accumulated over time due to the imperfection of inertial sensors.

5 Conclusions

In this paper, we describe a probabilistic method for map matching localisation based on magnetometry and total magnetic intensity maps. We demonstrated the effectiveness of the magnetometry map matching localisation via simulation. The magnetometry map matching removes accumulated position drift in the INS, that arises in the absence GNSS positioning. Simulation results verified the robustness and effectiveness of the proposed algorithm, particularly, the aiding precision improves with increasing magnetometer sensitivity, until the quality of the magnetic map limits precision.