1
$\begingroup$

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$

$\endgroup$
  • 1
    $\begingroup$ @deusy: If your $x^2+y^2+z^2$ has a known value (like in this question), then that simplifies the system. If not, well... $\endgroup$ – Tito Piezas III Mar 7 '15 at 2:38
  • $\begingroup$ There are other ways to solve the GPS equations, including least squares asl.ethz.ch/people/slynen/personal/lectures/aircraft/gps.pdf That actually takes into account the fact that you'll get (slightly perturbed) data from more than 3 satellites. $\endgroup$ – Fizz Mar 7 '15 at 2:49
  • $\begingroup$ Also the actual GPS equation, even for exact solutions, is set up with 4 satellites, not 3, because of the time inside the device is assumed inaccurate so it needs to be resolved too [that's the 4th unknown]. For pointers to several exact methods see uni-stuttgart.de/gi/research/schriftenreihe/quo_vadis/pdf/… $\endgroup$ – Fizz Mar 7 '15 at 3:01
3
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ Thanks, this is what I'm after! But I'm still getting lost at the Jacobian matrix part. How did you get the iterates without taking the inverse of the matrix? $\endgroup$ – deusy Mar 7 '15 at 4:23
  • $\begingroup$ You are actually getting a numeric $3x3$ matrix and then taking the inverse of it. Clear? $\endgroup$ – Amzoti Mar 7 '15 at 6:43
  • $\begingroup$ Hey, I have sorted everything out, but I was wondering how you actually calculated the iterates? I tried doing it by a calculator, but it was going to take a long time and likely result in me making an error. Did you use some sort of software to make it easier, and if so, what is it called? $\endgroup$ – deusy Apr 27 '15 at 4:06
2
$\begingroup$

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.

$\endgroup$
  • $\begingroup$ this looks like a much easier way of doing it, but I'm having trouble getting to the equations for $y$ and $z$ in terms of $x$. In the $y$ equation, what happens to $z$, and vice versa? $\endgroup$ – deusy Mar 7 '15 at 6:17
  • $\begingroup$ After simplifying $(f_2-f_1)$ and $(f_3-f_2)$, you are let with two equations which are linear wrt $x,y,z$. Solve for $y$ and $z$ treating $x$ as a constant. Finally plug these expressions in $f1$, solve for $x$ and back to $y$ and $z$, get their values. $\endgroup$ – Claude Leibovici Mar 7 '15 at 6:21
  • $\begingroup$ Sorry, but I'm still not sure how you got the equations for $y$ and $x$. Whenever I try and solve $(f_2-f_1)$ and $(f_3-f_2)$ for $y$, I get it in terms of $x$ and $z$, rather than just $x$ like you have. Where am I going wrong? $\endgroup$ – deusy Mar 7 '15 at 7:14
  • $\begingroup$ You have to solve simultaneously the two linear equations for $y$ and $z$, treating $x$ as a constant. $\endgroup$ – Claude Leibovici Mar 7 '15 at 7:26
  • $\begingroup$ ahh, got it. Thanks for your help! $\endgroup$ – deusy Mar 7 '15 at 12:54

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Not the answer you're looking for? Browse other questions tagged or ask your own question.