Solving a non-linear, multivariable system of equations

Question

I'm researching the mathematics behind GPS, and at the moment I'm trying to get my head around how to solve the following system of equations:

$\sqrt{(x-x_1)^2+(y-y_1)^2+(z-z_1)^2}=r_1$

$\sqrt{(x-x_2)^2+(y-y_2)^2+(z-z_2)^2}=r_2$

$\sqrt{(x-x_3)^2+(y-y_3)^2+(z-z_3)^2}=r_3$

$(x,y,z)$ is the coordinates of a GPS receiver's location, with $(0,0,0)$ being the center of the earth.

$(x_1,y_1,z_1)$, $(x_2,y_2,z_2)$ and $(x_3,y_3,z_3)$ are the coordinates of each satellite. Hence these are just three distance formulas, where $r_1$, $r_2$ and $r_3$ are the distances from each satellite to the point $(x,y,z)$ on the Earth's surface. By assigning values for $r_1$, $r_2$, $r_3$ and coordinates of each satellite, I want to know how I can solve for the coordinates $(x,y,z)$.

I understand the Newton-Raphson Method for one variable, but I'm getting lost when it comes to using the Jacobian matrix for more than one. All I've been able to do it take the partial derivative for each variable, and then I'm not sure where to go.

For the purpose of this, use the following values. I've tested them on WolframAlpha, so I know that they work:

$r_1=20000$

$r_2=19000$

$r_3=19500$

$x_1=20000$

$y_1=19400$

$z_1=19740$

$x_2=18700$

$y_2=1800$

$z_2=18500$

$x_3=18900$

$y_3=17980$

$z_3=20000$

Answer 1

The regular Newton-Raphson method is initialized with a starting point $x_0$ and then you iterate $x_{n+1}=x_n-\dfrac{f(x_n)}{f'(x_n)}$.

In higher dimensions, there is an exact analog. We define:

$$F\begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix}f_1(x,y,z) \\ f_2(x,y,z) \\ f_3(x,y,z)\end{bmatrix} = \begin{bmatrix}\sqrt{(x-20000)^2+(y-19400)^2+(z-19740)^2}-20000 = 0 \\ \sqrt{(x-18700)^2+(y-1800)^2+(z-18500)^2}-19000 = 0 \\ \sqrt{(x-18900)^2+(y-17980)^2+(z-20000)^2}-19500= 0 \end{bmatrix}$$

The derivative of this system is the $3x3$ Jacobian given by:

$J(x,y,z) = \begin{bmatrix} \dfrac{\partial f_1}{\partial x} & \dfrac{\partial f_1}{\partial y} & \dfrac{\partial f_1}{\partial z}\\ \dfrac{\partial f_2}{\partial x} & \dfrac{\partial f_2}{\partial y} & \dfrac{\partial f_2}{\partial z} \\ \dfrac{\partial f_3}{\partial x} & \dfrac{\partial f_3}{\partial y} & \dfrac{\partial f_3}{\partial z}\end{bmatrix} =\\ \begin{bmatrix} \frac{x-20000}{\sqrt{(x-20000)^2+(y-19400)^2+(z-19740)^2}} & \frac{y-19400}{\sqrt{(x-20000)^2+(y-19400)^2+(z-19740)^2}} & \frac{z-19740}{\sqrt{(x-20000)^2+(y-19400)^2+(z-19740)^2}} \\ \frac{x-18700}{\sqrt{(x-18700)^2+(y-1800)^2+(z-18500)^2}} & \frac{y-1800}{\sqrt{(x-18700)^2+(y-1800)^2+(z-18500)^2}} & \frac{z-18500}{\sqrt{(x-18700)^2+(y-1800)^2+(z-18500)^2}} \\ \frac{x-18900}{\sqrt{(x-18900)^2+(y-17980)^2+(z-20000)^2}} & \frac{y-17980}{\sqrt{(x-18900)^2+(y-17980)^2+(z-20000)^2}} & \frac{z-20000}{\sqrt{(x-18900)^2+(y-17980)^2+(z-20000)^2}} \\ \end{bmatrix}$

The function $G$ is defined as:

$$G(x) = x - J(x)^{-1}F(x)$$

and the functional Newton-Raphson method for nonlinear systems is given by the iteration procedure that evolves from selecting an initial $x^{(0)}$ and generating for $k \ge 1$,

$$x^{(k)} = G(x^{(k-1)}) = x^{(k-1)} - J(x^{(k-1)})^{-1}F(x^{(k-1)}).$$

We can write this as:

$$\begin{bmatrix}x^{(k)}\\y^{(k)}\\z^{(k)}\end{bmatrix} = \begin{bmatrix}x^{(k-1)}\\y^{(k-1)}\\z^{(k-1)}\end{bmatrix} + \begin{bmatrix}y_1^{(k-1)}\\y_2^{(k-1)}\\y_3^{(k-1)}\end{bmatrix}$$

where:

$$\begin{bmatrix}y_1^{(k-1)}\\y_2^{(k-1)}\\y_3^{(k-1)}\end{bmatrix}= -\left(J\left(x^{(k-1)},y^{(k-1)},z^{(k-1)}\right)\right)^{-1}F\left(x^{(k-1)},y^{(k-1)},z^{(k-1)}\right)$$

Here is a starting vector:

$$x^{(0)} = \begin{bmatrix}x^{(0)}\\y^{(0)}\\z^{(0)}\end{bmatrix} = \begin{bmatrix}1\\1\\1\end{bmatrix}$$

The iterates will be:

• $x_0 = (1,1,1)$
• $x_1 = (17289.9,15734.3,-8508.52)$
• $x_2 = (13614.9,9566.68,1369.85)$
• $x_3 = (16370.9,11019.8,1760.91)$
• $x_4 = (16274.,10923.6,2010.87)$
• $x_5 = (16276.2,10924.5,2011.53)$
• $x_6 = (16276.2,10924.5,2011.53)$

You should end up with a solution that looks something like:

$$\begin{bmatrix}x\\y\\z\end{bmatrix} = \begin{bmatrix} 16276.2 \\ 10924.5 \\ 2011.53 \end{bmatrix}$$

Of course, for a different starting vector you could get a different solution and perhaps no solution at all.

Answer 2

I think that you can make the problem simpler the solution of which not requiring Newton-Raphson or any root finder method; the solution is explicit and direct (it does not require any iteration).

Start squaring the expressions to define $$f_i={(x-x_i)^2+(y-y_i)^2+(z-z_i)^2}-r_i^2=0$$ Now, develop $(f_2-f_1)$ and $(f_3-f_2)$ for example. As a result, you have two linear equations since terms $x^2,y^2,z^2$ disappear. You can solve these two equations for $y$ and $z$ and the result will write $y=\alpha_1+\alpha_2 x$,$z=\beta_1+\beta_2 x$. Report these expressions in $f_1$ which is a quadratic equation in $x$.

Using the numbers you gave, I found $$y=\frac{121160315}{7921}-\frac{4255 }{15842}x$$ $$z=-\frac{340394455}{7921}+\frac{43785 }{15842}x$$ and the solution of $f_1=0$ is just $x=16276.24775$ from which $y=10924.45372$, $z=2011.526168$ (just as Amzoti found).

You must take care that there is a second root for the quadratic $x=27858.39152$ from which $y=7813.607758$, $z=34022.89878$.

Depending on the starting point chosen for Newton-Raphson, you would arrive to one of these two solutions.