Single Point Positioning using (Weighed) Least Squares Estimation

Algorithm - Linearized (Weighed) Least Squares Estimation

The solution is obtained by iteratively performing

1. Cofactor matrix:

   \begin{equation} \boldsymbol{Q} = \boldsymbol{H}(\boldsymbol{x})^T \boldsymbol{W} \boldsymbol{H}(\boldsymbol{x}) \end{equation}

2. Least squares solution:

   \begin{equation} \delta \boldsymbol{x} = (\boldsymbol{Q})^{-1} \boldsymbol{H}(\boldsymbol{x})^T \boldsymbol{W} \delta \boldsymbol{z}(\boldsymbol{x}) \end{equation}

3. Unknowns:

   \begin{equation} \hat{\boldsymbol{x}} = \boldsymbol{x} + \delta \boldsymbol{x} \end{equation}

4. Residuals:

   \begin{equation} \boldsymbol{e} = \delta\boldsymbol{z}(\boldsymbol{x}) - \boldsymbol{H}(\boldsymbol{x}) \cdot \delta\boldsymbol{x} \end{equation}

5. Covariance of the unknowns:

   \begin{equation} \boldsymbol{\Sigma}_{x} = \hat{\sigma}_{apost}^2 \cdot \boldsymbol{Q}^{-1} \text{ where } \hat{\sigma}_{apost}^2 = \frac{\boldsymbol{e}^T\boldsymbol{W}\boldsymbol{e}}{m-n} \end{equation}

6. Stop if \( \mid \delta \boldsymbol{x} \mid < 10^{-4} \) or the maximum number of iterations of 10 is reached.
7. Set \( \boldsymbol{x} = \hat{\boldsymbol{x}} \)

Unknowns

Pseudorange and Doppler observations are processed separately.

Pseudorange

\begin{equation} \mathbf{x} = \begin{bmatrix} \mathbf{r}^e \\ dt_R \\ dt_{ISB}^{S_2 \rightarrow S_1} \\ \vdots \\ dt_{ISB}^{S_o \rightarrow S_1} \end{bmatrix} = \begin{bmatrix} \text{Position [m]} \\ \text{Receiver clock error [m]} \\ \text{Inter-system clock error (System 2 to 1) [m]} \\ \vdots \\ \text{Inter-system clock error (System o to 1) [m]} \\ \end{bmatrix} \end{equation}

Doppler

\begin{equation} \mathbf{x} = \begin{bmatrix} \mathbf{v}^e \\ d\dot{t}_R \\ d\dot{t}_{ISB}^{S_2 \rightarrow S_1} \\ \vdots \\ d\dot{t}_{ISB}^{S_o \rightarrow S_1} \\ \end{bmatrix} = \begin{bmatrix} \text{Velocity [m/s]} \\ \text{Receiver clock drift [m/s]} \\ \text{Inter-system clock drift (System 2 to 1) [m/s]} \\ \vdots \\ \text{Inter-system clock drift (System o to 1) [m/s]} \\ \end{bmatrix} \end{equation}

Measurement Model

Measurement residuals

See Measurement innovation

Design matrix

See Design matrix / Measurement sensitivity matrix

In detail for Least squares estimation

Pseudorange

\begin{equation} \begin{aligned} \mathbf{H}_k &= \left[\begin{array}{ccccc} \frac{-x^s+x_r}{\rho_s^s} & \frac{-y^s+y_r}{\rho_s^s} & \frac{-z^s+z_r}{\rho_r^s} & 1 & 0 \\ \frac{-x^s+x_r}{\rho_r^s} & \frac{-y^s+y_r}{\rho_r^s} & \frac{-z^s+z_r}{\rho_r^s} & 1 & 1 \\ \ldots & \ldots & \ldots & \ldots & \ldots \end{array}\right] \\ &= \left[\begin{array}{ccccc} -u_{rs,x} & -u_{rs,y} & -u_{rs,z} & 1 & 0 \\ -u_{rs,x} & -u_{rs,y} & -u_{rs,z} & 1 & 1 \\ \ldots & \ldots & \ldots & \ldots & \ldots \end{array}\right] \end{aligned} \end{equation}

Doppler

\begin{equation} \begin{aligned} \mathbf{H}_k &= \left[\begin{array}{ccccc} -\frac{x^s-x_r}{\rho_r^s} & -\frac{y^s-y_r}{\rho_r^s} & -\frac{z^s-z_r}{\rho_r^s} & 1 & 0 \\ -\frac{x^s-x_r}{\rho_r^s} & -\frac{y^s-y_r}{\rho_r^s} & -\frac{z^s-z_r}{\rho_r^s} & 1 & 1 \\ \ldots & \ldots & \ldots & \ldots & \ldots \end{array}\right] \\ &= \left[\begin{array}{ccccc} -u_{rs,x} & -u_{rs,y} & -u_{rs,z} & 1 & 0 \\ -u_{rs,x} & -u_{rs,y} & -u_{rs,z} & 1 & 1 \\ \ldots & \ldots & \ldots & \ldots & \ldots \end{array}\right] \end{aligned} \end{equation}

Weight matrix

See NAV::GnssMeasurementErrorModel