Getting wrong coordinate with 3d trilateration in different cases (noisy environment).

Question

I implemented a trilateration algorithm based on the following arcticle: https://en.wikipedia.org/wiki/Trilateration

My problem is that sometimes I get pretty inaccurate data when I try to simulate a noisy environment where the distance is not 100% correct.

Sample case:

I want to localize a point which is at $(x=6 ,\; y=3 ,\; z=0.1)$

Having the following neighbours (r is the distance from the unknown point in noisy environment so it cannot be 100% accurate distance):

$\textbf{P}_1 \triangleq (x= 0.0,\; y= 0.0,\; z= 2.0,\; r= 7.072087205421344)$

$\textbf{P}_2 \triangleq (x= 0.0, \;y= 10.0,\; z= 2.0,\; r= 9.513288479590965)$

$\textbf{P}_3 \triangleq (x= 14.0,\; y= 0.0,\; z= 2.0,\; r= 8.852713864853573)$

The two possible coordinate I receive is:

$ \textbf{result}_1\; = (x= 5.987281238146913, \;y= 2.975587987258304,\; z=0.30494185317163947)$ $ \textbf{result}_2 \; = (x= 5.987281238146913,\; y= 2.975587987258304,\; z= 4.3049418531716395)$

My problem is that the result on the Z axis is quite inaccurate, but if I change the third neighbour to the following:

$ \textbf{P}_3 = (x = 4.9726,\; y = 1.9595,\; z = 2.0,\; r= 2.4685438564654025) $

than one of the 2 result on the z axis became much better:

$ \textbf{result}_1\; = (x = 6.116093226654657,\; y = 2.975587987258304,\; z= 0.06255394551554705)$

$ \textbf{result}_1\; = (x = 6.116093226654657,\; y = 2.975587987258304,\; z = 3.937446054484453)$

My question is, if I have more than $3$ neighbors how can I decide which combination makes the most accurate result?

Also am I using the right approach or there is a better algorithm to calculate unknown coordinates in noisy environment known the anchors and the distances?

I really appreciate any help you can provide.

Answer 1

Because I've had my hand in that article, I decided to show one implementation of that trilateration algorithm, using awk. tri.awk:

#!/usr/bin/awk -f
NF >= 3 && n < 3 {
    n++
    x[n] = $1
    y[n] = $2
    z[n] = $3
    if (n == 3) {
        x1 = x[2] - x[1]
        y1 = y[2] - y[1]
        z1 = z[2] - z[1]
        n1 = sqrt(x1*x1 + y1*y1 + z1*z1)
        if (n1 <= 0.0) {
            printf "First and second anchor are at the same point.\n" > "/dev/stderr"
            exit(1)
        }
        x1 = x1/n1
        y1 = y1/n1
        z1 = z1/n1

        i = (x[3] - x[1])*x1 + (y[3] - y[1])*y1 + (z[3] - z[1])*z1

        x2 = x[3] - x[1] - i*x1
        y2 = y[3] - y[1] - i*y1
        z2 = z[3] - z[1] - i*z1
        n2 = sqrt(x2*x2 + y2*y2 + z2*z2)
        if (n2 <= 0.0) {
            printf "The three anchors are collinear.\n" > "/dev/stderr"
            exit(1)
        }
        x2 = x2/n2
        y2 = y2/n2
        z2 = z2/n2

        x3 = y1*z2 - z1*y2
        y3 = z1*x2 - x1*z2
        z3 = x1*y2 - y1*x2
        n3 = sqrt(x3*x3 + y3*y3 + z3*z3)
        if (n3 <= 0.0) {
            printf "Something is wrong with anchors.\n" > "/dev/stderr"
            exit(1)
        }
        x3 = x3/n3
        y3 = y3/n3
        z3 = z3/n3

        d = sqrt((x[2] - x[1])**2 + (y[2] - y[1])**2 + (z[2] - z[1])**2)
        j = x2*(x[3] - x[1]) + y2*(y[3] - y[1]) + z2*(z[3] - z[1])
    }
    next
}
NF >= 3 && n >= 3  {
    r1 = $1
    r2 = $2
    r3 = $3

    xr = (r1*r1 - r2*r2 + d*d) / (2*d)

    yr = (r1*r1 - r3*r3 + i*i + j*j) / (2*j) - i * xr / j

    zr = sqrt(r1*r1 - xr*xr - yr*yr)

    printf "%.3f %.3f %.3f  ", x[1] + xr*x1 + yr*x2 + zr*x3, y[1] + xr*y1 + yr*y2 + zr*y3, z[1] + xr*z1 + yr*z2 + zr*z3
    printf "%.3f %.3f %.3f\n", x[1] + xr*x1 + yr*x2 - zr*x3, y[1] + xr*y1 + yr*y2 - zr*y3, z[1] + xr*z1 + yr*z2 - zr*z3
}

The first three lines should contain the Cartesian coordinates for the static points (anchors). Each additional line should contain the distance from the unknown point to each of the static points, in the same order.

Let's say the anchors are at $(0, 0, 2)$, $(0, 10, 2)$, and $(14, 0, 2)$. The distances to point $(6, 3, 0.1)$ are $6.972087$, $9.413288$, and $8.752714$, respectively. So, if we feed

0  0 2
0 10 2
14 0 2
6.972087 9.413288 8.752714

to the above tri.awk, it'll provide us with the coordinate pair

6.000 3.000 0.100  6.000 3.000 3.900

If we perturb the distances by 0.1, i.e. feed

0  0 2
0 10 2
14 0 2
6.972087 9.413288 8.752714
6.872087 9.413288 8.752714
7.072087 9.413288 8.752714
6.972087 9.313288 8.752714
6.972087 9.513288 8.752714
6.972087 9.413288 8.652714
6.972087 9.413288 8.852714

we receive

6.000 3.000  0.100   6.000 3.000 3.900
5.951 2.931  0.204   5.951 2.931 3.796
6.050 3.070  0.004   6.050 3.070 3.996
6.000 3.094  0.257   6.000 3.094 3.743
6.000 2.905 -0.042   6.000 2.905 4.042
6.062 3.000  0.309   6.062 3.000 3.691
5.937 3.000 -0.088   5.937 3.000 4.088

---

If you have exactly three static sites, and you make more than one measurement to a more or less stationary unknown point, you can average either the distances, or the measurement points.

If you have more than three static sites, your problem is actually multilateration. The solution can be described by a set of linear equations, basically leading to a linear least squares fit on the result.

Of course, in theory you could do the computation for each unique triplet among your $N$ anchors; this would yield $\binom{N}{3} = \frac{N^3 - 3 N^2 + 2 N}{6}$ triplets and thus $\frac{N^3 - 3 N^2 + 2 N}{3}$ estimated points, which you could treat as sampled locations with noise; perhaps reject outliers, and use the average (or median in coordinates?) of the rest.

However, the multilateration approach is probably your best bet, although I suppose it could depend on the noise distribution, too.