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

Question

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 coo

Answer 1

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;