Локализация точки с использованием расстояний до трех других точек в 3D

Question

Предположим, что у нас есть 4 точки в 3D (P1, P2, P3, P4). Если координаты этих точек даны с их евклидовыми расстояниями до пятой точки P5 (r1, r2, r3, r4), как рассчитать координаты P5?

В этом посте ответ Don Reba идеально подходит для 2-D. Но как мне расширить его до 3-х?

Вот мой код для 2D:

    static void localize(double[] P1, double[] P2, double[] P3, double r1, double r2, double r3)
    {
        double[] ex = normalize(difference(P2, P1));
        double i = dotProduct(ex, difference(P3, P1));
        double[] ey = normalize(difference(difference(P3, P1), scalarProduct(i, ex)));
        double d = magnitude(difference(P2, P1));
        double j = dotProduct(ey, difference(P3, P1));
        double x = ((r1*r1) - (r2*r2) + (d*d)) / (2*d);
        double y = (((r1*r1) - (r3*r3) + (i*i) + (j*j)) / (2*j)) - ((i*x) / j);
        System.out.println(x + " " + y);

    }

Я хочу перегрузить функцию с подписью

static void localize(double[] P1, double[] P2, double[] P3, double[] P4, double r1, double r2, double r3, double r4)

Answer 1

В статье трилатериации Википедии описан ответ. Этапы расчета:

1. e_x = (P2 - P1) / ‖P2 - P1‖
2. i = e_x(P3 - P1)
3. e_y = (P3 - P1 - i · e_x) / ‖P3 - P1 - i · e_x‖
4. d = ‖P2 - P1‖
5. j = e_y(P3 - P1)
6. x = (r₁² - r₂² + d²) / 2d
7. y = (r₁² - r₃² + i² + j²) / 2j - ix / j
8. z = ± sqrt (r₁² - x² - y²)

Answer 2

Вам необходимо решить систему из четырех уравнений (i=1..4, Di - расстояние до i-й точки)

(X-Xi)^2+(Y-Yi)^2+(Z-Zi)^2=Di^2

Можно решить систему из трех уравнений и использовать четвертое, чтобы выбрать правильное решение (из двух).

Вот как работает GPS (где временные задержки для расстояний).

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