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.

Answer 3

I followed @zerm's code shown above and it worked fairly well (yellow is calculated point from 3 towers). Results are shown in the Folium snippet. Multilateration using linalg.SVD

However, when I followed the same algorithm with the changes that @mcdowella suggested (#2) using the Least Squares of an MxN System solution, results are much better. Multilateration using Least Squares MxN

Here is the amended code:

A = []
b = []
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]]
    b += [[Dm]]
# Solve using Least Squares of an MxN System
# A*x = b --> x = (ATA)_inv.AT.b = A+.b
A = np.array(A)
b = np.array(b)
AT = A.T
ATA = np.matmul(AT,A)
ATA_inv = np.linalg.inv(ATA)
Aplus = np.matmul(ATA_inv,AT)
x = np.matmul(Aplus,b)
# convert back to lat/long from ECEF
# convert to degrees
lat = math.degrees(math.asin(x[2] / R))
lon = math.degrees(math.atan2(x[1],x[0]))

I'm still exploring other multilateration methods but this post really allowed me to understand the basics of N-points MLAT. Thanks!

Answer 4

To improve the accepted answer, one way to improve the SVD solution more is to account for the variation of the earth's radius by latitude; this particularly affects the altitude estimate, but it also has some knock-on effects on latitude and longitude. The "simple" solution would be to use the average value for R, which according to Wikipedia is 6371008.8 m rather than 6378137 m.

A more accurate estimate would be to adjust R for the latitude:

def EarthRadiusAtLatitude(lat):
    rlat = np.deg2rad(lat)

    a = np.float64(6378137.0)
    b = np.float64(6356752.3)

    rad = np.sqrt(((a*a*np.cos(rlat))**2 + (b*b*np.sin(rlat))**2) /
                  ((a*np.cos(rlat))**2 + (b*np.sin(rlat))**2))
    return rad

Then set R based on the latitude of one of the initial points. Or, if you have a large variation in latitude, you could compute the SVD based on an estimate of R and use the preliminary solution's latitude to solve using a closer estimate of R.

After making this adjustment, in my experiments with both constructed examples and "real world" data based on LTE eNodeB timing advance values, the SVD solution typically is within one second of latitude and longitude except in some degenerate cases, which is fairly comparable to a solution based on iterative optimization (i.e. minimizing the distance residuals).