23

I want to find the coordinate of an unknown node which lie somewhere in the space which has its reference distance away from 3 or more nodes which all of them have known coordinate.

This problem is exactly like Trilateration as described here Trilateration.

However, I don't understand the part about "Preliminary and final computations" (refer to the wikipedia site). I don't get where I could find P1, P2 and P3 just so I can put to those equation?

Thanks

3 Answers 3

29

Trilateration is the process of finding the center of the area of intersection of three spheres. The center point and radius of each of the three spheres must be known.

Let's consider your three example centerpoints P1 [-1,1], P2 [1,1], and P3 [-1,-1]. The first requirement is that P1' be at the origin, so let us adjust the points accordingly by adding an offset vector V [1,-1] to all three:

P1' = P1 + V = [0, 0]
P2' = P2 + V = [2, 0]
P3' = P3 + V = [0,-2]

Note: Adjusted points are denoted by the ' (prime) annotation.

P2' must also lie on the x-axis. In this case it already does, so no adjustment is necessary.

We will assume the radius of each sphere to be 2.

Now we have 3 equations (given) and 3 unknowns (X, Y, Z of center-of-intersection point).

Solve for P4'x:

x = (r1^2 - r2^2 + d^2) / 2d  //(d,0) are coords of P2'
x = (2^2 - 2^2 + 2^2) / 2*2
x = 1

Solve for P4'y:

y = (r1^2 - r3^2 + i^2 + j^2) / 2j - (i/j)x //(i,j) are coords of P3'
y = (2^2 - 2^2 + 0 + -2^2) / 2*-2 - 0
y = -1

Ignore z for 2D problems.

P4' = [1,-1]

Now we translate back to original coordinate space by subtracting the offset vector V:

P4 = P4' - V = [0,0]

The solution point, P4, lies at the origin as expected.

The second half of the article is describing a method of representing a set of points where P1 is not at the origin or P2 is not on the x-axis such that they fit those constraints. I prefer to think of it instead as a translation, but both methods will result in the same solution.

Edit: Rotating P2' to the x-axis

If P2' does not lie on the x-axis after translating P1 to the origin, we must perform a rotation on the view.

First, let's create some new vectors to use as an example: P1 = [2,3] P2 = [3,4] P3 = [5,2]

Remember, we must first translate P1 to the origin. As always, the offset vector, V, is -P1. In this case, V = [-2,-3]

P1' = P1 + V = [2,3] + [-2,-3] = [0, 0]
P2' = P2 + V = [3,4] + [-2,-3] = [1, 1]
P3' = P3 + V = [5,2] + [-2,-3] = [3,-1]

To determine the angle of rotation, we must find the angle between P2' and [1,0] (the x-axis).

We can use the dot product equality:

A dot B = ||A|| ||B|| cos(theta)

When B is [1,0], this can be simplified: A dot B is always just the X component of A, and ||B|| (the magnitude of B) is always a multiplication by 1, and can therefore be ignored.

We now have Ax = ||A|| cos(theta), which we can rearrange to our final equation:

theta = acos(Ax / ||A||)

or in our case:

theta = acos(P2'x / ||P2'||)

We calculate the magnitude of P2' using ||A|| = sqrt(Ax + Ay + Az)

||P2'|| = sqrt(1 + 1 + 0) = sqrt(2)

Plugging that in we can solve for theta

theta = acos(1 / sqrt(2)) = 45 degrees

Now let's use the rotation matrix to rotate the scene by -45 degrees. Since P2'y is positive, and the rotation matrix rotates counter-clockwise, we'll use a negative rotation to align P2 to the x-axis (if P2'y is negative, don't negate theta).

R(theta) = [cos(theta) -sin(theta)]
           [sin(theta)  cos(theta)]

  R(-45) = [cos(-45) -sin(-45)]
           [sin(-45)  cos(-45)]

We'll use double prime notation, '', to denote vectors which have been both translated and rotated.

P1'' = [0,0] (no need to calculate this one)

P2'' = [1 cos(-45) - 1 sin(-45)] = [sqrt(2)] = [1.414]
       [1 sin(-45) + 1 cos(-45)] = [0]       = [0]

P3'' = [3 cos(-45) - (-1) sin(-45)] = [sqrt(2)]    = [ 1.414]
       [3 sin(-45) + (-1) cos(-45)] = [-2*sqrt(2)] = [-2.828]

Now you can use P1'', P2'', and P3'' to solve for P4''. Apply the reverse rotation to P4'' to get P4', then the reverse translation to get P4, your center point.

To undo the rotation, multiply P4'' by R(-theta), in this case R(45). To undo the translation, subtract the offset vector V, which is the same as adding P1 (assuming you used -P1 as your V originally).

23
  • I don't know how I get those Point P1, P2 and P3. I mean for each of the reference known node, I have its x,y,z coordinate.
    – CB4
    Commented Apr 23, 2013 at 18:48
  • P1 is the point (which consists of a pair of X,Y coords) at the center of the first reference node. Likewise for P2 and P3. If you have the reference coordinates, you have P1, P2, and P3. They are one and the same. Commented Apr 23, 2013 at 22:20
  • I set up a test coordinates just to verify the math and it did not give me the correct result that is why i am confuse. This is my set up: p1 = (-1,1) p2 = (1,1) p3 = (-1,-1) From the equation I found: ex = (1,0) i = 0 ey = (0,-1) d = 2 and so to find x and y i use the other equations and i get x = 1 y = 1 Which is wrong. I expect to see x = 0 and y = 0. Note: In this case, I use r1 = sqrt(2) = r2 = r3 Can you spot the problem in the calculation?
    – CB4
    Commented Apr 24, 2013 at 16:49
  • I rewrote my answer to hopefully be much more helpful. Let me know if it still doesn't make sense. Commented Apr 27, 2013 at 16:10
  • 1
    @Dan i read the post again and found out that the x,y point is the "center" point. I had not known that we get the center. it is ok for now. Thanks
    – merveotesi
    Commented Aug 19, 2014 at 13:52
0

This is the algorithm I use in a 3D printer firmware. It avoids rotating the coordinate system, but it may not be the best.

There are 2 solutions to the trilateration problem. To get the second one, replace "- sqrtf" by "+ sqrtf" in the quadratic equation solution.

Obviously you can use doubles instead of floats if you have enough processor power and memory.

// Primary parameters
float anchorA[3], anchorB[3], anchorC[3];               // XYZ coordinates of the anchors

// Derived parameters
float Da2, Db2, Dc2;
float Xab, Xbc, Xca;
float Yab, Ybc, Yca;
float Zab, Zbc, Zca;
float P, Q, R, P2, U, A;

...

inline float fsquare(float f) { return f * f; }

...

// Precompute the derived parameters - they don't change unless the anchor positions change.
Da2 = fsquare(anchorA[0]) + fsquare(anchorA[1]) + fsquare(anchorA[2]);
Db2 = fsquare(anchorB[0]) + fsquare(anchorB[1]) + fsquare(anchorB[2]);
Dc2 = fsquare(anchorC[0]) + fsquare(anchorC[1]) + fsquare(anchorC[2]);
Xab = anchorA[0] - anchorB[0];
Xbc = anchorB[0] - anchorC[0];
Xca = anchorC[0] - anchorA[0];
Yab = anchorA[1] - anchorB[1];
Ybc = anchorB[1] - anchorC[1];
Yca = anchorC[1] - anchorA[1];
Zab = anchorB[2] - anchorC[2];
Zbc = anchorB[2] - anchorC[2];
Zca = anchorC[2] - anchorA[2];
P = (  anchorB[0] * Yca
     - anchorA[0] * anchorC[1]
     + anchorA[1] * anchorC[0]
     - anchorB[1] * Xca
    ) * 2;
P2 = fsquare(P);
Q = (  anchorB[1] * Zca
     - anchorA[1] * anchorC[2]
     + anchorA[2] * anchorC[1]
     - anchorB[2] * Yca
    ) * 2;  

R = - (  anchorB[0] * Zca
       + anchorA[0] * anchorC[2]
       + anchorA[2] * anchorC[0]
       - anchorB[2] * Xca
      ) * 2;
U = (anchorA[2] * P2) + (anchorA[0] * Q * P) + (anchorA[1] * R * P);
A = (P2 + fsquare(Q) + fsquare(R)) * 2;

...

// Calculate Cartesian coordinates given the distances to the anchors (La, Lb and Lc)
// First calculate PQRST such that x = (Qz + S)/P, y = (Rz + T)/P.
// P, Q and R depend only on the anchor positions, so they are pre-computed
const float S = - Yab * (fsquare(Lc) - Dc2)
                - Yca * (fsquare(Lb) - Db2)
                - Ybc * (fsquare(La) - Da2);
const float T = - Xab * (fsquare(Lc) - Dc2)
                + Xca * (fsquare(Lb) - Db2)
                + Xbc * (fsquare(La) - Da2);

// Calculate quadratic equation coefficients
const float halfB = (S * Q) - (R * T) - U;
const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2;

// Solve the quadratic equation for z
float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A;

// Substitute back for X and Y
float x = (Q * z + S)/P;
float y = (R * z + T)/P;
0

Here are the Wikipedia calculations, presented in an OpenSCAD script, which I think helps to understand the problem in a visual wayand provides an easy way to check that the results are correct. Example output from the script

// Trilateration example
// from Wikipedia
// 
// pA, pB and pC are the centres of the spheres
// If necessary the spheres must be translated
// and rotated so that:
// -- all z values are 0
// -- pA is at the origin
pA = [0,0,0];
// -- pB is on the x axis
pB = [10,0,0];
pC = [9,7,0];

// rA , rB and rC are the radii of the spheres
rA = 9;
rB = 5;
rC = 7;


if ( pA != [0,0,0]){
   echo ("ERROR: pA must be at the origin");
   assert(false);
}

if ( (pB[2] !=0 ) || pC[2] !=0){
   echo("ERROR: all sphere centers must be in z = 0 plane");
   assert(false);
}

if (pB[1] != 0){
   echo("pB centre must be on the x axis");
   assert(false);
}

// show the spheres
module spheres(){
   translate (pA){
      sphere(r= rA, $fn = rA * 10);
   }

   translate(pB){
      sphere(r = rB, $fn = rB * 10);
   }

   translate(pC){
      sphere (r = rC, $fn = rC * 10);
   }
}

function unit_vector( v) = v / norm(v);

ex = unit_vector(pB - pA) ;
echo(ex = ex);

i = ex * ( pC - pA);
echo (i = i);

ey = unit_vector(pC - pA - i * ex);
echo (ey = ey);

d = norm(pB - pA);
echo (d = d);

j =  ey * ( pC - pA);
echo (j = j);

x = (pow(rA,2) - pow(rB,2) + pow(d,2)) / (2 * d);
echo( x = x);

// size of the cube to subtract to show 
// the intersection of the spheres
cube_size = [10,10,10];

if ( ((d - rA) >= rB) || ( rB >= ( d + rA)) ){
   echo ("Error Y not solvable");
}else{
   y = (( pow(rA,2) - pow(rC,2) + pow(i,2) + pow(j,2)) / (2 * j))
      - ( i / j) * x;
   echo(y = y);
   zpow2 = pow(rA,2) - pow(x,2) - pow(y,2);
   if ( zpow2 < 0){
      echo ("z not solvable");
   }else{
      z = sqrt(zpow2);
      echo (z = z);
      // subtract a cube with one of its corners 
      // at the point where the sphers intersect
      difference(){
         spheres();
         translate ([x,y - cube_size[1],z]){
           cube(cube_size);
         }
      }
      translate ([x,y - cube_size[1],z]){
           %cube(cube_size);
      }
  }
}

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.

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