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$
Best Answer
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:
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.