I am trying to do an implematation of trilateration. Funtion gets three 3d cordiantes and distances from base stations for every cordinate. It must return postion of the point in 3d space trilateration.
def trilateration(P1, P2, P3, r1, r2, r3):
p1 = np.array([0, 0, 0])
p2 = np.array([P2[0] - P1[0], P2[1] - P1[1], P2[2] - P1[2]])
p3 = np.array([P3[0] - P1[0], P3[1] - P1[1], P3[2] - P1[2]])
v1 = p2 - p1
v2 = p3 - p1
Xn = (v1)/np.linalg.norm(v1)
tmp = np.cross(v1, v2)
Zn = (tmp)/np.linalg.norm(tmp)
Yn = np.cross(Xn, Zn)
i = np.dot(Xn, v2)
d = np.dot(Xn, v1)
j = np.dot(Yn, v2)
X = ((r1**2)-(r2**2)+(d**2))/(2*d)
Y = (((r1**2)-(r3**2)+(i**2)+(j**2))/(2*j))-((i/j)*(X))
Z1 = np.sqrt(r1**2-X**2-Y**2)
Z2 = np.sqrt(r1**2-X**2-Y**2)*(-1)
K1 = P1 + X*Xn + Y * Yn + Z1 * Zn
K2 = p1 + X * Xn + Y * Yn - Z2 * Zn
return K1
I have a test example. With those cordinates and distances P1=(2,2,0), P2=(3,3,0), P3=(1,4,0) r1=1, r2=1, r3=1.4142, it shoudl return P=(2,3,0).
But it is returnig [3.33253331 1.66746669 1.33373281]
The problem comes from the expression given to sqrt that is slightly negative due to numerical imprecision. This will fix it:
Z1 = np.sqrt(max(0, r1**2-X**2-Y**2))
Z2 = -Z1
Changing these lines give me the correct result: [1.99999361 3.00000639 0.]
Note: If the point lies on the same plane as the other 3 points, the Z value will be 0, otherwise there are two solutions. Also, providing precise values for r1, r2, and r3 is very important, as mentioned by @meowgoesthedog. However, even with precise values, you always need to be careful about floating point imprecisions and safely use sqrt.