Notebook Gauss-Newton iteration for GNSS Trilateration

Notebook Gauss-Newton iteration for GNSS Trilateration#

This notebook implements the Gauss-Newton algorithm to solve the trilateration problem in GNSS (Global Navigation Satellite System). The goal is to estimate the position of a receiver based on the distances to multiple satellites.

We have the distances to 5 satellites and their known positions in 3D space. The distances are given in the array y, and the satellite positions are provided in the satellite_positions array. The true position is given by the target array, normally unknown in real scenarios, but included here for validation and demonstration purposes.

We know that the distance from the receiver to each satellite can be expressed as:

\(d_i = \sqrt{(x - x_i)^2 + (y - y_i)^2 + (z - z_i)^2}\)

where \((x_i, y_i, z_i)\) are the coordinates of the i-th satellite and \((x, y, z)\) are the coordinates of the receiver we want to estimate.

Our functional model is:

\( \mathbb{E} ( \begin{bmatrix} Y_1\\ Y_2\\ \vdots\\ Y_5 \end{bmatrix} ) = q(\mathrm{x}) = \begin{bmatrix} q_{1}(\mathrm{x})\\ q_{2}(\mathrm{x})\\ \vdots\\ q_{5}(\mathrm{x}) \end{bmatrix} = \begin{bmatrix} d_1\\ d_2\\ \vdots\\ d_5 \end{bmatrix} \)

We know that the precision of the observations is \(\sigma_y = \sqrt{10}\) km. (This is a very high uncertainty for GNSS, but we use it for demonstration purposes. Normally GNSS systems have uncertainties in the order of meters or even centimeters with advanced techniques.)

We have implemented a function to calculate the Euclidean distance between two 3D points, which will be used in our Gauss-Newton iteration.

import numpy as np
import matplotlib.pyplot as plt
import os

target = np.array([ 2329.98740018, 18782.88246936,  8961.86067478]) # Target position in km

y = np.array([20677.48801802, 24895.98298007,
                16465.70462416, 26507.04323936, 22212.37447646]) # Distances to satellites in km

satellite_positions = np.array([[15600,  7540, 20140],
                                [18760,  2750, 18610],
                                [17610, 14630, 13480],
                                [19170,   610, 18390],
                                [17800,  6400, 19000]]) # Satellite positions in km 

def calculate_distance(point1, point2):
    '''Calculate the Euclidean distance between two 3D points.'''
    return np.sqrt((point1[0] - point2[0])**2 + 
                   (point1[1] - point2[1])**2 + 
                   (point1[2] - point2[2])**2)

Implementation of the Gauss-Newton algorithm#

We have already initialized the necessary matrices and vectors. The initial guess for the receiver’s position is set to the origin (0, 0, 0). In order to run the Gauss-Newton algorithm, we need to iteratively update our estimate of the receiver’s position based on the residuals between the observed distances and the distances calculated from our current estimate.

Before starting the iterations, we need to compute the Jacobian matrix, which contains the partial derivatives of the distance functions with respect to the receiver’s coordinates. This matrix is crucial for the Gauss-Newton update step. It is best to do this on paper first, and then implement it in code.

var_Y = 10  # variance of measurement noise
Sigma_y = np.eye(len(satellite_positions)) * var_Y  # noise covariance matrix
inv_Sigma_y = np.linalg.inv(Sigma_y)  # inverse of noise covariance matrix

x_init = np.zeros(3)  # initial guess for target position

N = 10  # number of iterations
dx_i = np.zeros((N, 3)) # change in position at each iteration
x_i = np.zeros((N+1, 3)) # estimated position at each iteration
x_i[0] = x_init # set initial guess

for i in range(N):
    y_comp = ? # computed distances based on current estimate
    dy = ? # observed minus computed observations
    J = ? # Jacobian matrix (derivatives of distances w.r.t. position)

    dx_i[i] = ? # position update using Gauss-Newton formula
    x_i[i+1] = ? # update estimated position for next iteration
    
x_hat = x_i[-1] # final estimated position
Sigma_x_hat = ? # covariance matrix of estimated position
sigma_x_hat = np.sqrt(np.diag(Sigma_x_hat)) # standard deviations of estimated position
print("Estimated position:", x_hat)
print("Uncertainty (1 std dev):", sigma_x_hat)
print("True position:", target)

assert np.all(np.abs(x_hat - target) < 3 * sigma_x_hat), "Estimated position is not within 3 standard deviations of the true position"

Solution

y_comp = np.array([calculate_distance(satellite, x_i[i]) for satellite in satellite_positions])
dy = y - y_comp
J = np.array([(x_i[i] - satellite) / calculate_distance(satellite, x_i[i]) for satellite in satellite_positions])
dx_i[i] = np.linalg.inv(J.T @ inv_Sigma_y @ J) @ (J.T @ inv_Sigma_y @ dy)
x_i[i+1] = x_i[i] + dx_i[i]
Sigma_x_hat = np.linalg.inv(J.T @ inv_Sigma_y @ J)

plotting convergence#

In the next cell we plot the convergence of the estimated position to the true position over the iterations. The plot shows how the estimated coordinates (x, y, z) approach the true coordinates as the number of iterations increases. This visualization helps to understand the effectiveness of the Gauss-Newton algorithm in refining the position estimate with each iteration.

fig, ax = plt.subplots(3, 1, figsize=(8, 5))
ax[0].plot(x_i[:, 0], label='Estimated X')
ax[0].axhline(target[0], color='r', linestyle='--', label='True X')
ax[0].set_title('X Position Estimation')    
ax[0].set_xlabel('Iteration')
ax[0].set_ylabel('X (km)')
ax[0].legend()
ax[0].grid()
ax[1].plot(x_i[:, 1], label='Estimated Y')
ax[1].axhline(target[1], color='r', linestyle='--', label='True Y')
ax[1].set_title('Y Position Estimation')    
ax[1].set_xlabel('Iteration')
ax[1].set_ylabel('Y (km)')
ax[1].legend(loc='right')
ax[1].grid()    
ax[2].plot(x_i[:, 2], label='Estimated Z')
ax[2].axhline(target[2], color='r', linestyle='--', label='True Z')
ax[2].set_title('Z Position Estimation')
ax[2].set_xlabel('Iteration')
ax[2].set_ylabel('Z (km)')  
ax[2].legend()
ax[2].grid()
plt.tight_layout()
Wrap-up exercises

After how many iterations does the algorithm converge?

Solution

The algorithm converges after a few iterations, in this case 3-4 iterations are sufficient to reach a stable solution.

What could improve the precision of the estimated position?

Solution

The precision could be improved by reducing the measurement noise (i.e., having a smaller \(\sigma_y\)), increasing the number of satellites (more observations), or improving the geometry of the satellite constellation (better spatial distribution of satellites).

Contents