我想用python实现三边测量。我找不到我的功能有什么问题？

Question

我正在试着实现三边测量。函数得到三个三维坐标以及每个坐标到基站的距离。它必须返回点在三维空间三边测量中的位置。你知道吗

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

我有一个测试例子。这些坐标和距离P1=（2,2,0），P2=（3,3,0），P3=（1,4,0） r1=1，r2=1，r3=1.4142，应返回P=（2,3,0）。你知道吗

但它是returnig[3.3325331 1.66746669 1.33373281]

Answer 1

这个问题来自于给sqrt的表达式，由于数值不精确，这个表达式有点负。这将解决它：

Z1 = np.sqrt(max(0, r1**2-X**2-Y**2))
Z2 = -Z1 

更改这些行可以得到正确的结果：[1.99999361 3.00000639 0.]

注：如果该点与其他3个点位于同一平面上，则Z值为0，否则有两种解决方案。另外，提供r1、r2和r3的精确值非常重要，正如@meowgoesthedog所提到的。但是，即使使用精确的值，也始终需要小心浮点不精确性，并安全地使用sqrt。你知道吗