Finding a 4th point in 3D space knowing 3 other points and 2 distances to the 4th point from them
Solution 1:
(This is not an answer to the stated question per se, but explains how to efficiently do trilateration.)
In 3D, you need four fixed points (that are not all on the same plane), and their distances to the unknown point, to exactly determine the location of the unknown point.
Three fixed points (that are not all on the same line) and their distances to the unknown point will typically give you two possibilities, symmetrically mirrored by the plane formed by the three fixed points.
Let's say the unknown point is at $\vec{p} = (x, y, z)$, the three fixed points are at $\vec{v}_1 = (x_1 , y_1 , z_1)$, $\vec{v}_2 = (x_2 , y_2 , z_2)$, and $\vec{v}_3 = (x_3 , y_3 , z_3)$, at distances $d_1$, $d_2$, and $d_3$ from the unknown point, respectively. Solving the system of equations $$\left\lbrace\begin{aligned} \left\lVert \vec{p} - \vec{p}_1 \right\rVert &= \lvert d_1 \rvert \\ \left\lVert \vec{p} - \vec{p}_2 \right\rVert &= \lvert d_2 \rvert \\ \left\lVert \vec{p} - \vec{p}_3 \right\rVert &= \lvert d_3 \rvert \\ \end{aligned}\right . \iff \left\lbrace\begin{aligned} (x - x_1)^2 + (y - y_1)^2 + (z - z_1)^2 &= d_1^2 \\ (x - x_2)^2 + (y - y_2)^2 + (z - z_2)^2 &= d_2^2 \\ (x - x_3)^2 + (y - y_3)^2 + (z - z_3)^2 &= d_3^2 \\ \end{aligned}\right.$$ is nontrivial, especially in algebraic form.
Instead, change to a coordinate system where $(x_1 , y_1 , z_1)$ is at origin, $(x_2 , y_2 , z_2)$ is at $(h , 0 , 0)$, and $(x_3 , y_3 , z_3)$ is at $(i, j, 0)$. The unit vectors $\hat{e}_1 = ( X_1 , Y_1 , Z_1 )$, $\hat{e}_2 = ( X_2 , Y_2 , Z_2 )$, and $\hat{e}_3 = (X_3 , Y_3 , Z_3 )$ are $$\left\lbrace\begin{aligned} \vec{e}_1 &= \vec{v}_2 - \vec{v}_1 \\ \hat{e}_1 &= \frac{\vec{e}_1}{\left\lVert\vec{e}_1\right\rVert} \\ \vec{e}_2 &= \vec{v}_3 - \vec{v}_1 - \hat{e}_1 \left ( \hat{e}_1 \cdot \left ( \vec{v}_3 - \vec{v}_1 \right ) \right ) \\ \hat{e}_2 &= \frac{\vec{e}_2}{\left\lVert\vec{e}_2\right\rVert} \\ \hat{e}_3 &= \hat{e}_1 \times \hat{e}_2 \\ \end{aligned}\right.$$ and the fixed point coordinates are $$\left\lbrace\begin{aligned} h &= \left\lVert \vec{v}_2 - \vec{v}_1 \right\rVert = \sqrt{ (x_2 - x_1)^2 + (y_2 - y_1)^2 + (z_2 - z_1)^2 } \\ i &= \hat{e}_1 \cdot \left(\vec{v}_3 - \vec{v}_1\right) = X_1 ( x_3 - x_1 ) + Y_1 ( y_3 - y_1 ) + Z_1 ( Z_3 - Z_1 ) \\ j &= \hat{e}_2 \cdot \left(\vec{v}_3 - \vec{v}_1\right) = X_2 ( x_3 - x_1 ) + Y_2 ( y_2 - y_1 ) + Z_2 ( Z_3 - Z_2 ) \\ \end{aligned}\right.$$ The distances stay the same, but the system of equations is much simpler. For clarity, I'll use $(u, v, w)$ in these new coordinates instead of $(x, y, z)$: $$\left\lbrace\begin{aligned} u^2 + v^2 + w^2 &= d_1^2 \\ (u - h)^2 + v^2 + w^2 &= d_2^2 \\ (u - i)^2 + (v - j)^2 + w^2 &= d_3^2 \end{aligned}\right.$$ which is easily solved: $$\left\lbrace\begin{aligned} u &= \frac{d_1^2 - d_2^2 + h^2}{2 h} \\ v &= \frac{d_1^2 - d_3^2 + i^2 + j^2 - 2 i u}{2 j} \\ w &= \pm \sqrt{d_1^2 - u^2 - v^2} \\ \end{aligned}\right.$$ In the original coordinate system, $$\vec{p} = \vec{v}_1 + u \hat{e}_1 + v \hat{e}_2 + w \hat{e}_3 \quad \iff \quad \left\lbrace\begin{aligned} x &= x_1 + u X_1 + v X_2 + w X_3 \\ y &= y_1 + u Y_1 + v Y_2 + w Y_3 \\ z &= z_1 + u Z_1 + v Z_2 + w Z_3 \\ \end{aligned}\right.$$ noting that if $w$ is not a real, then there is no solution; if $w \approx 0$, there is one solution; and otherwise there are two solutions, one with positive $w$, and the other with negative $w$.
If you know the distances to four fixed points, you only really need the fourth point (not coplanar with the three other fixed points) to distinguish which case it is. If the distances contain noise, it might make sense to calculate the result using each unique triplet ($123$, $124$, $134$, and $234$), and return their mean.
In pseudocode, for trilateration, you should precalculate the values that only depend on the fixed points:
Let ex1 = x2 - x1
Let ey2 = y2 - y1
Let ez2 = z2 - z1
Let h = sqrt( ex1*ex1 + ey1*ey1 + ez1*ez1 )
If h <= epsilon:
Error: First and second point are too close.
End If
Let ex1 = ex1 / h
Let ey1 = ey1 / h
Let ez1 = ez1 / h
Let i = ex1*(x2 - x1) + ey1*(y2 - y1) + ez1*(z2 - z1)
Let ex2 = x3 - x1 - i*ex1
Let ey2 = y3 - y1 - i*ey1
Let ez2 = z3 - z1 - i*ez1
Let t = sqrt(ex2*ex2 + ey2*ey2 + ez2*ez2)
If t <= epsilon:
Error: the three fixed points are too close to being on the same line.
End If
Let ex2 = ex2 / t
Let ey2 = ey2 / t
Let ez2 = ez2 / t
Let j = ex2*(x3 - x1) + ey2*(y3 - y1) + ez2*(z3 - z1)
If j <= epsilon and j >= -epsilon:
Error: the three fixed points are too close to being on the same line.
End If
Let ex3 = ey1*ez2 - ez1*ey2
Let ey3 = ez1*ex2 - ex1*ez2
Let ez3 = ex1*ey2 - ey2*ex1
where epsilon is the largest positive number that should be treated as zero, and represents the expected precision in coordinates and distances, for example 0.001.
The function that finds the coordinates for the unknown point is then
# Fixed points are at (x1,y1,z1), (x2,y2,z2), (x3,y3,z3)
# Function takes the distances to fixed points d1, d2, d3
# Function expects unit vectors (ex1,ey1,ez1), (ex2,ey2,ez2), (ex3,ey3,ez3)
# to be precalculated, with fixed points at (0,0,0), (h,0,0), (i,j,0)
# in that coordinate system, without changing the distances
Function Trilaterate(d1, d2, d3):
Let u = (d1*d1 - d2*d2 + h*h) / (2*h)
Let v = (d1*d1 - d3*d3 + i*(i - 2*u) + j*j) / (2*j)
Let ww = d1*d1 - u*u - v*v
If ww < -epsilon:
Return no solutions
Else
If ww < epsilon:
Return a single solution:
x = x1 + u*ex1 + v*ex2
y = y1 + u*ey1 + v*ey2
z = z1 + u*ez1 + v*ez2
Else:
w = sqrt(ww)
Return two solutions:
x = x1 + u*ex1 + v*ex2 + w*ex3
y = y1 + u*ey1 + v*ey2 + w*ey3
z = z1 + u*ez1 + v*ez2 + w*ez3
And
x = x1 + u*ex1 + v*ex2 - w*ex3
y = y1 + u*ey1 + v*ey2 - w*ey3
z = z1 + u*ez1 + v*ez2 - w*ez3
End If
End Function
This is simple enough to be done on 8-bit microcontrollers, if necessary.