Gravity-aided navigation using Viterbi map matching algorithm

Abstract

In Global Navigation Satellite Systems (GNSS)-denied environments, aiding a vehicle's inertial navigation system (INS) is crucial to reducing the accumulated navigation drift caused by sensor errors (e.g. bias and noise). One potential solution is to use measurements of gravity as an aiding source. The measurements are matched to a geo-referenced map of Earth's gravity to estimate the vehicle's position. In this paper, we propose a novel formulation of the map matching problem using a hidden Markov model (HMM). Specifically, we treat the spatial cells of the map as the hidden states of the HMM and present a Viterbi style algorithm to estimate the most likely sequence of states, i.e. most likely sequence of vehicle positions, that results in the sequence of observed gravity measurements. Using a realistic gravity map, we demonstrate the accuracy of our Viterbi map matching algorithm in a navigation scenario and illustrate its robustness compared with existing methods.

1 Introduction

Since the advent of Global Navigation Satellite Systems (GNSS) in the 1960s, GNSS-based positioning has become ubiquitous in the areas of navigation, guidance and control. However, in a growing number of situations, GNSS is either unavailable, e.g. in underwater environments (Wang et al., 2017), or deliberately jammed or spoofed (Hemann et al., 2016). In such GNSS-denied (or contested) situations, a platform's navigation performance is governed by on-board sensors – chief among them being the inertial navigation system (INS), which integrates inertial measurements from accelerometers and gyroscopes to obtain position. Unfortunately, even with high precision INSs, very small errors in the inertial sensors result in the build up of large navigation errors over time (Titterton and Weston, 2004; Groves, 2013). Thus, an on-board aiding source that provides a position fix is crucial to removing these accumulated navigation errors and retaining confidence in the navigation accuracy. A promising candidate, and the focus of this paper, is the use of gravitational information to obtain a position fix (Han et al., 2016; Liu et al., 2019a; Wang et al., 2022).

Gravity-aided navigation originates from the early 1990s (Affleck and Jircitano, 1990; Jircitano and Dosch, 1991). The central concept is that an on-board sensor measures elements of the gravitational vector (or gravity gradient tensor) whilst the platform is in motion and these measurements are then matched to a geo-referenced map of the Earth's gravitational field to determine a position. The advantage of such an approach is twofold. First, gravity is an inherent physical property of the Earth, and thus it is immune to environmental interference, jamming and spoofing. Second, the measurement of gravity is a passive operation that does not require external information, which is advantageous in sensitive situations (Muniraj and Farhood, 2017). The challenge in this approach however is how to match the measurements to a position in the map. The design of this matching procedure, known as the map matching algorithm, is key to the success of gravity-aided navigation.

The map matching algorithm needs to take into account: (1) the sensor noise when measuring gravity; (2) the uncertainty on the spatial location when a measurement is obtained; (3) possible ambiguity in the map resulting in a measurement being matched to multiple locations. In the literature, map matching algorithms tackle these problems in one of two ways. The first category of algorithms consider the problem as a single-point matching operation. Building on the framework introduced in the Sandia terrain-aided navigation system (SITAN) (Hostetler and Andreas, 1983; Bergman, 1997), these techniques use the geo-referenced map as a look-up function and compute a predicted gravitational measurement using the INS estimated position. The predicted measurement is then used in the prediction step of a statistical filter. Due to the nonlinear relationship between the estimation states and gravitational measurements, the extended Kalman filter (EKF) is often used to perform the estimation (Lee et al., 2015; Wei et al., 2017). This category of algorithms take into account the sensor noise and spatial uncertainty but do not consider the map ambiguity directly. The second category of map matching algorithms directly considers the structure of the map. They approach the problem as a sequential matching operation. The classical example is the iterative closest contour point (ICCP) algorithm proposed by Kamgar-Parsi and Kamgar-Parsi (1999) and used by Wang et al. (2016), Han et al. (2017) and Liu et al. (2019b). The ICCP is based on the observation that a single, scalar, gravity measurement creates an iso-contour of similar values in the gravity map space. Thus, given a sequence of gravity measurements, the matching problem is posed 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 regularisation term is introduced to control the shape of the trajectory. However, linking the kinematic constraints of the platform's motion to the regularisation term is not straightforward.

In this paper, we propose a novel sequential map matching algorithm based on formulating the matching operation as a hidden Markov model (HMM). Specifically, we consider the spatial cells of the map to be the set of all possible hidden states in the model and the platform's trajectory to be a particular sequence of these states. The observations from each state are the gravitational measurements, and the output and state transition probabilities relate to the sensor noise and platform movement models, respectively. Thus, the map matching problem is equivalent to determining this sequence of states given a sequence of gravitational measurements. The advantage to this formulation is twofold. First, our proposed approach works directly in the map space and incorporates the modelling of the sensor noise and platform motion with the underlying structure of the gravity map. Second, this type of HMM problem can be solved efficiently using a dynamic programming method known as the Viterbi algorithm (Viterbi, 1967; Rabiner, 1989; Ren and Karimi, 2009). Given the HMM formulation, the Viterbi algorithm estimates the most likely sequence of states that results in the sequence of observations and this sequence of states is optimal in the maximum a posteriori (MAP) sense. Accordingly, we present a Viterbi map matching algorithm that outputs the most likely trajectory of the platform given a sequence of gravitational measurements. Furthermore, to tackle limited map resolution, we implement a two-layer, coarse-to-fine, estimation scheme to achieve sub-cell accuracy. We validate our algorithm using the ultra-high resolution, non-parametric, gravity maps presented by Hirt et al. (2013) and demonstrate that it outperforms existing algorithms in terms of robustness and navigational accuracy. In particular, we demonstrate that our algorithm is able to tackle varying map spatial resolution as well as varying sensor noise levels.

2 Problem formulation

Suppose that the actual position of a vehicle at time $k$ is $\mathbf {s}_{k}=[x_k,y_k]^{{\rm T}}$, the INS position estimate is $\mathbf {s}_{k}^{{\rm INS}}=[x_k^{{\rm INS}},y_k^{{\rm INS}}]^{{\rm T}}$ and the position corrected via map matching algorithm is $\mathbf {s}_{k}^{\text {C-INS}}=[x_k^{\text {C-INS}},y_k^{\text {C-INS}}]^{{\rm T}}$. The gravity measurement at time $k$ and position $\mathbf {s}_{k}$ is modelled as

(1)\begin{equation} z_{k} = g(\mathbf{s}_{k})+\varepsilon_k \end{equation}

where $g(\mathbf {s}_{k})$ describes the true Earth's gravity at position $\mathbf {s}_{k}$, and $\varepsilon _k\sim \mathcal {N}(0,\sigma _z^2)$ is noise with $0$ mean and standard deviation $\sigma _z$. Without loss of generality, in the simulation, the true Earth gravity is obtained from the gravity map without noise for the purpose of demonstrating the algorithm. Additionally, we assume that the INS provides velocity measurements $\mathbf {v}_{k}$ in the navigation frame and it is modelled as

(2)\begin{equation} \mathbf{v}_{k}=\mathbf{v}^0_{k}+\boldsymbol{\phi}_{k} \end{equation}

where $\mathbf {v}_{k}^0$ is the ground truth, $\boldsymbol {\phi }_{k}$ is the noise term comprising a bias $\mathbf {b}_k$ and an independent zero mean Gaussian noise with co-variance $\sigma _v^2\mathbf {I}_2$, and $\mathbf {I}_2$ is the $2\times 2$ identity matrix.

We assume that the INS position estimates are corrected after every segment of $T$ gravity measurements. Without loss of generality, we assume that the INS position estimates and measurements are recorded from $k+1$. Then, after $T$ sampling times, i.e. at time $k+T$, we have the following available segments of measurements and INS position estimates:

\[ \mathbf{Z}_k=\{z_{k+1},\ldots,z_{k+T}\},\quad \mathbf{V}_k=\{\mathbf{v}_{k+1},\ldots,\mathbf{v}_{k+T}\},\quad \mathbf{S}^{{\rm INS}}_k=\{\mathbf{s}_{k+1}^{{\rm INS}},\ldots,\mathbf{s}_{k+T}^{{\rm INS}}\} \]

The corrected position sequence is $(\mathbf {Z}_k,\mathbf {S}^{{\rm INS}}_k,\mathbf {V}_k)$ and the proposed map matching algorithm is denoted by $\mathbf {S}_{k}^{\text {C-INS}}=\{\mathbf {s}_{k+1}^{\text {C-INS}},\ldots,\mathbf {s}_{k+T}^{\text {C-INS}}\}$.

In this work, since the map matching algorithm is implemented every $T$ time steps, we actually are interested in how the algorithm works within time slot $\{k+1,k+2,\ldots,k+T\}$. For simplicity, where there is no confusion, time stamp $k$ in this time slot is dropped from now on and we write $t=1,\ldots,T$.

3 Viterbi map matching algorithm

3.1 Constructing a grid on the gravity map

The gravity map can be viewed as a grid of pixels with each pixel representing an area equivalent to the resolution of the map. These pixels will henceforth be referred to as cells. The resolution of the gravity map, i.e. the dimensions of the cell, is $R_{x}\times R_{y}$. Since each cell on the gravity map can be associated with a location, we simply let the central position of each cell be its location. Denote the collection of all cells’ locations on the interested area of the map by $\boldsymbol {\mathcal {C}}$. We define $g_m(\cdot )$ as the ‘look-up function’ for finding the value from the gravity map using the given location, e.g. $g_m(\mathbf {s}_{t}^{{\rm INS}})$ is the gravity value associated to the cell that $\mathbf {s}_{t}^{{\rm INS}}$ lies in.

In fact, at a specific sampling time $t$, we are only interested in a limited area around the INS position estimate $\mathbf {s}_{t}^{{\rm INS}}$ since the true position should be within that region. Therefore, given an estimate $\mathbf {s}_{t}^{{\rm INS}}$, we can find a cell, or equivalently a pixel, on the geo-referenced map of gravity that $\mathbf {s}_{t}^{{\rm INS}}$ lies in. It should be noted that, since the cell is predefined and constructed according to the pixel of the map and the estimate $\mathbf {s}_{t}^{{\rm INS}}$ is arbitrary, $\mathbf {s}_{t}^{{\rm INS}}$ can be any point within the underlying cell and is unnecessary to be the centre position of this cell. Then, a grid of cells around the cell containing $\mathbf {s}_{t}^{{\rm INS}}$ can be constructed and denoted by $\mathbf {B}_t$. Let the size of the grid be $nR_{x}\times nR_{y}$, where $n$ is a relatively small odd integer, and the positions of the cells within this grid be denoted by $\mathbf {c}^{j}_t$ for $j=1,\ldots,n^2$. We also use $\mathbf {c}^{j}_t$ as the labels of the cells. The cells within the grid are arranged as shown in Figure 1, where the central cell of the grid, labelled as $\mathbf {c}^{\bar {n}}_t$ with $\bar {n}={(n^2+1)}/{2}$, is the one that $\mathbf {s}_{t}^{{\rm INS}}$ lies in. Its position is

(3)\begin{equation} \mathbf{c}^{\bar{n}}_t\triangleq[x^{\bar{n}}_t,y^{\bar{n}}_t]^{{\rm T}} = \arg\min_{[x,y]^{{\rm T}}\in\boldsymbol{\mathcal{C}}} \|[x,y]^{{\rm T}}-\mathbf{s}_{t}^{{\rm INS}}\|_2\end{equation}

Essentially, Equation (3) means that $\mathbf {c}^{\bar {n}}_t$ is the cell that the INS position estimate $\mathbf {s}_{t}^{{\rm INS}}$ lies in at time $t$. As a result, each cell is associated to a gravity value according to the gravity map. Based on $\mathbf {c}^{\bar {n}}_t$ and $n$, the positions of other cells in the grid $\mathbf {B}_k$ are

(4)\begin{equation} \mathbf{c}^{j}_t=\mathbf{c}^{\bar{n}}_t-[m_1R_x,m_2R_y]^{{\rm T}},\quad \text{where}\ m_1,m_2={-}n,\ldots,n\ \text{and}\ j=1,\ldots,n^2\end{equation}

Figure 1. Illustrative example of a $3\times 3$ grid of cells, at arbitrary time $t$, from a gravity map $g_m$. Each cell has a spatial location $\mathbf {c}_t^{j}=[x_t^j,x_t^j]^{{\rm T}}$ and an associated gravity value $g_m(\mathbf {c}_t^{j})$ at this location. The central cell of the grid, $\mathbf {c}^5_t$, is the one that the INS position estimate $\mathbf {s}_t^{{\rm INS}}$ lies in and can be determined by Equation (3), and the positions of other cells can then be calculated by Equation (4)

3.2 Viterbi-based map matching algorithm

At each time $t=1,2,\ldots,T$, we construct the grid $\mathbf {B}_{t}$ and its associated cells $\mathbf {c}_{t}^{j_t}$, where the subscript $t$ is again the time index and superscript $j_t$ is the cell label $j_t=1,\ldots,n^2$. In this paper, for the sake of simplification, the size of grid $\mathbf {B}_{t}$, i.e. $n\times n$ or equivalently $n$, is fixed for all $t$. However, it is worth noting that the value of $n$ may be calculated from the uncertainty area of the predicted measurement and the resolution of the map and, therefore, the value of $n$ can be different from scan to scan. Then, the problem becomes how to select a cell in each $\mathbf {B}_t$ such that this segment of cells is the optimal estimate of the actual trajectory. For a segment of cells, $\{\mathbf {c}^{j_1}_{1},\ldots,\mathbf {c}^{j_T}_{T}\}$, with corresponding referenced gravity values, $\{g_m(\mathbf {c}^{j_1}_{1}),\ldots,g_m(\mathbf {c}^{j_T}_{T})\}$, gravity measurements, $\mathbf {Z}_k$, INS information, $\mathbf {V}_k$ and $\mathbf {S}^{{\rm INS}}_k$, the joint posterior probability is

(5)\begin{equation} p(\{\mathbf{c}^{j_1}_{1},\ldots,\mathbf{c}^{j_T}_{T}\}\,|\,\mathbf{Z}_k,\mathbf{S}^{{\rm INS}}_k,\mathbf{V}_k)= p(\{g_m(\mathbf{c}^{j_1}_{1}),\ldots,g_m(\mathbf{c}^{j_T}_{T})\}\,|\,\mathbf{Z}_k,\mathbf{S}^{{\rm INS}}_k,\mathbf{V}_k) \end{equation}

The most probable trajectory from $t=1$ to $t=T$ is given by the MAP estimator

(6)\begin{equation} \{j_1^*,\ldots,j_T^*\}=\arg\max_{{\boldsymbol{\mathcal{I}}^{\times}}} p(\{\mathbf{c}^{j_1}_{1},\ldots,\mathbf{c}_T^{j_T}\}\,|\,\mathbf{Z}_k,\mathbf{S}^{{\rm INS}}_k,\mathbf{V}_k)\end{equation}

where $\boldsymbol {\mathcal {I}}=\{1,\ldots,n^2\}$, $\boldsymbol {\mathcal {I}}^{\times }=\times_{i=1}^T \boldsymbol {\mathcal {I}}$, notation $\times$ is the Cartesian product and $\{\mathbf {c}^{j_1^*}_{1},\ldots,\mathbf {c}^{j_T^*}_{T}\}$ is the optimal path (locations) of the vehicle estimated from the given measurements. It should be noted that $\mathbf {c}^{j_t}_{t}$ is actually the central position of each cell of the map; that is, the estimated path for time interval ${1,\ldots,T}$ passes the centre of each cell. Finally, the estimated position sequence is then used to reset the corresponding segment of this period estimated from INS.

An exhaustive searching process is required to find an optimal solution to Equation (6), where the computation load of the order of $n^T$ can explode quickly if $n$ and/or $T$ become large. In this work, we intend to find an efficient algorithm to solve this problem.

Note that for an arbitrary sequence, $\{j_1,\ldots,j_T\}$,

(7)\begin{equation} p(\mathbf{c}_{t}^{j_t}\,|\,\mathbf{c}_{t-1}^{j_{t-1}},\ldots,\mathbf{c}_{1}^{j_1}) =p(\mathbf{c}_{t}^{j_t}\,|\,\mathbf{c}_{t-1}^{j_{t-1}})\quad \forall t\in\{2,\ldots,T\}\ \text{and}\ j_t\in\boldsymbol{\mathcal{I}} \end{equation}

which implies that the selection of $\mathbf {c}_{t}^{j_t}$ is only dependent on that of $\mathbf {c}_{t-1}^{j_{t-1}}$. This is a typical Markov process (Stroock, 2013). The sequence $\{\mathbf {c}_{1}^{j_1},\ldots,\mathbf {c}_{T}^{j_T}\}$ with the measurements can be modelled by an HMM. The Viterbi algorithm (Viterbi, 1967) is available to compute Equation (6) and provides a most likely sequence of positions.

At each time $t$, the likelihood function of $z_t$ given $\mathbf {c}_{t}^{j_t}$ and $g_m(\mathbf {c}_{t}^{j_t})$ is denoted by

(8)\begin{equation} p_z(z_t\,|\,\mathbf{c}_{t}^{j_t})=p_z(z_t\,|\,g_m(\mathbf{c}_{t}^{j_t}))\end{equation}

where $p_z(\cdot )$ is the probability density function of the gravity measurement. For each successive pair $\{t-1,t\}$ with $t=2,\ldots,T$, the measurement likelihood function of $\mathbf {c}^{j_{t}}_{t}$, given $\mathbf {v}_{t-1}$, the velocity measurement at time ${t-1}$ and $\mathbf {c}_{t}^{j_{t}}$, is

(9)\begin{equation} p_v(\mathbf{c}^{j_{t}}_{t}\,|\,\mathbf{v}_{t-1},\mathbf{c}_{t-1}^{j_{t-1}}) \end{equation}

where $p_v(\cdot )$ is the probability density function of the velocity measurement. It should be noted that the transition model from $\mathbf {c}_{t-1}^{j_{t-1}}$ to $\mathbf {c}^{j_{t}}_{t}$ is assumed to be a constant velocity model.

Then, given $\mathbf {c}^{j_{t-1}}_{t-1}$ and measurement $z_t$, the posterior density of $\mathbf {c}^{j_{t}}_{t}$ is

(10)\begin{align} p(\mathbf{c}^{j_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j_{t-1}}_{t-1})& \propto p(z_t\,|\,\mathbf{c}_{t}^{j_t},\mathbf{v}_{t-1},\mathbf{c}^{j_{t-1}}_{t-1}) p(\mathbf{c}^{j_{t}}_{t}\,|\,\mathbf{v}_{t-1},\mathbf{c}^{j_{t-1}}_{t-1})\nonumber\\ & =p_z(z_t\,|\,g_m(\mathbf{c}^{j_{t}}_{t}))p_v(\mathbf{c}^{j_{t}}_{t}\,|\,\mathbf{v}_{t-1}, \mathbf{c}_{t-1}^{j_{t-1}}) \end{align}

We shall be considering this, in particular, in the case when the transition is a single time step, and $j_1=j_{t-1}$ and $j_2=j_t$, for a pair $\{j_1,j_2\}\in \boldsymbol {\mathcal {I}}\times \boldsymbol {\mathcal {I}}$, we use the notation

(11)\begin{equation} \{j_{1}\rightarrow j_{2},p(\mathbf{c}^{j_{2}}_{2}\,|\, z_2, \mathbf{v}_{1},\mathbf{c}^{j_{1}}_{1})\} \end{equation}

for the potential path from $\mathbf {c}^{j_{1}}_{1}$ to $\mathbf {c}^{j_{2}}_{2}$ and its associated conditional probability.

Similarly, for any pair $\{j_{t-1},j_{t}\}\in \boldsymbol {\mathcal {I}}\times \boldsymbol {\mathcal {I}}$, we can have such a path with its posterior probability as

(12)\begin{equation} \{j_{t-1}\rightarrow j_{t},p(\mathbf{c}^{j_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j_{t-1}}_{t-1})\} \end{equation}

Then, starting from $t=2$, for a fixed $j_{1}$, a local optimal path with greatest posterior probability is given by

(13)\begin{equation} \left\{j_{1}\rightarrow j^*_{2}\,|\,j^*_{2}=\arg\max_{j_{2}\in\boldsymbol{\mathcal{I}}}p(\mathbf{c}^{j_{2}}_{2}\,|\, z_2, \mathbf{v}_{1},\mathbf{c}^{j_{1}}_{1})\right\}\end{equation}

Similarly, at $t=3$, for pair $\{j^*_2,j_3\}$, where $j^*_2$ is obtained from Equation (13) and therefore fixed and $j_3\in \boldsymbol {\mathcal {I}}$, the following local optimal path is

(14)\begin{equation} \left\{j^*_{2}\rightarrow j^*_{3}\,|\,j^*_{3}=\arg\max_{j_{3}\in\boldsymbol{\mathcal{I}}}p(\mathbf{c}^{j_{3}}_{3}\,|\, z_3, \mathbf{v}_{2},\mathbf{c}^{j^*_{2}}_{2})\right\}\end{equation}

Iteratively, for any pair $\{j^*_{t-1},j_{t}\}$, $j_{t}\in \boldsymbol {\mathcal {I}}$, the following optimisation is done:

(15)\begin{equation} \left\{j^*_{t-1}\rightarrow j^*_{t}\,|\,j^*_{t}=\arg\max_{j_{t}\in\boldsymbol{\mathcal{I}}}p(\mathbf{c}^{j_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j^*_{t-1}}_{t-1})\right\}\end{equation}

As a result, for any fixed $j_1\in \boldsymbol {\mathcal {I}}$, there is a candidate path that the vehicle potentially travels:

(16)\begin{equation} \{j_{1}\rightarrow j^*_{2}\rightarrow \cdots\rightarrow j^*_{t}\rightarrow\cdots\rightarrow j^*_{T-1}\rightarrow j^*_{T}\}\triangleq\boldsymbol{\mathcal{J}}^*_{j_1}\end{equation}

where

(17)\begin{equation} j^*_{t} =\begin{cases} \displaystyle \arg\max_{j_{t}\in\boldsymbol{\mathcal{I}}}p(\mathbf{c}^{j_{t}}_{t}\,|\, z_t, \mathbf{v}_{1},\mathbf{c}^{j_{1}}_{1}) & t=2\\ \displaystyle \arg\max_{j_{t}\in\boldsymbol{\mathcal{I}}}p(\mathbf{c}^{j_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j^*_{t-1}}_{t-1}) & t=3,\ldots,T \end{cases}\end{equation}

Since $\boldsymbol {\mathcal {J}}^*_{j_1}$ depends on the selection of $j_1$, we are able to collect the potential paths corresponding to all possible values of $j_1$, i.e. $\boldsymbol {\mathcal {J}}^{*-}=\{\boldsymbol {\mathcal {J}}^*_1,\ldots,\boldsymbol {\mathcal {J}}^*_{n^2}\}$. Then, a back tracing step is performed as follows:

(18)\begin{equation} j^*_{1}=\arg\max_{\boldsymbol{\mathcal{J}}^{*-}}p_z(z_1\,|\,g_m(\mathbf{c}_1^{j_1}))p(\mathbf{c}^{j^*_{2}}_{2}\,|\, z_2, \mathbf{v}_{1},\mathbf{c}^{j_{1}}_{1})\prod_{t=3}^Tp(\mathbf{c}^{j^*_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j^*_{t-1}}_{t-1})\end{equation}

This step can reveal the initial state so that the sequence of hidden states corresponding to the highest probability can be found. Additionally, the complete estimated path is

(19)\begin{equation} \{j^*_{1}\rightarrow j^*_{2}\rightarrow \cdots\rightarrow j^*_{t}\rightarrow\cdots\rightarrow j^*_{T-1}\rightarrow j^*_{T}\}\triangleq\boldsymbol{\mathcal{J}}^*\end{equation}

Eventually, the estimated path $\boldsymbol {\mathcal {J}}^*$ is used to reset the INS estimated position directly. An example of how the algorithm works is shown in Figure 2.

Figure 2. An example of the Viterbi map matching algorithm. The time sequence is $t=1,2,3,4$ and the dimension of each grid is $5\times 5$. The blue lines are the INS estimated positions while the red ones are the actual positions. The black dashed lines indicate two candidate trajectories satisfying Equation (16), i.e. $\boldsymbol {\mathcal {J}}^*_1=\{1\rightarrow 1\rightarrow 1\rightarrow 1\}$ and $\boldsymbol {\mathcal {J}}^*_{25}=\{25\rightarrow 25\rightarrow 25\rightarrow 25\}$. All trajectories shown in this figure are for demonstration only

In practice, given a measurement $z_t$ at a specific time $t$, we may find more than one cell within a grid $\mathbf {B}_t$ that has the same likelihood value using Equation (8). Therefore, multiple optimal trajectories may be identified because of that they can have the same maximum posterior value. If this happens, we choose the trajectory that is closest to the INS estimated position for simplicity.

3.3 An enhanced two-layer algorithm

In Section 3.2, a grid-based Viterbi algorithm is proposed to find a path used to correct $\mathbf {S}^{{\rm INS}}_k$. However, it should be noted that the proposed algorithm only returns a path that passes through the centre of each cell. As a result, the deviation between this path and the actual trajectory is highly dependent on the resolution of the gravity map since the actual trajectory can be any point of each cell. In this section, an enhanced method is presented to improve the accuracy of the estimated path.

The key idea of this enhanced algorithm is to generate refined sub-cells for each cell and use these refined sub-cells to match the actual trajectory. This strategy takes effect when constructing the grid $\mathbf {B}_t$. An example of a refined cell is given in Figure 3.

Figure 3. Illustration of the refined grid. The cell $\mathbf {c}_t^5$ is an example to show how the refined sub-cells are constructed, where $n=3$ and $o=3$

Suppose that each cell is divided into $o\times o$ sub-cells, the centres of which, for $\mathbf {c}_t^{j_t}$, are denoted by $\mathbf {c}_t^{j_t,l_t}$, where $l_t \in \boldsymbol {\mathcal {O}}$ and $\boldsymbol {\mathcal {O}}=\{1,2,\ldots,o^2\}$. We also use $\mathbf {c}_t^{j_t,l_t}$ as label of $l_t$th sub-cell of $j_t$th cell at time $t$. Since the resolution of gravity map is fixed, therefore, the sub-cells $\mathbf {c}_t^{j_t,l_t}$ refine cells $\mathbf {c}_t^{j_t}$ to provide better resolution on positions and do not improve the actual resolution of the gravity map. In the other word, the cell $\mathbf {c}_t^{j_t}$ within $\mathbf {B}_t$ is divided into multiple sub-cells $\mathbf {c}_t^{j_t,l_t}$. Then we have the following relation, for any given $j_t$:

(20)\begin{equation} g_m(\mathbf{c}_t^{j_t})=g_m(\mathbf{c}_t^{j_t,l_t}),\quad \forall l_t\in \boldsymbol{\mathcal{O}} \end{equation}

and

(21)\begin{equation} p_z(z_t\,|\,g_m(\mathbf{c}_t^{j_t,l_t}))=p_z(z_t\,|\,g_m(\mathbf{c}_t^{j_t}))\quad \forall l_t\in \boldsymbol{\mathcal{O}} \end{equation}

In similar vein to Equations (15), (16) and (17), the potential path for a given $\{j_1,l_1\}$ is

(22)\begin{equation} \{\{j_1,l_{1}\}\rightarrow \{j^*_2,l^*_{2}\}\rightarrow \cdots\rightarrow \{j^*_{T-1},l^*_{T-1}\}\rightarrow \{j^*_{T},l^*_{T}\}\}\triangleq\boldsymbol{\mathcal{L}}_{j_1,l_1}^*\end{equation}

where $l^*_{t}$ in $\{j^*_t,l^*_{t}\}$ is the optimal sub-cell and $j^*_t$ is the associated cell. Here, $\{j^*_t,l^*_{t}\}$ is obtained using $\{j^*_{t-1},l^*_{t-1}\}$ and it can be calculated from Equation (17) through replacing $j_t$ by $\{j_t,l_t\}$.

Let $\boldsymbol {\mathcal {L}}^{*-}=\{\boldsymbol {\mathcal {L}}^*_{1,1},\boldsymbol {\mathcal {L}}^*_{1,2}\ldots,\boldsymbol {\mathcal {L}}^*_{n^2,o^2}\}$. Then, $\{j^*_1,l^*_{1}\}$ in the estimated path

(23)\begin{equation} \boldsymbol{\mathcal{L}}^*=\{\{j^*_1,l^*_{1}\}\rightarrow \{j^*_2,l^*_{2}\}\rightarrow \cdots\rightarrow \{j^*_{T-1},l^*_{T-1}\}\rightarrow \{j^*_{T},l^*_{T}\}\}\end{equation}

is obtained by the back tracing step

(24)\begin{equation} \{j^*_1,l^*_{1}\} =\arg\max_{\boldsymbol{\mathcal{L}}^{*-}}p_z(z_1\,|\,g_m(\mathbf{c}_1^{j_1,l_1)})p(\mathbf{c}^{j^*_{2},l^*_{2}}_{2}\,|\, z_2, \mathbf{v}_{1},\mathbf{c}^{j_{1},l_{1}}_{1})\prod_{t=3}^Tp(\mathbf{c}^{j^*_{t},l^*_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j^*_{t-1},l^*_{t-1}}_{t-1})\end{equation}

3.4 Reducing the computation

In the algorithms presented above, the computational overhead increases dramatically as more cells are considered in the algorithm. To mitigate this issue, we constrain the number of $j_t$ in each time $t$ via a threshold. Relative likelihood values to select the available $j_t$ are calculated as follows:

(25)\begin{equation} \boldsymbol{\mathcal{A}}_t =\left\{j_t\left| \frac{p_z(z_t\,|\,g_m(\mathbf{c}_t^{j_t}))}{\max_{j_t\in\boldsymbol{\mathcal{I}}} p_z(z_t\,|\,g_m(\mathbf{c}_t^{j_t}))}\geq \alpha,\ \forall j_t\in\boldsymbol{\mathcal{I}}\right.\right\} \end{equation}

where $\alpha \in [0,1]$. Equation (25) implies that only those cells with relatively high likelihood values are chosen. In other words, cells that are very unlikely to form part of the path are discarded.

An example of higher likelihood cells from $\mathbf {c}_t^{j_t}$, $j_t\in \boldsymbol {\mathcal {I}}$, is given in Figure 4, where we assume that $T=3$ and the size of the grid at each time stamp is $n=11$. The values of $p_z(z_t\,|\,g_m(\mathbf {c}_1^{j_t}))$, $t=1,2,3$, are represented in different colours. At time $1$, there are three higher likelihood cells. Similarly, at times 2 and 3, there are $3$ and $2$ grids available, respectively. As a result, $\boldsymbol {\mathcal {A}}_t$, $t=1,2,3$, is obtained accordingly, e.g. $\boldsymbol {\mathcal {A}}_1=\{8,18,29,40,61,83,94\}$. The computational complexity is reduced from $O((T-1)n^2)$ to $O(\sum _{t=1}^{T-1} \#\boldsymbol {\mathcal {A}}_t\times \#\boldsymbol {\mathcal {A}}_{t+1})$, where $\#\boldsymbol {\mathcal {A}}_t$ is the number of elements in $\boldsymbol {\mathcal {A}}_t$. In practice, to simplify the problem, $\alpha$ can be set to be a fixed value, such as $0\cdot 1$ or $0\cdot 2$.

Figure 4. Illustration of likelihood values of cells within the grids at successive time stamps $t=1,2,3$. The windows size is $n=11\times 11$. For arbitrary cell $j_t$ at time $t$, the likelihood values are calculated from $p_z(z_t\,|\,g_m(\mathbf {c}_t^{j_t}))$ and normalised by ${p_z(z_t\,|\,g_m(\mathbf {c}_t^{j_t}))}/{\max _{j'_t\in \boldsymbol {\mathcal {I}}}\{p_z(z_t\,|\,g_m(\mathbf {c}_t^{j'_t}))\}}$. There is no sub-cell for each cell to simplify the illustration. In each grid, there are only up to seven cells having relative high likelihood values while other cells’ values are close to $0$. If Equation (25) is applied and let $\alpha =0\cdot 1$, the maximum number of possible trajectories is $7^3$; otherwise, in the algorithm without applying Equation (25), the maximum number of possible trajectories is $121^3$

Similarly, we use indexes in ${\boldsymbol {\mathcal {A}}_1,\ldots,\boldsymbol {\mathcal {A}}_T}$ to find multiple candidate paths using the same method as Equation (22) by replacing the associated indexes sequence, i.e.

(26)\begin{equation} \{\{j_1,l_{1}\}\rightarrow \{j^*_2,l^*_{2}\}\rightarrow \cdots\rightarrow \{j^*_{T-1},l^*_{T-1}\}\rightarrow \{j^*_{T},l^*_{T}\}\}\triangleq\boldsymbol{\mathcal{R}}_{j_1,l_1}^*\end{equation}

where $j_1\in \boldsymbol {\mathcal {A}}_1$ and $j^*_t\in \boldsymbol {\mathcal {A}}_t$ for $t=2,\ldots,T$.

Let $\boldsymbol {\mathcal {R}}^{*-}=\{\boldsymbol {\mathcal {R}}_{j_1,l_1}^*\,|\,\forall j_1\in \boldsymbol {\mathcal {A}}_1\}$ contain these candidate paths. Then the back tracing step in Equation (24) can be rewritten as, where $j_1\in \boldsymbol {\mathcal {A}}_1$,

(27)\begin{equation} \{j^*_1,l^*_{1}\} =\arg\max_{\boldsymbol{\mathcal{R}}^{*-}}p_z(z_1\,|\,g_m(\mathbf{c}_1^{j_1,l_1}))p(\mathbf{c}^{j^*_{2},l^*_{2}}_{2}\,|\, z_2, \mathbf{v}_{1},\mathbf{c}^{j_{1},l_{1}}_{1})\prod_{t=3}^Tp(\mathbf{c}^{j^*_{t},l^*_{t}}_{t}\,|\, z_t, \mathbf{v}_{t-1},\mathbf{c}^{j^*_{t-1},l^*_{t-1}}_{t-1})\end{equation}

The estimated path is, then, as follows:

(28)\begin{equation} \boldsymbol{\mathcal{R}}^*=\{\{j^*_1,l^*_{1}\}\rightarrow \{j^*_2,l^*_{2}\}\rightarrow \cdots\rightarrow \{j^*_{T-1},l^*_{T-1}\}\rightarrow \{j^*_{T},l^*_{T}\}\},\quad \text{where}\ j_t\in\boldsymbol{\mathcal{A}}_t\end{equation}

In summary, the algorithm proposed in Section 3.2 with enhanced strategies presented in Sections 3.3 and 3.4 is given in Algorithm 1.

Algorithm 1: The Viterbi-based map matching algorithm

4 Simulation

In the first part of the simulation, we assume that a vehicle (aircraft) travels across Australia, approximately from Melbourne to Sydney, at a constant velocity $7\cdot 54^\circ$/h, roughly $837$ Km/h [$1^\circ \approx 111$ Km (Scales et al., 2017)]. The total flight distance is approximately $814$ Km. The gravity map of the region of interest has been obtained from the GGMplus software (Hirt et al., 2013) with a variety of resolutions, where the component taken from the map is gravitational acceleration. The map is represented in grids that have specified resolutions with the grid equally spaced in terms of geodetic (GRS80) latitude and longitude. The map with lower resolution is created by down-sampling the higher one. An illustrative example is shown in Figure 5a.

Figure 5. Illustration of the scenarios used in the simulation. The coloured map is the reference gravitational acceleration map of (a) Victoria and New South Wales and (b) Western Australia. The INS estimated trajectory without aiding deviates gradually from the true trajectory and its resolution is $\frac {1}{200}^\circ$

To simplify the simulation, the noise in the velocity measurement, i.e. Equation (2), from the IMU is modelled as $\boldsymbol {\phi }_{k}=\mathbf {b}_{k}+\mathcal {N}(0,\sigma _v^2\mathbf {I}_2)$, where $\mathbf {b}_{k}$ is the bias vector. The measured gravitational acceleration is assumed to be corrupted by additive Gaussian noise $\mathcal {N}(0,\sigma _z^2)$, where $\sigma _z$ is the standard deviation. As mentioned earlier, the true Earth gravity is obtained from a gravity map without noise for the purpose of demonstrating the algorithm.

A Monte Carlo (MC) simulation of 500 runs is carried out for each scenario and simulation performance at time $k$ is measured by the root-mean-square error, defined as

(29)\begin{equation} \text{Error}_{k}=\frac{1}{nMC}\sum_{m=1}^{nMT}\text{Dist}(\mathbf{s}_{k,m},\mathbf{s}_{k,m}^{\text{C-INS}}) \end{equation}

where $nMC$ is the number of MC, $\mathbf {s}_{k,m}$ and $\mathbf {s}_{k,m}^{\text {C-INS}}$ are the position and updated position, respectively, of the target at time $k$ and $m$th simulation, and $\text {Dist}(\cdot,\cdot )$ is the Haversine distance (Km) between two points given their longitude and latitude (Gade, 2010).

The algorithms are evaluated by different parameters. In the simulation, the bias of the velocity is set to be $1^\circ$/hr and $\sigma _v={9\times 10^{-6}}^\circ$/s, roughly $1$ m/s. In the simulation, we assume that the inertial sensors used in the vehicle INS exhibit white noise and bias, such that the INS computed position deviates from ground truth at the end of journey, as shown in Figure 5. The standard deviation of the gravitational acceleration measurement is $\sigma _z=1,2$ mGal. The INS reports positions and velocities every $12$ s.

First, the performances of the Viterbi-based map matching algorithm (VBMP) proposed in Section 3.2 and the computation reduced algorithm (RVBMP) proposed in Section 3.4 are compared in terms of the time efficiency. In the simulation, standard deviation is $\sigma _z=1$ mGal, the map resolution is $1/100^\circ$, the correction rate, i.e. $T$, is set to be $6$, window size is $n=13$, correspondingly the size of $\mathbf {B}_t$ is $13\times 13$, and the sub-cell algorithm proposed in Section 3.3 is not applied. It also can be noticed that the value of $\alpha$ can potentially have an impact on the performance of RVBMP; that is, a larger $\alpha$ means the more cells will be included in $\boldsymbol {\mathcal {A}}_t$ and the more time will be required to find a solution. In considering this, the performance of RVBMP using different values of $\alpha$ are compared with VBMP. When $\alpha =0$, set $\boldsymbol {\mathcal {A}}_t$ will contain all cells and the algorithm becomes VBMP. The simulation results are given in Table 1, where TCR stands for time consumption ratio between the time consumption using a specific $\alpha$ and a referenced time, i.e. ${\text {Time}_{\alpha }}/{\text {Time}_{\text {ref}}}$, and $\text {Time}_{\alpha =0\cdot 4}$ is chosen as the reference in Table 1, Mean and Std. dev. are sample mean and sample standard deviation of $\text {Error}_{k}$, i.e.

\[ \text{Mean}=\frac{1}{L}\sum_{k=1}^L \text{Error}_{k},\quad \text{Std. dev.}=\sqrt{\frac{1}{L-1}\sum_{k=1}^L (\text{Error}_{k}-\text{Mean})} \]

where $L$ is the total number of sampling points over the trajectory.

Table 1. Performance comparison between RVBMP and RVBMP using different values of $\alpha$

Note: the scenario is shown in Figure 5(a).

It can be seen that the RVBMP performs better than VBMP in terms of time efficiency and they have a similar tracking performance when $\alpha$ is less than $0\cdot 1$.

Second, the performance of RVBMP and the enhanced two-layer algorithm (RVBMP-2) in Section 3.3 are compared via standard deviation $\sigma _z=1$ mGal and two map resolutions. The correction rate, i.e. $T$, is set to be $6$, window size is $n=13$ and the size of a sub-cell is $o=7$ for RVBMP-2, correspondingly, the size of $\mathbf {B}_t$ is $13\times 13$ and that of sub-cell is $7\times 7$. The results are given in Figure 6 and Table 2. The definitions of Mean and Std. dev are the same as those in Table 1 and they are calculated over those estimated results that are not divergent. The time consumption ratio (TCR) is given to compare the time efficiency of these two algorithms. The TCR in this table is defined as ${\text {Time}_{\text {RVBMP-2}}}/{\text {Time}_{\text {RVBMP}}}$.

Figure 6. Simulation results. The scenario is shown in Figure 5(a). (a) The map resolution is $1/100^\circ$; (b) the map resolution is $1/200^\circ$

Table 2. Performance comparison between RVBMP and RVBMP-2 using two different map resolutions

Note: the scenario is shown in Figure 5(a).

From Figure 6 and Table 2, we can see that the RVBMP-2 outperforms RVBMP in both resolutions, which indicates that the sub-cell can improve the RVBMP. It should be noticed that the sub-cell strategy works better when the map resolution is relative low. This is because a high map resolution implies a high position resolution as well, so that the sub-cell strategy has less influence on the performance.

Finally, to further demonstrate the efficiency of RVBMP-2, the performance of RVBMP-2 and ICCP (Kamgar-Parsi and Kamgar-Parsi, 1999; Han et al., 2017) are compared using two different noise levels, i.e. $\sigma _z$. The map resolution is $1/200^\circ$, window size is $n=13$ and the size of a sub-cell is $o=5$, correspondingly the size of $\mathbf {B}_t$ is $13\times 13$ and that of a sub-cell is $5\times 5$. The simulation results are shown in Figures 7 and 8, and summarised in Table 3, where the success rate represents the percentage of times the algorithm followed the true trajectory without divergence, and the definitions of Mean and Std. dev. are as in Table 2.

Figure 7. Simulation results of ICCP and proposed algorithm when the correction rate is $4$. The scenario is shown in Figure 5(a). (a) $\sigma _z=1$ mGal; (b) $\sigma _z=2$ mGal

Figure 8. Simulation results of ICCP and proposed algorithm when the correction rate is $6$. The scenario is shown in Figure 5(a). (a) $\sigma _z=1$ mGal; (b) $\sigma _z=2$ mGal

Table 3. Performance comparison between RVBMP-2 and ICCP using two different standard deviations of the noise, $\sigma _z$ and correction rate

Note: the scenario is shown in Figure 5(a).

From Table 3 and Figures 7 and 8, we see that the RVBMP-2 algorithm outperforms ICCP both in success rate and tracking performance.

In the second part of the simulation, a reference gravity map that covers a major area of Western Australia (WA), Australia, see Figure 5(b), is used to further demonstrate the performance of the proposed algorithm. The major challenge of this scenario is that the gravitational acceleration is relatively flat; that is, the variation in gravity across distinct pixels/cells is, on the whole, smaller than that in the regions in the earlier simulations. This flatness of the gravity map suggests that aiding with it will perform less well than in the previous results.

In the simulation, the start point of the vehicle is near Perth, and the end point is located at ($[-24\cdot 427111,124\cdot 978610]$). The distance is approximately $1206$ Km. The remaining parameters are similar to those in Table 3.

In Table 4, we can see that, in terms of accuracy, the performances of RVBMP-2 and ICCP are similar because the map is relatively flat and provides less information to the algorithm. However, the former has a better success rate than the latter.

Table 4. Performance comparison between RVBMP-2 and ICCP using two different standard deviations of the noise, $\sigma _z$ and the correction rate

Note: the scenario is shown in Figure 5(b).

5 Summary

In this paper, we have presented a Viterbi-inspired map matching algorithm based on a hidden Markov model (HMM) formalisation of the map matching problem. Our algorithm determines the optimal trajectory of a platform given a sequence of gravitational measurements, a geo-reference gravity map and INS velocity information. This trajectory is optimal in the maximum a posteriori sense and is used to correct the platform's INS. The simulation results demonstrate that the algorithm is efficient in correcting the INS estimated trajectory. In addition, we proposed an enhanced 2-layer addition to the algorithm to improve the performance when the map resolution is low. We showed by simulations that our proposed algorithm outperforms existing map matching algorithms in terms of navigational accuracy and is also robust to varying levels of sensor noise. In terms of future work, we have validated our algorithm using gravitational measurements; however, it can be potentially generalised to other aiding resources, such as terrain elevation and magnetic field, that have similar maps. Additionally, we currently use a straightforward method when combining the output of the algorithm with the INS position – the INS position is simply reset with the corrected position. Accordingly, another area of future work is to develop a more sophisticated fusion process.