Introduction

Global navigation satellite systems (GNSS) are the dominant source for positioning, velocity and time (PVT) and are widely utilized in various applications (Zhang et al. 2018). Smart devices with chip-scale GNSS receivers can generate meter-level position results in a standalone manner in a GNSS friendly environment. In GNSS, the user receivers determine the PVT solutions with a least square (LS) method or Kalman filter (KF). In the LS-based PVT determination, the extracted pseudorange and pseudorange rate measurements, together with the position and velocity of satellites in view, are utilized to compute the PVT information at each epoch independently. The inherent relationship between the velocity and the position are ignored (Jiang et al. 2020). In the KF-based PVT determination a position-velocity (PV) dynamic motion model is utilized to estimate the PVT information and the velocity is utilized to smooth the position (Xu and Hsu 2019; Jiang et al. 2021). In fact, not only are adjacent positions correlated through velocity, but also all historical position information is correlated one by one through the velocity. Utilizing the inherent relationship among these states in the time domain might further decrease the position errors. Optimizing these states estimation together at an epoch is a prospective method to obtain better positioning results.

Nonlinear optimization originates from the simultaneous localization and mapping (SLAM) algorithm and can optimize the state estimation by considering historical information (Schubert et al. 2021; Indelman et al. 2012). Unlike the KF, all the state propagation and measurement constraints are used to construct a cost function. Optimal estimations of these states are solved by minimizing the cost function through an iterative method. Such nonlinear optimization methods have been extended to various applications, and their better performance versus KF has been comprehensively assessed and analyzed (Dellaert 2012). Therefore, it is of great significance to explore what benefits the optimization method could bring to GNSS receivers. We develop a nonlinear optimization method with its application in smartphone GNSS position smoothing. Contributions are summarized as:

  1. (1)

    An optimization program suitable for GNSS position smoothing applications is proposed and implemented; state propagation and measurement models are employed as the constraints in optimally estimating these states. In addition, the Levenberg–Marquardt (LM) algorithm is employed to solve for the optimal states in our code.

  2. (2)

    An optimization method is developed, and the resultant code will be open-source. In addition, a detailed user manual is provided together with more example experiments, and we hope it will provide a reference or an illustrative example for implementing the optimization method suitable for other GNSS applications, i.e., signal tracking or vector tracking.

The remaining sections describe the methodology, including the PVT determination equations, the KF method and the proposed optimization method. Then, the following experimental sections present a test case, the results and the quantified and in-depth analysis are revealed and presented. Finally, the conclusions are drawn, and suggestions for future work are given.

Methodology

This section introduces the model and methods utilized in the GNSS PVT determination. First, the PVT determination model with the pseudorange measurements is presented; second, the LS and KF methods are introduced in the PVT determination, and the state and measurement models are presented.

PVT determination

In a GNSS receiver, considering the clock bias, the position vector \({\mathbf{p}}_{u}\) is usually defined as

$${\mathbf{p}}_{u} = \left[ {x_{u} \, y_{u} \, z_{u} } \right]$$
(1)

where \({\mathbf{p}}_{u} = \left[ {x_{u} \, y_{u} \, z_{u} } \right]\) is the three-axis position in the Earth-Centered-Earth-Fixed (ECEF) coordinates. Assuming there are N in-view satellites, the relationship of the pseudorange, the user position, and the position of the satellites are given by (Hofmann-Wellenhof et al. 2007)

$$\sqrt {\left( {x_{{{\text{sv}}}}^{\left( i \right)} - x_{u} } \right)^{2} + \left( {y_{{{\text{sv}}}}^{\left( i \right)} - y_{u} } \right)^{{2}} + \left( {z_{{{\text{sv}}}}^{\left( i \right)} - z_{u} } \right)^{{2}} } + c \cdot t_{{\text{b}}} = \rho^{\left( i \right)}$$
(2)

where tb denotes the user local clock bias, \(\left[ {x_{{{\text{sv}}}}^{\left( i \right)} \, y_{{{\text{sv}}}}^{\left( i \right)} \, z_{{{\text{sv}}}}^{\left( i \right)} } \right]\) denotes the coordinates of the ith satellite in the ECEF system, the constant c denotes the light speed, \(\rho^{\left( i \right)}\) denotes the pseudorange of between the ith satellite and the user. The pseudorange has been corrected, which means the ionospheric, tropospheric, relativistic and group delays and satellite clock offsets have been corrected. Similarly, the velocity computation model is given by

$${\mathbf{H}}_{{{\text{LOS}}}}^{i} \cdot \left( {{\mathbf{v}}_{u} - {\mathbf{v}}_{{{\text{sv}}}}^{i} } \right)^{{\text{T}}} + c \cdot t_{{\text{d}}} = \dot{\rho }^{\left( i \right)}$$
(3)

where \({\mathbf{H}}_{{{\text{LOS}}}}^{i} = \left[ { - l_{x}^{\left( i \right)} \, - l_{y}^{\left( i \right)} \, - l_{z}^{\left( i \right)} } \right]\), \(\left[ { - l_{x}^{\left( i \right)} \, - l_{y}^{\left( i \right)} \, - l_{z}^{\left( i \right)} } \right] = \left( {{\mathbf{p}}^{\left( i \right)} - {\mathbf{p}}_{u} } \right){/}\left\| {{\mathbf{p}}^{\left( i \right)} - {\mathbf{p}}_{u} } \right\|\) is the unit vector pointing from the receiver to the ith satellite, \({\mathbf{v}}_{u} = \left[ {v_{x} \, v_{y} \, v_{z} } \right]\) denotes the user three-axis velocity in the ECEF system, \({\mathbf{v}}_{{{\text{sv}}}}^{i} = \left[ {v_{{{\text{sv}},x}}^{\left( i \right)} \, v_{{{\text{sv}},y}}^{\left( i \right)} \, v_{{{\text{sv}},z}}^{\left( i \right)} } \right]\) refers to the ith satellite velocity in the ECEF coordinate, \(\dot{\rho }^{\left( i \right)}\) denotes the ith satellite pseudorange rate measurement, td denotes the user local clock drift.

Least squares method

With the PVT model in (2–3), the LS algorithm is usually utilized to compute the PVT solutions. Expanding (2) at \({\mathbf{p}}_{u}^{0} = \left[ {x_{u}^{0} \, y_{u}^{0} \, z_{u}^{0} } \right]\) and ignoring higher-order terms (Jiang et al. 2020), the new equation is written as

$$\Delta {{\varvec{\uprho}}} = \left[ {\begin{array}{*{20}c} {\Delta \rho^{\left( 1 \right)} } \\ \vdots \\ {\Delta \rho^{\left( i \right)} } \\ \vdots \\ {\Delta \rho^{\left( N \right)} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {{\mathbf{H}}_{{{\text{LOS}}}}^{1} } & 1 \\ \vdots & \vdots \\ {{\mathbf{H}}_{{{\text{LOS}}}}^{i} } & 1 \\ \vdots & \vdots \\ {{\mathbf{H}}_{{{\text{LOS}}}}^{N} } & 1 \\ \end{array} } \right] \, \cdot \left[ {\Delta {\mathbf{p}}_{u} \, c \cdot \Delta t_{{\text{b}}} } \right]^{{\text{T}}} + {{\varvec{\upnu}}}_{\rho } = {\mathbf{A}} \cdot {\mathbf{B}} + {{\varvec{\upnu}}}_{\rho }$$
(4)

Similar to (4), the relationship between the change in velocity and the change of the pseudorange rates is given by

$$\Delta {\dot{\mathbf{\rho }}} = \left[ {\begin{array}{*{20}c} {\Delta \dot{\rho }^{\left( 1 \right)} } \\ \vdots \\ {\Delta \dot{\rho }^{\left( i \right)} } \\ \vdots \\ {\Delta \dot{\rho }^{\left( N \right)} } \\ \end{array} } \right] = \left[ {\begin{array}{*{20}c} {{\mathbf{H}}_{{{\text{LOS}}}}^{1} } & 1 \\ \vdots & \vdots \\ {{\mathbf{H}}_{{{\text{LOS}}}}^{i} } & 1 \\ \vdots & \vdots \\ {{\mathbf{H}}_{{{\text{LOS}}}}^{N} } & 1 \\ \end{array} } \right] \cdot \left[ {\Delta {\mathbf{v}}_{u} \, c \cdot \Delta t_{b} } \right]^{{\text{T}}} + {{\varvec{\upnu}}}_{{\dot{\rho }}}$$
(5)

where \({{\varvec{\upnu}}}_{{\dot{\rho }}}\) denotes the pseudorange noise vector, and the velocity can be computed using the similar LS method. With (4–5), the PVT is determined with the LS method; more details are presented in the reference (Jiang et al. 2020).

Kalman filter

In the KF method, the Position-Velocity dynamic state transformation model and the measurement model are given as

$${\mathbf{X}}_{k + 1} = \left[ \begin{gathered} \left( {{\mathbf{p}}_{u,k + 1} } \right)^{{\text{T}}} \hfill \\ \left( {{\mathbf{v}}_{u,k + 1} } \right)^{{\text{T}}} \hfill \\ \end{gathered} \right] = {\mathbf{F}}_{k,k + 1} \cdot {\mathbf{X}}_{k} + {\mathbf{w}}_{k + 1}$$
(6)
$${\mathbf{z}}_{k + 1} = {\mathbf{H}}_{k + 1} \cdot {\mathbf{X}}_{k + 1} + {{\varvec{\upeta}}}_{k + 1}$$
(7)

where \({\mathbf{F}}_{k,k + 1} = \left[ {\begin{array}{*{20}c} {{\mathbf{I}}_{3 \times 3} } & {\left[ {\begin{array}{*{20}c} {T_{0} } & 0 & 0 \\ 0 & {T_{0} } & 0 \\ 0 & 0 & {T_{0} } \\ \end{array} } \right]} \\ 0 & {{\mathbf{I}}_{3 \times 3} } \\ \end{array} } \right]\) denotes the state transformation matrix, the variable T0 denotes the position updating duration, and its value is one second. \({\mathbf{H}}_{k + 1} = \left[ \begin{gathered} {\mathbf{I}}_{3 \times 3} \, {\mathbf{0}}_{3 \times 3} \hfill \\ {\mathbf{0}}_{3 \times 3} \, {\mathbf{I}}_{3 \times 3} \hfill \\ \end{gathered} \right]\), and \({\mathbf{z}}_{k + 1}\) is the measurement vector which is composed of position and velocity from the LS method. \({\mathbf{w}}_{k + 1}\) denotes the process noise vector, which is assumed to be subject to Gaussian distribution with zero mean value. \({{\varvec{\upeta}}}_{k + 1}\) denotes the measurement noise vector, which is assumed to be subject to Gaussian distribution with zero mean value. With the state and measurement models, the KF estimates the position through the five basic equations, which can be found in Jiang et al. (2020).

Optimization

Unlike the KF, all the past states are regarded as unknowns in the optimization method. These states and measurements are highly correlated through the state propagation model. We construct an optimization method that considers all the past state vectors as unknowns; the model is given by

$$\begin{aligned} {\hat{\mathbf{Y}}} = \left( {{\hat{\mathbf{X}}}_{2} ,...,{\hat{\mathbf{X}}}_{k + 1} } \right) & = \arg \min \left( {f\left( {\mathbf{Y}} \right)} \right) = \arg \min \left( {f\left( {{\mathbf{X}}_{2} ,...,{\mathbf{X}}_{k + 1} } \right)} \right) \\ & = \arg \min \left( {\sum\limits_{{i = {1}}}^{k} {\left( {\left( {{\mathbf{X}}_{i + 1} - {\mathbf{F}}_{i,i + 1} \cdot {\mathbf{X}}_{i} } \right) \cdot {\mathbf{Q}}_{i + 1}^{ - 1} \cdot \left( {{\mathbf{X}}_{i + 1} - {\mathbf{F}}_{i,i + 1} \cdot {\mathbf{X}}_{i} } \right)^{{\text{T}}} } \right)} } \right) \\ & \quad + \arg \min \left( {\sum\limits_{i = 1}^{k} {\left( {\left( {{\mathbf{z}}_{{i{ + 1}}} - {\mathbf{H}}_{i + 1} \cdot {\mathbf{X}}_{i + 1} } \right) \cdot {\mathbf{R}}_{i + 1}^{ - 1} \cdot \left( {{\mathbf{z}}_{{i{ + 1}}} - {\mathbf{H}}_{i + 1} \cdot {\mathbf{X}}_{i + 1} } \right)^{{\text{T}}} } \right)} } \right) \\ \end{aligned}$$
(8)

where \({\mathbf{Q}}_{i + 1}\) denotes covariance matrix of the process noise vector \({\mathbf{w}}_{i}\) in (6), \({\mathbf{R}}_{{k{ + 1}}}\) denotes covariance matrix with covariance matrix of the measurement noise vector \({{\varvec{\upeta}}}_{k + 1}\) in (7).

Optimal estimation of the state vector \({\mathbf{Y}} = \left( {{\mathbf{X}}_{2} ,...,{\mathbf{X}}_{k + 1} } \right)\) can be solved by finding the minimal values of (8). The key is how to solve for the optimal estimates with (8). Here, we implement the Levenberg–Marquart (LM) algorithm to solve for the optimal estimations (Dellaert 2012). Note that Gauss–Newton and Powell’s dog leg methods are also usable to find similar optimal estimates. The three-step solution of the LM method is described as follows:

  1. (1)

    In the first step, expanding the function \(f\left( {\mathbf{Y}} \right)\) at \(\left( {{\tilde{\mathbf{X}}}_{2} ,...,{\tilde{\mathbf{X}}}_{k + 1} } \right)\left( {{\tilde{\mathbf{X}}}_{2} ,...,{\tilde{\mathbf{X}}}_{k + 1} } \right)\) and ignoring high order terms, the following equation is obtained

    $$\begin{aligned} f\left( {{\tilde{\mathbf{Y}}} + \Delta {\mathbf{Y}}} \right) & \cong \sum\limits_{{i = {1}}}^{k} {\left( {\left( {\left( {{\tilde{\mathbf{X}}}_{i + 1} + \Delta {\mathbf{X}}_{i + 1} } \right) - {\mathbf{F}}_{i,i + 1} \cdot \left( {{\tilde{\mathbf{X}}}_{i} + \Delta {\mathbf{X}}_{i} } \right)} \right) \cdot {\mathbf{Q}}^{{ - {1}}} \cdot \left( {\left( {{\tilde{\mathbf{X}}}_{i + 1} + \Delta {\mathbf{X}}_{i + 1} } \right) - {\mathbf{F}}_{i,k + 1} \cdot \left( {{\tilde{\mathbf{X}}}_{i} + \Delta {\mathbf{X}}_{i} } \right)} \right)^{{\text{T}}} } \right)} \\ & \quad + \sum\limits_{i = 1}^{k} {\left( {\left( {{\mathbf{z}}_{{i{ + 1}}} - {\mathbf{H}}_{i + 1} \cdot \left( {{\tilde{\mathbf{X}}}_{i + 1} + \Delta {\mathbf{X}}_{i + 1} } \right)} \right) \cdot {\mathbf{R}}^{{ - {1}}} \cdot \left( {{\mathbf{z}}_{{i{ + 1}}} - {\mathbf{H}}_{i + 1} \cdot \left( {{\tilde{\mathbf{X}}}_{i + 1} + \Delta {\mathbf{X}}_{i + 1} } \right)} \right)^{{\text{T}}} } \right)} \\ \end{aligned}$$
    (9)

Equation (9) can be simplified as

$$f\left( {{\tilde{\mathbf{Y}}} + \Delta {\mathbf{Y}}} \right) \cong \sum\limits_{i = 1}^{k} {\left( {c_{i} + {2}{\mathbf{b}}_{i} \cdot \left[ \begin{gathered} \Delta {\mathbf{X}}_{{i{ + 1}}} \hfill \\ \Delta {\mathbf{X}}_{i} \hfill \\ \end{gathered} \right] + \left[ \begin{gathered} \Delta {\mathbf{X}}_{{i{ + 1}}} \hfill \\ \Delta {\mathbf{X}}_{i} \hfill \\ \end{gathered} \right] \cdot {\mathbf{a}}_{i} \cdot \left[ \begin{gathered} \Delta {\mathbf{X}}_{{i{ + 1}}} \hfill \\ \Delta {\mathbf{X}}_{i} \hfill \\ \end{gathered} \right]^{{\text{T}}} } \right)}$$
(10)

where

$$c_{i} = {\mathbf{err}}_{{{\text{st}},i}} \cdot {\mathbf{Q}}^{{ - {1}}} \cdot \left( {{\mathbf{err}}_{{{\text{st}},i}} } \right)^{{\text{T}}} + {\mathbf{err}}_{{{\text{st}},i}} \cdot {\mathbf{R}}^{{ - {1}}} \cdot \left( {{\mathbf{err}}_{{{\text{st}},i}} } \right)^{{\text{T}}}$$
(11)
$${\mathbf{b}}_{i} = {\mathbf{err}}_{{{\text{st}},i}} \cdot {\mathbf{Q}}^{{ - {1}}} \cdot {\mathbf{J}}_{i} + {\mathbf{err}}_{{{\text{st}},i}} \cdot {\mathbf{R}}^{{ - {1}}} \cdot \left( {{ - }{\mathbf{H}}_{i + 1} } \right)$$
(12)
$${\mathbf{a}}_{i} = {\mathbf{J}}_{i} \cdot {\mathbf{Q}}^{{ - {1}}} \cdot {\mathbf{J}}_{i}^{{\text{T}}} + {\mathbf{H}}_{i + 1} \cdot {\mathbf{R}}^{{ - {1}}} \cdot \left( {{\mathbf{H}}_{i + 1} } \right)^{{\text{T}}}$$
(13)

Equation (10) can be re-written in a general form

$$f\left( {{\tilde{\mathbf{Y}}} + \Delta {\mathbf{Y}}} \right) \cong \sum\limits_{i = 1}^{k} {\left( {c_{i} + 2{\mathbf{b}}_{i} \cdot {\mathbf{C}}_{i} \cdot \Delta {\mathbf{Y}} + \left( {{\mathbf{C}}_{i} \cdot \Delta {\mathbf{Y}}} \right)^{{\text{T}}} \cdot {\mathbf{a}}_{i} \cdot {\mathbf{C}}_{i} \cdot \Delta {\mathbf{Y}}} \right)}$$
(14)
  1. (2)

    In the second step, differentiating (14) for \(\Delta {\mathbf{Y}}\), and assuming the differentiated equation is set to zero

    $${{\varvec{\Phi}}} \cdot \Delta {\mathbf{Y}} = - {{\varvec{\Omega}}}$$
    (15)

where

$${{\varvec{\Omega}}} = \sum\limits_{i = 1}^{k} {{\mathbf{b}}_{i} \cdot {\mathbf{C}}_{i} }$$
$${{\varvec{\Phi}}} = \sum\limits_{i = 1}^{k} {\left( {{\mathbf{C}}_{i} } \right)^{{\text{T}}} \cdot {\mathbf{a}}_{i} \cdot {\mathbf{C}}_{i} }$$

In addition, to avoid the problem that the \({{\varvec{\Phi}}}\) might be non-singular, a damping factor \(\beta\) is utilized and (15) is re-written as (Dellaert 2012):

$$\left( {{{\varvec{\Phi}}} + \beta \cdot {\mathbf{I}}} \right) \cdot \Delta {\mathbf{Y}} = - {{\varvec{\Omega}}}$$
(16)

An increment is computed, and the \({\tilde{\mathbf{Y}}}\) is updated as:

$${\tilde{\mathbf{Y}}} = {\tilde{\mathbf{Y}}} + \Delta {\mathbf{Y}}^{*}$$
(17)
  1. (3)

    Repeat the first and second steps until the iteration count reaches a predefined threshold or the increment \(\Delta {\mathbf{Y}}^{*}\) reaches a predefined threshold.

Test case and results

A smartphone dataset collected by Google with Xiaomi 8 is used to assess the performance of the proposed method. A high-accuracy commercial position system serves as the reference (Fu et al. 2020). To evaluate the influence of the velocity errors on the proposed method, we divide the trajectory into three parts according to the velocity errors plotted in Fig. 1 (top). Corresponding horizontal position errors from the LS, KF, and optimization methods are presented in the bottom panel.

Fig. 1
figure 1

Position and velocity results from the LS, KF, and Optimization methods

Full size image

With the position errors plotted in Fig. 1 (bottom), we observe that the KF and the optimization method effectively smooth the horizontal position errors and reduce their rooted mean square error (RMSE) values. Further statistical analysis of these horizontal position errors is presented in Table 1. In the first part, compared with the LS method, the mean values of the KF and the optimization methods decrease by 21.2% and 41.2%, the corresponding RMSE errors from KF and the optimization methods decrease by 9.9% and 31.9%, and the corresponding figures are 4.7%, 10.1%, 17.4 and 27.8%, respectively, in the third part. However, the KF and the optimization methods seem ineffective in the second part. The mean values of the horizontal position errors from the LS, KF, and optimization methods are almost identical, and only the RMSE values show minor differences. By observing the velocity errors plotted in Fig. 1 (top), the velocity errors are much larger than that of the first part and third part, which accounts for the phenomenon that the position smoothing methods fail to work. Comparing the velocity errors from the three different parts, we can observe that the position smoothing performance relies on the velocity accuracy. Therefore, the position smoothing schemes are effective in the first and third parts. In addition, the measurement errors covariance matrix, are fixed in the experiment, which is another reason for the smoothing failure in the second part. The measurement noise covariance parameters are obviously different from those in the first and third parts.

Table 1 Statistical analysis of the horizontal position errors
Full size table

Conclusions

We propose an optimization approach to smooth the smartphone GNSS position with its velocity information. The following conclusions can be drawn: the optimization method effectively improves the positioning accuracy. In addition, the following directions are recommended and suggested for future work:

  1. (1)

    As stated, the state process and measurement noise are assumed to have a Gaussian distribution. Also, the covariance parameters stay the same while performing the smoothing. Adaptively tuning the covariance matrixes under different conditions might be helpful to improve the position accuracy.

  2. (2)

    We analyzed the computation load of the KF and optimization methods and found that the computation load of the optimization method is much heavier. It is of great significance to explore how to optimize and reduce the computation load, which is important to extend the optimization method to other applications, especially real-time applications. Also, a larger window size can be utilized, and more past states can be employed in the optimization, and we confirm that a larger window size is adequate to improve and smooth the position results. Here, the window size is 10 for the listed computation load.

  3. (3)

    The last but most important thing is that the open-source optimization method has great potential for many other GNSS signal processing applications, such as carrier tracking, vector tracking, and cooperative navigation. Once the state and measurement models are set up, the optimization method can be implemented. We aimed to present how to understand and implement the optimization method by implementing it in the simple application of smoothing the position with the velocity. Finally, we hope this study and the open-source software inspire exciting applications in GNSS. The MATLAB software is available on the GPS Toolbox website at: https://geodesy.noaa.gov/gps-toolbox/. You can also contact the corresponding author Yuwei Chen (yuwei.chen@nls.fi) to access the source code.