Я хочу реализовать трилатерацию в Python. Я не могу найти, что не так с моей функцией? - PullRequest
Новая
       44

Я хочу реализовать трилатерацию в Python. Я не могу найти, что не так с моей функцией?

2 голосов
/ 09 мая 2019

Я пытаюсь осуществить трилатерацию.Funtion получает три 3d-кордианта и расстояния от базовых станций для каждого кординиата.Он должен возвращать положение точки в трехмерном пространственном трилатерации.

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).

Но это возврат [3.33253331 1.66746669 1.33373281]

1 Ответ

0 голосов
/ 09 мая 2019

Проблема возникает из выражения, заданного для 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.

...