I have 3 points in space A, B, and C all with (x,y,z) coordinates, therefore I know the distances between all these points. I wish to find point D(x,y,z) and I know the distances BD and CD, I do NOT know AD.
The method I have attempted to solve this using is first saying that there are two spheres known on points B and C with radius r (distance to point D). The third sphere is found by setting the law of cosines equal to the formula for distance between two vectors ((V1*V2)/(|V1||V2|)) = ((a^2+b^2-c^2)/2ab).
Now point D should be the intersection of these three spheres, but I have not been able to calculate this or find a way to. I either need help finding point D and I can give numeric points for an example, or I need to know if I need more information to solve (another point with a distance to D known).
Ok, now assuming that distance AD is known, how do I calculate point D?
This is what it looks like when I graph the two spheres and the one I calculated, as you can see it intersects on point F. (D in this case)
Best Answer
(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:
where
epsilon
is the largest positive number that should be treated as zero, and represents the expected precision in coordinates and distances, for example0.001
.The function that finds the coordinates for the unknown point is then
This is simple enough to be done on 8-bit microcontrollers, if necessary.