Determine the position of a point in 3D space give

Question

I am trying to determine the (x,y,z) coordinates of a point p. What I have are the distances to 4 different points m1, m2, m3, m4 with known coordinates.

In detail: what I have is the coordinates of 4 points (m1,m2,m3,m4) and they are not in the same plane:

m1: (x1,y1,z1),

m2: (x2,y2,z2),

m3: (x3,y3,z3),

m4: (x4,y4,z4)

and the Euclidean distances form m1->p, m2->p, m3->p and m4->p which are

D1 = sqrt( (x-x1)^2 + (y-y1)^2 + (z-z1)^2);

D2 = sqrt( (x-x2)^2 + (y-y2)^2 + (z-z2)^2);

D3 = sqrt( (x-x3)^2 + (y-y3)^2 + (z-z3)^2);

D4 = sqrt( (x-x4)^2 + (y-y4)^2 + (z-z4)^2);

I am looking for (x,y,z). I tried to solve this non-linear system of 4 equations and 3 unknowns with matlab fsolve by taking the euclidean distances but didn't manage.

There are two questions:

1. How can I find the unknown coordinates of point p: (x,y,z)
2. What is the minimum number of points m with known coordinates and distances to p that I need in order to find (x,y,z)?

EDIT:

Here is a piece of code that gives no solutions:

Lets say that the points I have are:

m1 = [ 370; 1810;  863];

m2 = [1586;  185; 1580];

m3 = [1284; 1948;  348];

m4 = [1732; 1674; 1974];

x = cat(2,m1,m2,m3,m4)';

And the distance from each point to p are

d = [1387.5; 1532.5; 1104.7; 0855.6]

From what I understood if I want to run fsolve I have to use the following: 1. Create a function 2. Call fsolve

function F = calculateED(p)

m1 = [ 370; 1810;  863];

m2 = [1586;  185; 1580];

m3 = [1284; 1948;  348];

m4 = [1732; 1674; 1974];

x = cat(2,m1,m2,m3,m4)';

d = [1387.5; 1532.5; 1104.7; 0855.6]

F = [d(1,1)^2 - (p(1)-x(1,1))^2 - (p(2)-x(1,2))^2 - (p(3)-x(1,3))^2;
 d(2,1)^2 - (p(1)-x(2,1))^2 - (p(2)-x(2,2))^2 - (p(3)-x(2,3))^2;
 d(3,1)^2 - (p(1)-x(3,1))^2 - (p(2)-x(3,2))^2 - (p(3)-x(3,3))^2;
 d(4,1)^2 - (p(1)-x(4,1))^2 - (p(2)-x(4,2))^2 - (p(3)-x(4,3))^2;];

and then call fsolve:

p0 = [1500,1500,1189];  % initial guess
options = optimset('Algorithm',{'levenberg-marquardt',.001},'Display','iter','TolX',1e-1);                     
[p,Fval,exitflag] = fsolve(@calculateED,p0,options);

I am running Matlab 2011b.

Am I missing something?

How would the least squares solution be?

One note here is that m1, m2, m3, m4 and d values may not be given accurately but for an analytical solution that shouldn't be a problem.

Answer 1

Take first three equation and solve i for 3 equation and 3 variables in MATLAB. After solving the equation you will get two pairs of values or we can say two set of coordinates of p. keep each set in the 4th equation and you can find out the set which satisfies the equation is the answer

Answer 2

mathematica readily numericall solves the three point problem:

p = Table[ RandomReal[{-1, 1}, {3}], {3}]
r = RandomReal[{1, 2}, {3}]
Reduce[Simplify[ Table[Norm[{x, y, z} - p[[i]]] == r[[i]] , {i, 3}], 
      Assumptions -> {Element[x | y | z, Reals]}], {x, y, z}, Reals]

This will typically return false as random spheres will typically not have triple intersection points.

When you have a solution you'll typically have a pair like this..

      (*   (x == -0.218969 && y == -0.760452 &&  z == -0.136958) ||
           (x == 0.725312 && y == 0.466006 &&   z == -0.290347)  *)

This somewhat surprisingly has a failrly elegent analytic solution. Its a bit involved so I'll wait to see if someone has it handy and if not and there is interest I'll try to remember the steps..

Edit, approximate solution following Dmitys least squares suggestion:

p = {{370, 1810, 863}, {1586, 185, 1580}, {1284, 1948, 348}, {1732, 
1674, 1974}};
r = {1387.5, 1532.5, 1104.7, 0855.6};
solution = {x, y, z} /. 
              Last@FindMinimum[ 
                     Sum[(Norm[{x, y, z} - p[[i]]] - r[[i]] )^2, {i, 1, 4}] , {x, y, z}]
Table[ Norm[ solution - p[[i]]], {i, 4}]

As you see you are pretty far from exact..

(* solution point {1761.3, 1624.18, 1178.65} *)
(* solution radii: {1438.71, 1504.34, 1011.26, 797.446} *)

Answer 3

I'll answer the second question. Let's name the unknown point X. If you have only known point A and know the distance form X to A then X can be on a sphere with the center in A.

If you have two points A,B then X is on a circle given by the intersection of the spheres with centers in A and B (if they intersect that is).

A third point will add another sphere and the final intersection between the three spheres will give two points.

The fourth point will finaly decide which of those two points you're looking for.

This is how GPS actually works. You have to have at least three satellites. Then the GPS will guess which of the two points is the correct one, since the other one is in space, but it won't be able to tell you the altitude. Technically it should, but there are also errors, so the more satellites you "see" the less the error.

Have found this question which might be a starting point.