I want to implement trilateration in python. I can't t find what is wrong with my funtion?

Question

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]

Please give some background info on your problem. What are the inputs and outputs, what are their dimensions? Where did you take the algorithm from? Last but not the least, what did you try to solve the problem yourself before asking here? — ivan_pozdeev, May 09 '19 at 12:14
E.g. drawing a graph on paper and calculating intermediate values yourself based on theory, then stepping through the code, should show where your program starts producing wrong data. — ivan_pozdeev, May 09 '19 at 12:17
Your example input value for `r3` is imprecise – it should be `sqrt(2)` but you are approximating it as `1.4142`. Using the former gives `(2, 3, 1.825e-08)` which is correct to within floating point error. Trilateration distances must be as accurate as possible. — meowgoesthedog, May 09 '19 at 12:24

score 1 · Answer 1 · edited Oct 15 '21 at 08:23

I don't have enough reputation to comment, so I have to write a new answer.

Your code still have some error. After many tests, I find that the calculation of K2 has some typos.

The perfect function should be

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(max(0, r1**2-X**2-Y**2))
  Z2 = -Z2

  K1 = P1 + X * Xn + Y * Yn + Z1 * Zn
  K2 = P1 + X * Xn + Y * Yn + Z2 * Zn
  return K1,K2

The two return arrays are two possible positions.

This code is very useful because the code from this answer is Python form of https://en.wikipedia.org/wiki/True-range_multilateration#Three_Cartesian_dimensions,_three_measured_slant_ranges and it is the simplest code for trilateration in 3D coordinates.

Line ```Z2 = -Z2``` is not correct because it doesn't exist yet. I think you mean: ```Z2 = -Z1``` — Wallem89, Aug 17 '23 at 08:45

Gilles-Philippe Paillé · Accepted Answer · 2019-05-09T13:51:15.793

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.

I want to implement trilateration in python. I can't t find what is wrong with my funtion?

2 Answers2