Why does my implementation of trilateration give wrong results?

Question

I am trying to do an implementation of trilateration. The function gets three 3d coordinates and distances from base stations for every coordinate. It must return the position 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 should return P=(2,3,0).

But it is returning [3.33253331 1.66746669 1.33373281].

What is wrong in my code?

Answer 1

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.