Grid-based 3DMA GNSS with clustering and Doppler velocity using factor graph optimisation

Abstract

Three-dimensional mapping-aided (3DMA) Global Navigation Satellite System (GNSS) positioning improves the positioning in urban canyons for non-precision GNSS receivers. However, the 3DMA GNSS algorithms often produce a multimodal position solution, and simply taking the average of these modes reduces accuracy. A further problem, named ‘solution shifting’, is the effect of large numbers of low-scoring candidates shifting the overall position solution away from high-scoring regions. This study uses a clustering method to separate the different modes and exclude low-scoring regions from the position solution. Factor graph optimisation (FGO) is then used to integrate clustered 3DMA GNSS position and GNSS Doppler measurements or estimated velocity over multiple epochs. Positioning performance is assessed using data collected in London. The results show that the clustering method can successfully mitigate the multimodal effect, and integrating the FGO can mitigate the occurrence of multimodality and solution shifting. Static experiments in London achieve an RMSE of approximately 10 m for FGO 3DMA GNSS with clustering and 11 m without clustering.

1. Introduction

Global navigation satellite system (GNSS) positioning in urban canyons is challenging due to non-line-of-sight (NLOS) reception and multipath effects. High-rise buildings obstruct GNSS signals travelling in a line-of-sight (LOS) path. Instead, these signals reach the receiver by bouncing off reflective surfaces; this is known as NLOS reception (Groves, 2013a). NLOS signals travel a longer distance than the direct LOS path, resulting in a positive ranging error. Where both direct LOS and reflected signals are received from the same satellite, multipath interference occurs. The ranging error can be either positive or negative depending on whether the multipath interference is constructive or destructive. Erroneous pseudorange measurements can result in positioning errors that exceed 50 m (Hsu, 2018).

Techniques that use three-dimensional (3D) building models to assist GNSS positioning are known as 3D mapping-aided (3DMA) GNSS (Groves, 2016). 3DMA GNSS enables NLOS signals to contribute usefully to the position solution, in contrast to techniques such as consistency checking (Groves and Jiang, 2013) that attempt to exclude them.

Existing 3DMA GNSS positioning frameworks usually estimate the receiver’s location with a candidate-based approach. The candidate-based approach distributes the candidates and then samples the consistency between predictions and measurements for each candidate. The candidate with higher consistency should be the receiver’s location. Signal strength and pseudorange measurements are usually used as the features for consistency comparison of each candidate. 3DMA GNSS techniques may be divided into two main categories, known as shadow matching and ranging-based 3DMA GNSS. Shadow matching (Groves, 2011; Wang et al., 2015) uses satellite visibility as the matching feature. LOS and NLOS predictions at each sampled candidate are compared with those computed from the received signal strength, and the receiver’s location is determined based on the highest similarity.

In contrast, ranging-based 3DMA GNSS uses pseudorange measurements as the comparison feature. Ranging-based 3DMA GNSS methods detect and correct the NLOS ranging errors and fundamentally improve the quality of the ranging measurements during the position estimation stage. Unlike reweighting approaches that only mitigate errors of unhealthy measurements, corrections are applied to the predicted pseudoranges at candidate positions where the signal is predicted to be NLOS. These corrections may be categorised as either geometrical or statistically based. Geometrical ranging-based 3DMA GNSS, comprising ray-tracing GNSS (Hsu et al., 2016; Miura et al., 2015) and skymask 3DMA GNSS (Ng et al., 2020), determines the reflecting point and calculates the reflection delays in the ranging domain with geometrical functions. Ray-tracing GNSS (Hsu et al., 2016; Miura et al., 2015) simulates the propagation path over all surrounding reflectors to find the best match simulated path and calculates reflection delays, resulting in a huge computational load. Algorithms are proposed to accelerate the reflecting point estimation, such as the accelerated ray-tracing algorithm selects effective front surfaces (Ziedan, 2017), applies graphical processing unit (GPU) technologies to speed-up processing capacity (Gegout et al., 2014) and pre-generate the result as offline processes (Liu et al., 2018). However, existing ray-tracing GNSS simulations perform best in single-reflection cases but struggle with multiple bounce paths and reflected paths. Another ranging-based 3DMA GNSS, known as skymask 3DMA GNSS (Ng et al., 2020), also identifies the reflection location by using an enhanced skymask. The enhanced skymask expands from only including the elevation angle of building boundaries to also incorporating their corresponding height.

Likelihood-based ranging 3DMA GNSS estimates the NLOS ranging delays statistically (Groves et al., 2020). The distribution of NLOS pseudorange errors is assumed to be positively skewed. This method estimates the cumulative probability of NLOS pseudorange innovations using a skew-normal distribution. The NLOS pseudorange innovations are then converted to LOS pseudorange innovations by remapping their estimated cumulative probabilities to the equivalent LOS normal distribution. Without a geometrical calculation of the reflection geometry, likelihood-based ranging 3DMA GNSS provides a lightweight ranging-based 3DMA GNSS without significantly compromising the positioning accuracy. Given their computational efficiency, this study integrates shadow matching and likelihood-based ranging 3DMA GNSS.

However, several factors limit the performance of candidate-based 3DMA GNSS over a single epoch. A multimodal problem, also known as a local minima or ambiguity problem, frequently occurs when only limited matching features exist. This results in multiple clusters of position hypothesis candidates with a high similarity or likelihood score. If a weighted average is used to compute the position solution, that position is likely to fall within a low-scoring region between clusters. A region-growing method has been developed to separate the candidate positions into a set of high-likelihood clusters (Zhong and Groves, 2023). The challenge is then to identify the correct cluster.

In contrast, the ‘solution shifting’ problem refers to the effect of large numbers of low-scoring candidates shifting a position solution based on weighted averaging away from high-scoring regions. The problem can be caused by satellite visibility prediction errors (Zhong and Groves, 2022a), unhealthy measurements, such as multipath, and insufficient LOS satellites. Note that this error can also be found when multimodality occurs. However, some of these errors can be mitigated using outlier detection or only selecting candidates with higher scoring in solution computation. Gathering measurements across multiple epochs should increase the size of the correct peak in the likelihood surface and reduce the scores of both incorrect peaks and their surrounding regions. Thus, both the multimodality and solution-shifting effects are mitigated. However, this relies on given receiver’s motion between epochs, and hence require the velocity measurements. Therefore, temporal information on velocity is required. Specifically, the velocity of the receiver can be estimated by GNSS Doppler (or pseudorange rate) measurements.

Multi-epoch navigation using filtering techniques is extensively discussed, such as the Kalman filter (KF) and extended Kalman filter (EKF) for the linearised problem, while particle filter (PF) was used to handle a nonlinear distribution by Groves (2013b). Compared with single-epoch solutions, filtered solutions are less affected by random errors. Hence, smooth and robust solutions can be provided. Various filtering techniques have been applied to 3DMA GNSS. A KF is used to integrate shadow matching 3DMA GNSS with an inertial navigation system (INS) in both a loosely and tightly coupled manner (Zhang et al., 2020). Meanwhile, a particle filter was also used to sample the location particles with unequal spacing, and the likelihood of the position hypothesis was evaluated by shadow matching (Wang, 2014; Yozevitch and Moshe, 2015) and LOS pseudoranges (Suzuki, 2016). A grid filter samples location candidates evenly with unequal weighting initialisation, and provides a positioning performance similar to that of the particle filter (Zhong and Groves, 2022b). A modified map-matching algorithm, known as map-matching with tracking feedback (MMTF), is proposed by Ziedan (2020) to find the sequence of candidate positions on the map based on predictions of signal reception status with 3D maps and the output from adaptive tracking with signal monitoring (ATSM). Then, reflection and diffraction are removed from estimated code delays before the estimation of the solution. Last but not least, factor graph optimisation (FGO) demonstrated a robust performance for urban positioning (Wen and Hsu, 2021). GNSS measurements are modelled as a factor graph (Sünderhauf and Protzel, 2012). It is able to identify and remove multipath observations with switch variables to each of the pseudorange measurements. FGO shows a promising improvement in robustness, while 3DMA GNSS increases the positioning accuracy. Examples include 3DMA GNSS-based collaborative positioning (Zhang et al., 2021), loosely coupled 3DMA GNSS (Ng et al., 2022) and Kriging-based 3DMA GNSS (Ng et al., 2023).

The study on Kriging-based 3DMA GNSS (Ng et al., 2023) models the likelihood scoring surface as a differentiable continuous function using the ordinary Kriging method, so the likelihood score can be interpolated at a given location that is unsampled. As a result, much information is preserved during state estimation in the optimisation process. The results show that multimodal issues can be mitigated by inputting higher-resolution information into the FGO. However, the estimated uncertainty is large for the multimodal cases if multiple peaks are not identified. In contrast, only a single high scoring peak is identified when a solution-shifting problem occurs, where the estimated uncertainty is relatively small, but the position estimate shifts away from the true location, resulting in the estimated position uncertainty not being bound to the actual error.

In this paper, the likelihood surface outputted by the 3DMA GNSS algorithms is segmented into clusters around each peak, and the covariance estimation is cluster dependent. Then, the most likely cluster at each epoch is selected for the integration over the temporal domain. As a result, this study uses factor graph optimisation (FGO) to connect information from multiple epochs and resolve the receiver positions as a batch. FGO tries to achieve two deliverables over the process. The first objective is to select the reliable cluster when multimodality is identified using region-growing. Meanwhile, the solution shifting problem can be mitigated by connecting multiple epochs measurements in an optimisation window. The position of the unimodal epoch and the selected confident cluster can constrain the drifted position solution. The contributions of this study are as follows.

1. 1. Identify the multimodality in the scoring surface of 3DMA GNSS and select the most likely cluster.

2. 2. Integrate the selected cluster information using FGO, comparing loosely coupled and hybrid-coupled architectures.

3. 3. Evaluate performance of different algorithms using data recorded in urban areas of London.

Experimental datasets, which are presented previously (Zhong and Groves, 2022b), are used to evaluate the performance of the proposed positioning algorithm. The testing took place in the City of London and Canary Wharf, UK. Both static and dynamic designed experiments were included to demonstrate the performance under different application scenarios. Positioning results show that 3DMA GNSS with clustering and FGO can provide a more robust positioning result than a grid filtering approach.

The remaining parts of this paper are structured as follows: Section 2 presents the proposed algorithm on the grid-based 3DMA GNSS with clustering using factor graph optimisation (FGO). The results of the designed experiment are shown and analysed in Section 3. Finally, the paper concludes and presents recommendations for the future in Section 4.

2. Integrated 3DMA GNSS with clustering and velocity using FGO

2.1. Overview of the integration on 3DMA GNSS with clustering and Doppler frequency estimated velocity using factor graph optimisation (FGO)

An overview flowchart of the proposed loosely coupled 3DMA GNSS with factor graph optimisation is shown in Figure 1.

Figure 1. Flowchart of the proposed algorithm (SM, shadow matching; LBR, likelihood-based ranging 3DMA GNSS).

Velocity is estimated from the Doppler measurements for the loosely coupled FGO and for the position solution propagation of the candidate distribution to the next epoch. After the GNSS Doppler measurements at epoch $k$ are given, a consistency check (Hsu et al., 2017) is performed to exclude Doppler measurement outliers. The fault detection scheme iteratively excludes the measurement with the largest residual, until one of the following criteria is met: (1) the degree of freedom of the problem is reached; (2) the position dilution of precision (PDOP) is equal or greater than 2.0; or (3) all residuals are within three standard deviations above or below their mean. After that, the receiver velocity, ${{\bf{v}}_{k,LS}}$ , and clock drift are estimated by the least-squares (LS) method (Wen and Hsu, 2021). This study applies outlier detection to prevent excessively large, estimated velocity values. The velocity is identified as an outlier if the acceleration between two epochs exceeds the acceleration due to gravity. The estimated velocity from the previous valid epoch replaces the velocity at epoch $k$ , while the variances of the corresponding epoch are doubled.

Meanwhile, the candidates are distributed based on the propagated position solution from the previous solution, while the first epoch is initiated by the conventional position solution. The likelihood score of each candidate is determined by the core 3DMA GNSS algorithms, as presented in Section 2.2. After the likelihood scores are obtained, the sampled candidates are clustered by region-growing, and the details are provided in Section 2.3. 3DMA GNSS with clustering is loosely integrated using FGO with velocity or Doppler measurements, as loosely or hybrid-coupled, respectively, as shown in Sections 2.4.1 and 2.4.2. Section 2.5 discusses the operation mode of FGO.

2.2. Candidate sampling and integration of shadow matching and likelihood-based ranging 3DMA GNSS

Positioning-hypothesis-candidate-based 3DMA GNSS first distributes the positioning hypothesis candidates. Then, a likelihood score is assigned to each candidate based on the similarity between predictions and measurements for each satellite. The scores of the candidates are used to estimate the receiver location as a weighted average of the candidate positions. The likelihood scoring surface evaluation follows the study by Zhong and Groves (2022b).

Positioning hypothesis candidate sampling is important for 3DMA GNSS to ensure the true receiver location is included. The initial location for candidate distribution is propagated based on the optimised position and velocity of the previous epoch. In contrast, the first epoch is initialised by the conventional single-epoch GNSS positioning with outlier detection and terrain-height aiding (Groves and Jiang, 2013). The centre of the sampling area, ${{\bf{r}}_{k,0}}$ , can be expressed as

(1) $${{\bf{r}}_{k,0}} = \left\{ {\matrix{ {{{\bf{r}}_{0,{\rm{conv}}}}} & {k = 0} \cr {{{\bf{r}}_{k - 1}} + {{\bf{v}}_{k - 1}} \times \Delta {t_{k,k - 1}}} & {k \gt 0} \cr } } \right.$$

where ${{\bf{r}}_{k - 1}}$ and ${{\bf{v}}_{k - 1}}$ are the optimised position and estimated velocity in an Earth-centred-Earth-fixed (ECEF) coordinate system at epoch $k - 1$ , respectively, and $\Delta {t_{k,k - 1}}$ is the time difference between epochs $k$ and $k - 1$ .

The sampling radius, ${R_k}$ , at epoch $k$ is set as twice the positioning uncertainty of the previous epoch $k - 1$ , for example, ${R_k} = 2 \times {\left\| {\bf{\Sigma }}_{k - 1}\right\|}$ , where ${{\bf{\Sigma }}_{k - 1}}$ is a diagonal variance-covariance (VC) matrix representing position variance at each axis of the solution at epoch $k$ and $\left\| \cdot \right\|$ is the root-sum-squared operation on the diagonal elements. Therefore, distributed candidates, ${{\bf{r}}_{k,j}}$ , form a grid within a circular area that obeys the condition,

(2) $${\left\| {\bf{r}}_{k,j} - {\bf{r}}_{k,0}\right\| _2 \lt {R_k}}$$

where ${\left\| \cdot \right\|_2}$ is the Euclidean distance between locations. Note that the grid spacing used in this study is 1 m, and only the candidates outside buildings are considered.

Considering their computational efficiency, this study integrates shadow matching and likelihood-based ranging 3DMA GNSS. The shadow matching and likelihood-based ranging 3DMA GNSS implementations follow the study by Zhong and Groves (2022b).

Shadow matching determines a visibility consistency score, $p_j^i$ , for each satellite $i$ and candidate $j$ :

(3) $$p_j^i = 1 - p\left( {LOS{\rm{|}}C/{N_0}^i} \right) - p\left( {LOS{\rm{|}}BB_j^i} \right) + 2p\left( {LOS{\rm{|}}C/{N_0}^i} \right)p\left( {LOS{\rm{|}}BB_j^i} \right)$$

where $p\left( {LOS{\rm{|}}BB_j^i} \right)$ is the probability that the satellite is predicted to be LOS at the corresponding candidate using the building boundaries precomputed from the 3D building model. Additionally, $p\left( {LOS{\rm{|}}C/{N_0}^i} \right)$ is the probability that the received signal is LOS as determined from the $C/{N_0}$ measurement using a statistical model, which can be found in Equations (A1) and (A2) of Zhong and Groves (2022b). Finally, an overall likelihood score from shadow matching, ${{\rm{\Lambda }}_{{S_j}}}$ , for position, $j$ , is obtained by multiplying the scores for each satellite:

(4) $${{\rm{\Lambda }}_{{S_j}}} = \mathop \prod \nolimits_i p_j^i$$

Likelihood-based ranging 3DMA GNSS first obtains measurement innovations at each candidate position by subtracting the predicted pseudorange from the measured pseudorange, then differencing with respect to the reference satellite to eliminate the receiver clock offset. The measurement innovation of each satellite and candidate is denotated $\delta {{z}}_j^i$ . If the satellite is predicted to be LOS at the corresponding candidate, the measurement innovation is modelled by a normal distribution. Otherwise, for satellites predicted to be NLOS, the measurement innovation is modelled by a skew-normal distribution and then remapped to an equivalent normal distribution using the cumulative probability. Remapping of the measurement innovation, denoted as $\delta z_j^{i'}$ , is described by Equations (A6)–(A11) of Zhong and Groves (2022b). The likelihood score for the likelihood-based ranging 3DMA GNSS, ${{\rm{\Lambda }}_{{R_j}}}$ , can then be calculated from the measurement innovations set, $\delta {\bf{z}}_j^\prime$ , using

(5) $${{\rm{\Lambda }}_{{R_j}}} = \exp \left( { - \delta {\bf{z}}{{_j^{\rm{\prime}}}^{\rm{T}}}{\bf{C}}_{\delta {\bf{z}}}^{ - 1}\delta {\bf{z}}_j^{\rm{\prime}}} \right)$$

where ${{\bf{C}}_{\delta {\bf{z}}}}$ is the measurement error covariance matrix, given by Equation (A13) of Zhong and Groves (2022b).

The overall likelihood score of the candidate, ${\tilde \Lambda _j}$ , can be calculated by the product of the scores from shadow matching and likelihood-based ranging 3DMA GNSS, as

(6) $${\tilde \Lambda _j} = {\Lambda _{{R_j}}}{\Lambda _{{S_j}}}^{{W_j}}$$

where ${W_j}$ is the weighting factor, given by Equation (A15) of Zhong and Groves (2022b).

The sampled candidates and estimated likelihood score are then inputted to the region-growing algorithm to classify into clusters.

2.3. Clustering sampled candidates with a region-growing algorithm

Region growing (Adams and Bischof, 1994) is commonly used in image segmentation. The position candidates are evenly distributed on the ground, making the samples similar to the pixels of an image. This section presents the processes used in this study to identify multimodality by separating the candidates into clusters. A simplified example of the region growing is shown in Figure 2.

Figure 2. A simplified example of the region-growing algorithm.

Region growing aims to select candidates with high scores that most influence the position estimation, while filtering out sampled candidates with low scores. Region growing requires initial seed locations for the growing process. The sampled candidates are first sorted in descending order, and their cumulative sum of the scores, ${\rm{{\rm K}}}$ , is calculated by

(7) $$K = {{\mathop \sum \nolimits_{j}}}{\tilde \Lambda _j}$$

An example is shown in Figures 2b and 2c, respectively. The seed locations are selected if the cumulative sum of the score is smaller than the threshold, ${\alpha _1}$ . Seed location selection and cluster forming are done in ascending order of the cumulative sum of the score, until adding a further candidate would increase the cumulative score of the clustered candidates above the threshold, ${\alpha _2}$ . The threshold ${\alpha _1}$ is the sum of the scores, ${\rm K}$ , multiplied by the constant ${c_1}$ . The sum of scores of the selected initial seed locations is thus at or just below ${\alpha _1}$ . Similarly, the threshold ${\alpha _2}$ is ${\rm K}$ multiplied by the constant ${c_2}$ , so the sum of scores of the selected growing candidates is at or just below ${\alpha _2}$ .

From the example shown in Figure 2d, the sorted scores in the blue curve are used to calculate the cumulative scores in the brown curve, and the sum of the scores is 159. Constants ${c_1}$ and ${c_2}$ are set empirically as 0.2 and 0.6, respectively, giving thresholds ${\alpha _1}$ and ${\alpha _2}$ of 32 and 97, respectively. Therefore, the candidates with cumulative scores lower than 97 are selected as the candidates for region growing, shown as black-framed candidates in Figure 2e. The candidates with cumulative scores lower than 32 are selected as the seed for region growing, as candidates in red text in Figure 2e. The remaining candidates in grey text are excluded from the following processes as they have lower scores than the sampled candidates.

After the initial seed and growing candidates are selected, each initial seed candidate is assumed to be a cluster and we examine whether the neighbours satisfy the predefined criteria: (1) it is a growing candidate and (2) the distance is smaller than the threshold, ${d_1}$ . The distance threshold is determined empirically as 1.5 units of the separations of the candidates. Satisfied neighbours are added to the cluster accordingly, and then the growing process is repeated iteratively until all valid growing candidates are examined. The example in Figure 2f found two seed locations at the top right-hand corner that are close to each other, so they are merged as a single cluster. The result indicates that two clusters were found.

Finally, the position, ${{\bf{r}}_m}$ , and covariance matrix, ${{\bf{\Sigma }}_m}$ , can be determined for each identified cluster with its position candidates, and indices denoted as $m$ :

(8) $${{\bf{r}}_m} = {{\mathop \sum \nolimits_{j'} {{\bf{r}}_{j'}} \cdot {{\tilde \Lambda }_{j'}}} \over {\mathop \sum \nolimits_{j'} {{\tilde \Lambda }_{j'}}}}\,{\rm{where\, }}j' = \left\{ {j \in m} \right\}$$

(9) $${{\bf{\Sigma }}_m} = {1 \over {\mathop \sum \nolimits_{j'} {{\tilde \Lambda }_{j'}}}}\mathop \sum \nolimits_{j'} {\tilde \Lambda _{j'}}{\left( {{{\bf{r}}_{j'}} - {{\bf{r}}_m}} \right)^{\rm{T}}}\left( {{{\bf{r}}_{j'}} - {{\bf{r}}_m}} \right){\rm{\,where}}\,j' = \left\{ {j \in m} \right\}$$

2.3.1. Selection of the cluster and visibility estimation

Selecting the correct cluster is the key to identify a better position solution that falls within the high likelihood score area with a smaller estimated covariance than estimated by all sampled candidates, especially when multimodal or solution shifting occurs. Theoretically, positioning should consider all possible combinations across different epochs and adjust their scores according to the consistency of their displacement between epochs with the velocity solution. However, testing all combinations is impractical, so this study focuses on retaining only the most likely cluster for future positioning. To ensure reliable cluster selection, NLOS Doppler measurements and abnormal estimated velocities are excluded; ray-tracing corrected Doppler measurements could be used in the future to improve the accuracy of the estimated velocity (Zhang et al., 2024). Additionally, scoring over the position domain should be preserved and one should evaluate the consistency between velocity in the cluster selection to increase the redundancy. Therefore, this study selects clusters based on both the estimated position and velocity of the previous epoch and the sum of the scores of the corresponding candidates. The cluster is selected, denoted as ${m^*}$ , if it has minimum on the product of Mahalanobis distance between its position and the propagated position of the previous epoch and the average score of itself. This can be expressed as

(10) $$\begin{align}{m^{\rm{*}}} & = \mathop {{\rm\raise-8pt{argma{\it x}}\atop{\it m}}} {{\mathop \sum \nolimits_{j'} {{\tilde \Lambda }_{j'}}} \over {{n_m}}}\exp \left( { - \sqrt {{\bf{d}}_m^{T}{\bf{\Sigma }}_m^{ - 1}{{\bf{d}}_m}} } \right),\,{\rm{where}}\,j' = \left\{ {j \in m} \right\}{\rm{\, and \,}}\,{{\bf{d}}_m} \\ & = {{\bf{r}}_m} - \left( {{{\bf{r}}_{k - 1}} + {{\bf{v}}_{k - 1}} \times \Delta {t_{k,k - 1}}} \right)\end{align}$$

where ${{\bf{r}}_{k - 1}}$ and ${{\bf{v}}_{k - 1}}$ are the optimised position and estimated velocity at epoch $k - 1$ , respectively. Here, ${n_m}$ is the total number of candidates in cluster $m$ , ${{\bf{d}}_m}$ is the vector of the differences between the cluster’s position and the propagated position at each axis, and $\Delta {t_{k,k - 1}} = {t_k} - {t_{k - 1}}$ is the time difference between epochs $k$ and $k - 1$ . The selected cluster is used as a position measurement in both loosely and hybrid-coupled FGO, which is described in Section 2.4. The estimated velocity provides robust information about position changes between consecutive epochs. However, relying solely on estimated velocities and a single absolute position as the initial point, known as dead reckoning, can be significantly impacted by errors in the initial location, resulting in a shift in the entire trajectory. Therefore, it is crucial to incorporate absolute position measurements into the estimation process to prevent error propagation caused by relying exclusively on velocity data.

Satellite visibility can also be estimated using the weighted average of the elevation angle difference between satellite’s and building boundary at the corresponding azimuth angle, $\beta $ , across all candidates with their scores in the selected cluster, ${m^*}$ . If ${\beta ^i} \gt 0$ , LOS is assigned to the visibility flag, and vice versa for pseudorange and Doppler measurement factors. It can be expressed as

(11) $${\beta ^i} = {1 \over {\mathop \sum \nolimits_{j'} {{\tilde \Lambda }_{j'}}}}\mathop \sum \nolimits_{j'} \left\{ {{{\tilde \Lambda }_{j'}} \times \left[ {{\theta ^i} - B{B_{j'}}\left( {{\psi ^i}} \right)} \right]} \right\}, \,{\rm{where \; }}j' = \left\{ {j \in {m^{\rm{*}}}} \right\}$$

where $j'$ is the candidates in the selected cluster ${m^*}$ , ${\psi ^i}$ and ${\theta ^i}$ are the azimuth and elevation angles for satellite $i$ , respectively $,\;B{B_{j'}}\left( {{\psi ^i}} \right)$ is the highest elevation angle of the building boundary at the corresponding azimuth angle, $\psi $ , at candidate $j'$ . The determined value determines the satellite visibility flag that affects the pseudorange factors formulation for hybrid-coupled FGO only, which is described in Section 2.4.2, on whether to apply the likelihood-based ranging remapping for measurement innovation. In addition, only the score from shadow matching is used for hybrid-coupled FGO when clustering in Section 2.3 and satellite visibility estimation in Equation (8) to avoid measurement duplication.

2.4. Integration of 3DMA GNSS and Doppler velocity using factor graph optimisation (FGO)

Two different factor graphs, a loosely coupled one and a hybrid-coupled one, have been implemented in this study. FGO is selected to optimise the states by minimising the residuals in the error factors associated with the corresponding factor graph. It achieves robust estimation through multiple iterations and accurately reflects the time correlation between measurements and states, even when measurements do not follow the Gaussian noise assumption, by using a batch of historical data (Wen et al., 2021). The state estimated at each epoch $k$ , ${{\bf{x}}_k}$ , including the receiver’s position, ${{\bf{r}}_k}$ , and receiver clock offset, $\delta {t_k}$ , as the state set ${\bf{\chi }} = \left[ {{{\bf{x}}_1},{{\bf{x}}_2}, \ldots, {{\bf{x}}_k}} \right]$ .

2.4.1. Loosely coupled

Loosely coupled FGO includes error factors on position of the selected cluster and motion propagation. Cluster position factors constrain the position solution for the corresponding epoch. In contrast, motion model factors connect and constrain the displacement between two consecutive epochs. The structure of the factor graph is shown in Figure 3.

Figure 3. Structure of the factor graph for the proposed algorithm on loosely coupled (LC) approach.

The position factor of the selected cluster loosely constrains the distance between the receiver’s position, ${{\bf{r}}_k}$ , and the selected cluster’s position, ${{\bf{r}}_{{m^*}}}$ . The position factor of the selected cluster can be expressed as

(12) $$\|{e_{k,{m^{\rm{*}}}}\|}_{{\bf{\Sigma }}_{k,{m^{\rm{*}}}}^{ - 1}}^2 = \|{{\bf{r}}_k} - {{\bf{r}}_{{m^{\rm{*}}}}\|}_{{\bf{\Sigma }}_{k,{m^{\rm{*}}}}^{ - 1}}^2{\rm{\;}}$$

where ${{\bf{\Sigma }}_{k,{m^*}}}$ is the covariance matrix of the selected cluster calculated by Equation (9) multiplied by the tuning factor, ${{\rm{c}}_3}$ .

The motion propagation factor is applied to the factor graph. The motion propagation factor connects the positions of two consecutive epochs and minimises the difference between the time-differenced position states and velocity derived from single-epoch Doppler measurements using the least-squares (LS) method (Wen and Hsu, 2021), denoted as ${{\bf{v}}_{k,LS}}$ , multiplied by the time interval. The factor can be written as

(13) $$\left\| {{e_{k,{\bf{v}}}}} \right\|_{{\bf{\Sigma }}_{k,{\bf{v}}}^{ - 1}}^2 = \left\| {\left( {{{\bf{r}}_{k + 1}} - {{\bf{r}}_k}} \right) - \Delta t \times {{\bf{v}}_{k,LS}}} \right\|_{{\bf{\Sigma }}_{k,{\bf{v}}}^{ - 1}}^2$$

where ${{\bf{\Sigma }}_{k,{\bf{v}}}}$ is the covariance matrix of the velocity estimated using weighted least squares and multiplied by the tuning factor, ${{\rm{c}}_4}$ .

Finally, the FGO for loosely coupled (LC) 3DMA GNSS with clustering is formulated,

(14) $${\bf{\chi }}_{LC}^{\rm{*}} = \mathop {{\rm\raise-9pt{argmin}}\atop{\bf{\chi }}} \mathop \sum \nolimits_k \left\{ \|{{e_{k,{m^{\rm{*}}}}\|}_{{\bf{\Sigma }}_{k,{m^{\rm{*}}}}^{ - 1}}^2 + \|{e_{k,{\bf{v}}}\|}_{{\bf{\Sigma }}_{k,{\bf{v}}}^{ - 1}}^2} \right\}$$

where ${\bf{\chi }}_{LC}^*$ denotes the optimal estimate of the state set of loosely coupled FGO.

2.4.2. Hybrid-coupled

Hybrid-coupled FGO loosely integrates error factor on position of the selected cluster, tightly integrates error factors on pseudorange and Doppler frequency measurements. The structure of the factor graph is shown in Figure 4.

Figure 4. Structure of the factor graph for the proposed algorithm on hybrid-coupled (HC) approach.

Each pseudorange factor optimises the estimated position states by minimising the residual between the measured pseudorange and predicted pseudorange at the relevant position and receiver clock, similar to measurement innovations in likelihood-based ranging 3DMA GNSS. Depending on the determined visibility, pseudorange measurement innovation is remapped by likelihood-based ranging 3DMA GNSS, as $\delta {{z}^i}\left( {{{\bf{x}}_k}} \right)$ . The pseudorange measurement factor can be expressed as

(15) $$\left\| {{e_{k,i,\rho }}} \right\|_{\sigma _{k,i,\rho }^{ - 1}}^2 = \left\| {\delta {{z}^i}\left( {{{\bf{x}}_k}} \right)} \right\|_{\sigma _{k,i,\rho }^{ - 1}}^2$$

where ${\sigma _{k,i,\rho }}$ is the error standard deviation of pseudorange measurement given by Equation (A5) of Zhong and Groves (2022b) times the tuning constant, ${{\rm{c}}_5}$ . The measurement innovation, $\delta {{\cal z}^i}\left( {{{\bf{x}}_k}} \right)$ , can be expressed as

(16) $$\delta {z^i}\left( {{{\bf{x}}_k}} \right) = \left\{ {\matrix{ {\tilde \rho _k^i - {{\hat \rho }^i}\left( {{{\bf{x}}_k}} \right)} & {LOS} \cr {{\rm{LBR}}\left( {\tilde \rho _k^i - {{\hat \rho }^i}\left( {{{\bf{x}}_k}} \right)} \right)} & {NLOS} \cr } } \right.$$

where ${\rm{LBR}}\left( \cdot \right)$ is the function to remap the measurement innovation that assumed to be NLOS to LOS, given by Equations (A6) to (A11) of Zhong and Groves (2022b).The visibility flag, $LOS$ or $NLOS$ , is determined using Equation (8). $\tilde \rho _k^i$ is the pseudorange measurement. Here, ${\hat \rho ^i}\left( {{{\bf{x}}_k}} \right)$ is the estimated pseudorange, given by

(17) $${\hat \rho ^i}\left( {{{\bf{x}}_k}} \right) = {r^i}\left( {{{\bf{r}}_k}} \right) + {\rm{c}} \times \left( {\delta {t_k} - \delta t_k^i} \right) + {T^i} + {I^i}$$

where ${r^i}\left( {{{\bf{r}}_k}} \right)$ is the geometric range between the satellite position and estimated receiver position with a Sagnac effect correction, $c$ is the speed of light, $\delta t_k^i$ is the satellite clock offset, and ${T^i}$ and ${I^i}$ are atmospheric errors on the tropospheric and ionospheric delays, respectively. This study uses Saastamoinen total delay model (Saastamoinen, 1973) and Klobuchar ionospheric model (Klobuchar, 1987) to model the tropospheric and ionospheric delays, respectively. Noted that only raw pseudorange measurements have to apply these offset compensations.

Similar to the pseudorange measurement factors, each Doppler frequency measurement factor optimises the state by minimising the difference between Doppler frequency measurement, ${\tilde D^i}$ , and modelled Doppler frequency, ${\hat D^i}\left( {{{\bf{x}}_k}} \right)$ . However, to model the Doppler frequency for NLOS signals, an exact reflecting point is required to estimate the line-of-sight unit vector from the receiver to the reflecting point. Therefore, only the LOS Doppler frequency measurements are used in the FGO. The LOS Doppler measurements are modelled by

(18) $${{\it{\lambda}} ^i}{\hat D^i}\left( {{{\bf{x}}_k}} \right) = \left[ {\left( {{{\bf{r}}_{k + 1}} - {{\bf{r}}_k}} \right)/\Delta t - {\bf{v}}_k^i} \right] \cdot {\bf{u}}_k^i + {\rm{c}} \cdot \left[ {\dot \delta t_k^i - \left( {\delta {t_{k + 1}} - \delta {t_k}} \right)/\Delta t} \right]$$

where ${\lambda ^i}$ is the wavelength of the signal, $\Delta t$ is the time difference between epochs $k$ and $k + 1$ , ${\bf{v}}_k^i$ and $\dot \delta t_k^i$ are the satellite’s velocity and clock drift provided by the ephemeris, and ${\bf{u}}_k^i$ is the LOS unit vector from the satellite to the receiver.

Therefore, the Doppler frequency measurement factor is expressed as

(19) $$\|{e_{k,i,D}\|}_{\sigma _{k,i,D}^{ - 1}}^2 = \|{{\it{\lambda}} ^i}{\tilde D^i} - {{\it{\lambda}} ^i}{\hat D^i}\left( {{{\bf{x}}_k}} \right)\|_{\sigma _{k,i,D}^{ - 1}}^2$$

where ${\sigma _{k,i,D}} = 0.1 \times {\sigma _{k,i,\rho }}$ is the error standard deviation of Doppler frequency measurement. This study empirically assumes that the error standard deviation of Doppler frequency measurement is 10 times smaller than that of pseudorange measurement.

As a result, the FGO for hybrid-coupled (HC) 3DMA GNSS with clustering is formulated,

(20) $${\bf{\chi }}_{HC}^{\rm{*}} = \mathop {{\rm\raise-9pt{argmin}}\atop{\bf{\chi }}}\mathop \sum \nolimits_k \left\{ \|{{e_{k,{m^{\rm{*}}}}\|}_{{\bf{\Sigma }}_{k,{m^{\rm{*}}}}^{ - 1}}^2 + \mathop \sum \nolimits_i \|{e_{k,i,\rho }}\|_{\sigma _{k,i,\rho }^{ - 1}}^2 + \mathop \sum \nolimits_i \|{e_{k,i,D}}\|_{\sigma _{k,i,D}^{ - 1}}^2} \right\}$$

where ${\bf{\chi }}_{HC}^*$ denotes the optimal estimate of the state set of hybrid-coupled FGO.

3. Experiment results and analysis

3.1. Experiment setup

The positioning performance of the proposed algorithms is evaluated using GNSS data recorded in the City of London and Canary Wharf, UK. Both static and dynamic experiments are included to better illustrate different real-world positioning scenarios. Static tests were performed for the datasets in the City of London. A vehicular experiment was performed in Canary Wharf.

Static datasets in the City of London were collected at various locations in July 2017 using a u-blox EVK-M8T, which outputs single-frequency GNSS measurements on GPS, GLONASS, and Galileo at 1 Hz. The antenna was 1.1 m above the ground. Test sites around the City of London represent typical European city environments with narrow roads, and the walls are generally constructed with masonry. There are 23 test locations, and each datum was collected for 2 min at each location. A total of 2,760 epochs exist in the dataset. The average elevation angle of the highest building boundary at ground truth and the average received SNR of received satellites of each experiment are shown in Figure 5. Most of the experiment locations are in deep urban areas, where the building blockage exceeds 40 degrees in most of the experiment. The tuning constant ${c_3} = 1.0$ , ${c_4} = 1.0$ and ${c_5} = 1.0$ are set for the error factors for selected cluster’s position, motion propagation and pseudorange measurements, respectively.

Figure 5. (a) Average elevation angle of the highest building boundary at ground truth and the average received SNR across experiments. Skymask on the ground truth of test location, (b) C5 and (c) C12_N with received and non-received satellites.

Vehicular data in Canary Wharf were recorded in July 2019 with a Racelogic Labsat 3 GNSS front-end in intermediate frequency samples and post-processed by Focal Point Positioning Ltd. The dataset spans 1,200 epochs and comprises single-frequency GPS and Galileo measurements. The reference trajectory of the vehicular data was provided by a NovAtel iMAR INS/GNSS system. The tuning constants ${c_3} = 1.0$ , ${c_4} = 10.0$ and ${c_5} = 5.0$ are set for the error factors related to the selected cluster’s position, motion propagation and pseudorange measurements, respectively. The constants were tuned using data from the same set, following the methodology outlined by Zhong and Groves (2022b).

3.2. Positioning results

The recorded datasets are post-processed and the following algorithms are compared.

1. 1. Device output: Position solution output by the device.

2. 2. Single epoch conv.: Single-epoch conventional GNSS with outlier detection and terrain-height aiding.

3. 3. Single-epoch 3DMA GNSS: Single-epoch 3DMA GNSS without clustering.

4. 4. Conv. EKF: Conventional extended Kalman filter (EKF) with terrain-height aiding.

5. 5. 3DMA GNSS GF: 3DMA GNSS grid filter (GF) without clustering.

6. 6. LC FGO 3DMA GNSS: Loosely coupled (LC) FGO on 3DMA GNSS.

7. 7. LC FGO 3DMA GNSS-C: Loosely coupled (LC) FGO on 3DMA GNSS with clustering using region growing.

8. 8. HC FGO 3DMA GNSS: Hybrid-coupled (HC) FGO on 3DMA GNSS.

9. 9. HC FGO 3DMA GNSS-C: Hybrid-coupled (HC) FGO on 3DMA GNSS with clustering using region growing.

The 3DMA GNSS with clustering requires temporal information to select the best cluster, so only the multi-epoch positioning methods are involved in the comparison. As the grid filter and particle filter produce similar results (Zhong and Groves, 2022b), only one of them has been used here. The grid filter is a convenient choice as it uses the same grid of candidate positions as the 3DMA GNSS algorithms. FGO is implemented using the Google Ceres Solver (Agarwal and Mierle, 2012). The sliding window size for the optimisation is empirically set to 200 s. This value is set based on the study by Zhong and Groves (2022b).

3.2.1. Static experiments

Statistics and histograms of the horizontal radial positioning error comprising the root mean square error (RMSE), 50th, 90th and 95th percentile errors across all static experiments are shown in Table 1 and Figure 6, respectively. Overall, the positioning performance of the single-epoch conventional approach is the worst, with an RMSE of nearly 14 m. This worst positioning performance is due to the signal blockage and reflection by the buildings in urban canyons. The device solution provides performance comparable to the 3DMA GNSS-based methods at the 50th percentile and in terms of RMSE, but not at 90th and 95th percentiles. The positioning accuracies of single-epoch 3DMA GNSS, the conventional EKF and the 3DMA GNSS grid filter are similar, with RMSEs of approximately 11.5 m. The FGO-based approaches reduce the RMSE to less than 10 m.

Table 1. Statistics of all static experiments across different algorithms. (RMSE, root mean square error)

Multi-epoch positioning algorithms generally demonstrate better positioning performance than the single-epoch positioning algorithms for the 90th and 95th percentile errors. However, the 3DMA GNSS grid filter generally provides a similar accuracy to the single-epoch 3DMA GNSS particularly for the RMSE and the 50th percentile errors.

The FGO-based approaches provide better positioning performance than the 3DMA GNSS grid filter for the RMSE and 95th percentile errors, but the grid filter is better at the 50th and 90th percentiles. The FGOs perform better than single-epoch 3DMA GNSS except at the 50th percentile. The 95th percentile errors suggest that the FGO-based approach is more robust against outliers.

Comparing the different FGO-based approaches, both loosely and hybrid-coupled approaches provide similar positioning performance in static experiments. FGO 3DMA GNSS with clustering is generally more accurate than without clustering, with an improvement of approximately a metre. Overall, FGO 3DMA GNSS, with clustering and combined direction optimisation, provides the best positioning performance compared with the others.

There was significant variation in performance across the different experimental locations. Two of these sites are selected as examples in more detail. Test location C14_W is selected as an example of where with clustering methods outperform methods without clustering and grid filter approaches. Here, the RMSE of LC and HC FGO with clustering are approximately 6.5 m and 6 m, respectively, while the RMSE of LC and HC FGO without clustering are approximately 9 m and the grid filter RMSE is approximately 10 m. From the map plot shown in Figure 7b, the position estimates of approaches without clustering fall on the opposite side of the street of the ground truth. Figure 7c shows the two-dimensionnal positioning error in the lateral street direction for the single-epoch 3DMA GNSS and selected cluster; the error with clustering is much smaller than that without.

Figure 6. Root mean square error (RMSE), 50th, 90th and 95th percentile of the horizontal radial positioning error for the static experiments across different algorithms.

Figure 7. (a) Horizontal errors, (b) map plot on positioning results, (c) horizontal error in lateral street direction and (d) example of multiple clusters at test location C14_W.

The discussion on the clustering performance can be divided into multiple clusters and single-cluster cases. A multiple clusters case is shown in Figure 7d. The two clusters are identified: one covers the truth location and one falls on the opposite side of the street to the actual location. This shows that the clustering can potentially identify the peaks. With a correctly selected cluster, this can mitigate the effects of multimodality.

However, the clustering method may worsen the positioning performance if the peak shifts off from the actual location. For location C15_E, the RMSE of the grid filter is smaller than that of the FGO approaching approximately 11 m, while the RMSEs of LC and HC FGO without clustering are approximately 12 m. After applying clustering, the RMSE increased to over 17 m. The positioning performance of the grid filter and FGO without clustering are similar over the whole experiment period; the grid filter performs approximately 2 m better during 40 s to 80 s, as shown in Figure 8a. However, the RMSE increased to approximately 20 m after applying the clustering, especially around 30 to 60 s. This error is mainly due to the offset in the lateral street direction, as shown in Figure 8c, while the error in the longitudinal direction is similar with and without clustering, as shown in Figure 8d. The positioning error of the selected cluster during the first 40 s and after 80 s shows a large error in the lateral street direction compared with the case without clustering. This result shows that clustering could be a double-edged sword where a correctly identified cluster can provide a more accurate positioning, while if a wrong cluster is identified, caused by a shift in the high score peak, the positioning might become worse. This is also reflected in the overall positioning performance; the positioning performance with clustering is better than that without clustering at the 50th percentile, but performance becomes worse at the 95th percentile.

Figure 8. (a) Horizontal errors, (b) map plot on positioning results, horizontal error in (c) lateral and (d) longitudinal street direction at test location C15_E.

Figure 9. Root mean square error (RMSE), 50th, 90th and 95th percentile of the horizontal radial positioning error for the vehicular experiment using different algorithms.

Figure 10. (a) Positioning error, (b) trajectories on map plot, (c) velocity error and (d) accumulated scoring surface between epochs 520 and 650 s of the vehicular experiment.

3.2.2. Vehicular experiment

The RMSE, 50th, 90th and 95th percentile errors of the vehicular experiment are shown in Table 2 and Figure 9. For the dynamic case, the positioning RMSE of single-epoch conventional and 3DMA GNSS are the worst, at over 40 m. The conventional EKF improves the positioning accuracy to approximately 30 m. The FGO and grid filter 3DMA GNSS enhances the positioning RMSE to within 20 m. Grid filter performs better than all FGO-based approaches on the 50th percentile of errors, while FGO approaches have smaller error in RMSE, the 90th and 95th percentile errors.

Table 2. Statistics of vehicular experiment across different algorithms. (RMSE, root mean square error)

The performance of the FGO-based methods is worse than the grid filter-based method at the 50th percentile. However, LC and HC FGO with clustering outperform the grid filter for RMSE. All FGO-based methods outperform the grid filter at the 90th and 95th percentile. This indicates that the FGO-based approach is better at mitigating outliers, while the grid filter-based approach maintains better average position accuracy. In further comparisons among the grid filter, loosely coupled FGO and hybrid-coupled forward FGO, the grid filter generally outperforms the loosely coupled FGO, but not the hybrid-coupled FGO. This is due to the different types of measurements used. The loosely coupled FGO relies solely on positions and velocities for position estimation, whereas the grid filter incorporates Doppler measurements for system propagation and updates, providing a more direct impact on position estimation. Additionally, the hybrid-coupled FGO uses both pseudorange and Doppler measurements directly in position estimation, offering enhanced robustness. However, the positioning error of HC FGO without clustering is noticeably larger than that of other FGO-based methods at the 95th percentile. This is because the visibility evaluation in Equation (11) is affected by low-score candidates that provide incorrect elevation angle differences, leading to a skewed weighted average and incorrect visibility classification results.

However, the FGO-based approach may perform worse than the grid filter, as the example between epochs 520 and 650 shows in Figure 10. Although the positioning error of the grid filter 3DMA GNSS exceeds 50 m during epochs 570 s to 610 s, the error decreases to within 20 m after epoch 610. The position error is perpendicular to the driving direction before the turn. In contrast, FGOs exhibit a sudden jump in position error around epoch 530, but the hybrid-coupled FGO with clustering quickly reduces the error to approximately 20 meters. This large error is due to a large velocity error when the vehicle passed through a tunnel, where only a few Doppler measurements were accepting, resulting in a large DOP. Afterwards, between 530 s and 550 s, LC and HC FGO with clustering has a smaller positioning error than the FGOs without clustering. This shows that the clustering approach can recover from an erroneous position faster than without clustering. During epochs 570 s to 610 s, the positioning error of the grid filter approach is larger than the FGO-based approaches. This could be caused by the velocity error at around 560 s, resulting in the grid filter searching candidate locations in the wrong place. However, for the FGO, the search area covers the actual driving trajectory across this period, as shown in Figure 10d.

4. Conclusions and future work

This study uses FGO to integrate a 3DMA GNSS with clustering and the estimated velocity from Doppler measurements. The clustering process separates the individual peaks in a multimodal position likelihood distribution instead of simply taking the mean. By selecting the most appropriate cluster using the propagated location based on the position solution and estimated velocity from the previous epoch, a more accurate receiver location and position uncertainty can be provided. Thus, the solution shifting issues can be mitigated by integrating the solution from multiple epochs. This study uses FGO to integrate the positions of the selected cluster in two modes: with the estimated velocity as loosely coupled; and pseudorange and Doppler measurements as hybrid-coupled approach. This study uses data recorded in London to evaluate the proposed algorithms. The proposed FGO-based 3DMA GNSS with clustering approaches outperforms the 3DMA GNSS grid filter, using both forward and combined optimisation with a position RMSE of approximately 10 m for the static datasets. However, for the vehicular experiment, the grid filter gives the best performance with an RMSE of approximately 19 m, while LC and TC FGO 3DMA GNSS with clustering have an RMSE of approximately 22.5 m.

Dividing the candidate positions into clusters based on their likelihood scores enables more accurate positioning when the 3DMA GNSS position distribution is multimodal. Hybrid integrating GNSS measurement provides more accurate positioning than a loosely coupled manner. However, results identify that visibility estimation is important for HC FGO 3DMA GNSS with clustering. More research is needed to determine suitable measurements input to the FGO.

This study uses the predicted position obtained from the previous positioning estimate and the estimated velocity or relative position to determine the most appropriate cluster. Selecting the correct cluster is important for the proposed method to provide the correct information to the optimisation processes. Thus, further research on cluster selection is needed. One of the directions is adopting multi-hypothesis testing of all combinations of the identified clusters and taking the weighted average using their probabilities.

Furthermore, outlier measurements are currently excluded using a consistency check, and NLOS pseudorange innovations are remapped to a LOS one using NLOS measurements’ error distribution with likelihood-based ranging 3DMA GNSS method. Meanwhile, NLOS Doppler measurements are excluded from the optimisation. These approaches may not perfectly mitigate measurement errors in position estimation, so robust optimisation methods, such as M-estimator, in the future could enhance positioning accuracy.