0.3.0
Loading...
Searching...
No Matches
Single Point Positioning using (Weighed) Least Squares Estimation

Table of Contents

Algorithm - Linearized (Weighed) Least Squares Estimation

The solution is obtained by iteratively performing

  1. Cofactor matrix:

    \begin{equation} \label{eq:eq-SPP-LseAlgorithm-Q} \boldsymbol{Q} = \boldsymbol{H}(\boldsymbol{x})^T \boldsymbol{W} \boldsymbol{H}(\boldsymbol{x}) \end{equation}

  2. Least squares solution:

    \begin{equation} \label{eq:eq-SPP-LseAlgorithm-dx} \delta \boldsymbol{x} = (\boldsymbol{Q})^{-1} \boldsymbol{H}(\boldsymbol{x})^T \boldsymbol{W} \delta \boldsymbol{z}(\boldsymbol{x}) \end{equation}

  3. Unknowns:

    \begin{equation} \label{eq:eq-SPP-LseAlgorithm-xhat} \hat{\boldsymbol{x}} = \boldsymbol{x} + \delta \boldsymbol{x} \end{equation}

  4. Residuals:

    \begin{equation} \label{eq:eq-SPP-LseAlgorithm-e} \boldsymbol{e} = \delta\boldsymbol{z}(\boldsymbol{x}) - \boldsymbol{H}(\boldsymbol{x}) \cdot \delta\boldsymbol{x} \end{equation}

  5. Covariance of the unknowns:

    \begin{equation} \label{eq:eq-SPP-LseAlgorithm-Sigma} \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} \label{eq:eq-SppLSE-unknowns-pseudorange} \mathbf{x} = \begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace] \matrix (m)[matrix of math nodes,left delimiter={[},right delimiter={]}, nodes={rectangle,text width=2.5em,align=center},row sep=0.3em, column sep=0.4em] { \mathbf{r}^e \\ dt_R \\ dt_{ISB}^{S_2 \rightarrow S_1} \\ \vdots \\ dt_{ISB}^{S_o \rightarrow S_1} \\ }; \end{tikzpicture} = \begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace] \matrix (m)[matrix of math nodes,left delimiter={[},right delimiter={]}, nodes={rectangle,text width=20em,align=center},row sep=0.3em, column sep=0.4em] { \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{tikzpicture} \end{equation}

Doppler

\begin{equation} \label{eq:eq-SppLSE-unknowns-doppler} \mathbf{x} = \begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace] \matrix (m)[matrix of math nodes,left delimiter={[},right delimiter={]}, nodes={rectangle,text width=2.5em,align=center},row sep=0.3em, column sep=0.4em] { \mathbf{v}^e \\ d\dotup{t}_R \\ d\dotup{t}_{ISB}^{S_2 \rightarrow S_1} \\ \vdots \\ d\dotup{t}_{ISB}^{S_o \rightarrow S_1} \\ }; \end{tikzpicture} = \begin{tikzpicture}[baseline=-\the\dimexpr\fontdimen22\textfont2\relax,decoration=brace] \matrix (m)[matrix of math nodes,left delimiter={[},right delimiter={]}, nodes={rectangle,text width=20em,align=center},row sep=0.3em, column sep=0.4em] { \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{tikzpicture} \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} \label{eq:eq-SppLSE-measurementModel-designMatrix-pseudorange} \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} \label{eq:eq-SppLSE-measurementModel-designMatrix-pseudorangeRate} \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