Localizing a point using distances to three other

问题:

Assume that we have 4 points in 3-D (P1, P2, P3, P4). If the coordinates of those points are given with their euclidian distances to a fifth point P5 (r1, r2, r3, r4), how to calculate the coordinates of P5?

In this post, answer of Don Reba is perfect for 2-D. But how do I extend it to 3-D?

Here is my code for 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);

    }

I want to overload the function with the signature

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

回答1:

The Wikipedia trilateriation article describes the answer. The calculation steps are:

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

回答2:

You need to solve system of four equations (i=1..4, Di is distance to ith point)

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

It is possible to solve system of three equations and use fourth to choose proper solution (from two).

This is how GPS works (where time delays are for distances).

In GPS receivers optimization methods are frequently used, especially when many satellites are available and algebraic solution may be instable.