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, Reference Stroock2013). 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, Reference Viterbi1967) 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