GPS, Conditioning, and Nonlinear Least Squares

PART I

If we assume that four satellites is the minimum number of satellites needed to accurately calculate the position of the receiver, then we have a system of 4 unknowns and 4 equations. However, the system of equations is nonlinear, so the standard method of Gaussian Elimination will not solve the problem. Instead, we will apply Newton's Multivariate Method to solve the system of nonlinear equations.

The algorithm for Multivariate Newton's Method is reasonably straightforward. \[ x_0 = initial guess \] \[ DF(x_k) \times s = -F(x_k) \] \[ x_{k+1} = x_k + s \] Rinse, repeat, for a suitable number of iterations or required accuracy.

A few notes on this method. First, \( F(x_k) \) is the solution to our equations given the satellite positions \( A, B, C \), a known clock error for the receiver, \(d\), and our current approximation of the receiver position \(x_k\). Second, \(DF(x_k)\) is the Jacobian of our system of equations, where

\[ DF(x_k) = \begin{bmatrix} \frac{\partial S_1}{\partial x} & \frac{\partial S_1}{\partial y} & \frac{\partial S_1}{\partial z} \frac{\partial S_1}{\partial d} \\ \frac{\partial S_2}{\partial x} & \frac{\partial S_2}{\partial y} & \frac{\partial S_2}{\partial z} \frac{\partial S_2}{\partial d} \\ \frac{\partial S_3}{\partial x} & \frac{\partial S_3}{\partial y} & \frac{\partial S_3}{\partial z} \frac{\partial S_3}{\partial d} \\ \frac{\partial S_4}{\partial x} & \frac{\partial S_4}{\partial y} & \frac{\partial S_4}{\partial z} \frac{\partial S_4}{\partial d} \end{bmatrix} \]

and \(S_i\) is our spherical equation, or psedo-range equation, for a given satellite.

For this part, we will use the following values: \[ A_1 = 15600, B_1 = 7540, C_1 = 20140 \] \[ A_2 = 18760, B_2 = 2750, C_2 = 18610 \] \[ A_3 = 17610, B_3 = 14630, C_3 = 13480 \] \[ A_4 = 19710, B_4 = 610, C_4 = 18390 \] \[ t_1 = 0.07074, t_2 = 0.07220, t_3 = 0.07690, t_4 = 0.07242\]

where distance is in kilometers, and time in seconds.

As a test of the implementation, the following are the results: \[ (x,y,z) = (-41.77271, -16.78919, 6370.0596) \] \[ d = -0.003201566 \]

Source code for part 1. If you're wondering how the figure above was created, look here.