Trilateration and locating the point (x,y,z)

Question 1

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).

Question 2

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;

Question 3

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);
      }
  }
}