Multilateration of GPS Coordinates

Question

I have N GPS coordinates with N distances given to an unknown position which I wish to determine.

My first approach was to use just three points and trilateration, exactly as described here. This approach was already quite accurate (best error~5km), but I would like to improve this and increase the robustness. Because the given distances are not very accurate to begin with, I thought about using multiple measurements and multilateration. However, it turned out that this approach is by far less accurate (best error~100km) although I provide more than 3 points/distances (tested with up to 6) and now I am asking, if someone has an idea what I could have done wrong.

In short, my approach for multilateration is as follows:

1. Convert all coordinates into ECEF
2. Build a matrix as described in Eq.7 at wikipedia
3. Use SVD to find the minimizer
4. As the solution is only up to scale, I use a root-finding approach to determine a normalization so that the coordinates converted back into LLA result in a height of 0 (my initial assumption is that all coordinates are at zero height)
5. Convert back into LLA

LLA/ECEF conversion is double-checked and correct. Step 2 and 3 I've checked with euclidean coordinates (and exact distances) and appear correct. I came up with step 4 by myself, I have no clue if this is a good approach at all, so suggestions are welcome.

+++UPDATE

I've put together sample code in python to illustrate the problem with some ground truth. Trilateration gets as close as 400m, while Multilateration ranges at 10-130km here. Because of length, I've put it at ideone

Answer 1

Eventually, I figured it out myself - or at least improve the accuracy significantly.

The approach described at wikipedia (Eq.7) is apparently not very suited for this application, but in this case it is already a lot easier.

Considering Eq. 6 from wikipedia, we can simplify it a lot: R_0 can be guessed as the earth radius, as the origin of ECEF coordinates lies in the center of earth. Therefore, there is no need to shift everything to make one Point the origin and we can use all N equations.

In python, with P an array of ECEF coordinates and dists the distances to these points, it all boils down to

R = 6378137 # Earth radius in meters
A = []
for m in range(0,len(P)):
    x = P[m][0]
    y = P[m][1]
    z = P[m][2]
    Am = -2*x
    Bm = -2*y
    Cm = -2*z
    Dm = R*R + (pow(x,2)+pow(y,2)+pow(z,2)) - pow(dists[m],2)
    A += [[Am,Bm,Cm,Dm]]
# Solve using SVD
A = numpy.array(A)
(_,_,v) = numpy.linalg.svd(A)
# Get the minimizer
w = v[3,:]
w /= w[3] # Resulting position in ECEF

With this approach, what I described as Step 4 is no longer necessary. In fact, it even makes the solution worse.

Now, accuracy ranges between 2km and 275m -- in most cases better than the "optimal" trilateration with an error of 464m.

Answer 2

Some comments:

1) You have already checked some steps against exact answers. I suggest that you create toy problems with known amounts of random noise added to the observations. Since you know the right answer in this case you can see what happens with error propagation. If your method works well here but badly on real data you might want to think about horrid behaviour in real life, such as one or a few of the distances being seriously wrong.

2) I don't know why your solution is only up to scale, as the underlying data are properly scaled - if I went out there with ropes cut to length and tied them to the fixed points there would be no ambiguity. When you use SVD to solve the equations (7) are you doing something like www.cse.unr.edu/~bebis/MathMethods/SVD/lecture.pdf to get out a least squares solution? That should give you x, y, and z without ambiguity.

3) I'm not at all sure about how observational errors work through (7). I don't like all the divisions, for one thing. It might be worth writing down an equation for the sum of the squares of the differences between measured distances and computed distances given x,y,z for the unknown position, and then minimising this for x,y,z. The Wikipedia article discards this approach due to its cost, but it might give you a more accurate answer, and computing and comparing this answer might tell you something even if you can't use this method in practice.