algorithm - 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.