Math - 3d позиционирование/мультилатерация

Question

У меня проблема с позиционированием 3d - вроде GPS. Учитывая набор известных 3d-координат (x, y, z) и их расстояний d от неизвестной точки, я хочу найти неизвестную точку. Может быть любое количество опорных точек, однако их будет не менее четырех.

Так, например, точки находятся в формате (x, y, z, d). Я мог бы:

(1,0,0,1) 
(0,2,0,2) 
(0,0,3,3) 
(0,3,4,5) 

И здесь неизвестный пункт был бы (0,0,0,0).

Что было бы самым лучшим способом? Существует ли существующая библиотека, поддерживающая 3d multilateration? (Я не смог найти его). Так как маловероятно, что мои данные будут иметь точное решение (все сферы с 4+, вероятно, не будут иметь ни одной совершенной точки пересечения), алгоритм должен быть способен аппроксимировать его.

До сих пор я думал о том, чтобы взять каждое подмножество из трех точек, триангулируя неизвестное на основе этих трех, а затем усредняя все результаты. Есть лучший способ сделать это?

Answer 1

Вы можете использовать нелинейный подход оптимизации, определяя функцию «стоимость», которая включает в себя ошибку расстояния от каждой точки наблюдения.

Установка неизвестной точки в (x,y,z), и принимая во внимание множество N точек наблюдения (xi,yi,zi,di) следующие функции может быть использован для характеристики суммарной погрешности расстояния:

C(x,y,z) = sum(((x-xi)^2 + (y-yi)^2 + (z-zi)^2 - di^2)^2) 
      ^^^ 
      ^^^ for all observation points i = 1 to N 

Это сумма квадратов ошибок расстояния для все точки в множестве. (Это на самом деле основано на ошибке в квадрате расстояния, так что нет квадратных корней!)

Когда эта функция как минимум, конечная точка (x,y,z) будет находиться в оптимальном положении. Если решение дает C(x,y,z) = 0, все наблюдения будут точно выполнены.

Одним из утверждений для сведения к минимуму этого уравнения будет Newton's method. Вам нужно будет указать начальную отправную точку для итерации - возможно, среднее значение точек наблюдения (если они охватывают (x,y,z)) или, возможно, начальное триангулированное значение из любых трех наблюдений.

Редактировать: Метод Ньютона - это итеративный алгоритм, который можно использовать для оптимизации. Простая версия будет работать по этим линиям:

H(X(k)) * dX = G(X(k)); // solve a system of linear equations for the 
         // increment dX in the solution vector X 
X(k+1) = X(k) - dX;  // update the solution vector by dX 

G(X(k)) обозначает вектор градиента оценивали при X(k), в данном случае:

G(X(k)) = [dC/dx 
      dC/dy 
      dC/dz] 

H(X(k)) обозначает матрицу Гессе, оцениваемые по X(k), в этом случае симметричная матрица 3x3:

H(X(k)) = [d^2C/dx^2 d^2C/dxdy d^2C/dxdz 
      d^2C/dydx d^2C/dy^2 d^2C/dydz 
      d^2C/dzdx d^2C/dzdy d^2C/dz^2] 

Вы должны иметь возможность дифференцировать аналитическими выражениями для G,H.

Другой подход - если вам не нравятся производные - это приблизительное число G,H с использованием конечных разностей.

Надеюсь, что это поможет.

Answer 2

Если вы можете потратить время, итерационное решение должно приблизиться к правильному решению довольно быстро. Таким образом, выберите любую точку на правильном расстоянии от сайта A, затем перейдите к набору, задав расстояние до точки, затем отрегулируйте точку так, чтобы она находилась в одном направлении от места, но на правильном расстоянии. Продолжайте до тех пор, пока ваша требуемая точность не будет достигнута (или пока точка больше не продвинется достаточно далеко на каждой итерации, чтобы она соответствовала вашей точности, в зависимости от возможных эффектов приближенных входных данных).

Для аналитического подхода я не могу придумать ничего лучшего, чем то, что вы уже предлагаете.

Answer 3

Нелинейные процедуры решения не требуются. Вы можете легко линеаризовать систему. Если вы принимаете пару разницы

$ (x-x_i)^2- (x-x_j)^2 + (y-y_i)^2- (y-y_j)^2 + (z-z_i)^2- (г-z_j)^2 = d_i^2-d_j^2 $

затем немного алгебры дает линейных уравнений

$ (x_i-x_j) х + (y_i-Y_j) y + (zi-zj) z = -1/2 (d_i^2-d_j^2 + ds_i^2-ds_j^2) $,

где $ ds_i $ - расстояние от $ i^{th } $ в начало координат. Это уравнения плоскостей, определяемые пересечением $ i^{th} $ и $ j^{th} $ сфер.

Для четырех датчиков вы получаете переопределенную систему linear с $ 4 выбором 2 = 6 $ уравнений. Если $ A $ есть результирующая матрица и $ B $ соответствующий вектор RHS, то можно решить нормальные уравнения

$ А^ТА г = А^Т Ь $

для позиции вектора $ г $. Это будет работать до тех пор, пока ваши датчики не будут копланарными.

Answer 4

Посмотрите, Ньютон или Ньютон-Гаусс не будут работать. Это потому, что у вас есть разрыв в определенных точках пространства производных этой функции. Поверьте мне, я попробовал Ньютона - он почти никогда не сходится. Вместо этого я попробовал другой набор алгоритмов. Попробуйте использовать алгоритм прямого поиска для оптимизации (найти минимум или максимум функции). Он работает для меня - но иногда он сходится в неправильной точке - вероятно, из-за «планарности» из 4 выбранных точек. В этом случае вы можете выбрать другую отправную точку - и вы, вероятно, получите правильный ответ. алгоритмический пример, который я нашел, ОЧЕНЬ прост в реализации и работает удивительно хорошо. Надеюсь, что помогли. Приветствия.