Finding an unknown point using weighted multilateration

Question

I have a series of points (latitude/longitude coordinates) on the earth and a series of distance estimates from each point to an unknown location. I would like to use multilateration to estimate the location of this unknown location. Starting with a simple example imagine 4 points and associated distance estimates to an unknown point with unknown location:

latitude, longitude, distance estimate 3-tuples below:

p1 = (31.2297, 121.4734, 3335.65)                           
p2 = (34.539, 69.171, 2477.17)                          
p3 = (47.907, 106.91, 1719.65)                      
p4 = (50.43, 80.25, 1242.27)

Finding the unknown point has already been explained here and a trilateration example here. Using the above example, the unknown is located at a latitude/longitude coordinate of: 36.989, 91.464

My question is unique because I'm looking for a way to perform multilateration with weights. Each distance estimate is only an estimate; the measurements are imprecise, but the smaller the distance the more accurate the measurement. I'd like use multilatertion but I'd like to give points associated with smaller distance estimations more 'weight' in determining a final answer as these shorter estimations are more accurate. How can I do this? I am looking for a solution in Python.

Going back to the previous example, but introducing error, I want to find the unknown location of the point again:

p1 = (31.2297, 121.4734, 4699.15)           
p2 = (34.539, 69.171, 2211.97)        
p3 = (47.907, 106.91, 1439.75)                              
p4 = (50.43, 80.25, 1222.07)

can you describe how your estimate of the error associated with each measurement varies as a function of distance - does it increase linearly? quadratically? — ali_m, Jul 19 '13 at 22:50
That's a good question. For simplicity, let's assume it's linear. — turtle, Jul 19 '13 at 22:56

Daniel · Accepted Answer · 2013-07-20T13:52:25.417

While this probably not exactly what you are looking for you can use this as starting point:

import numpy as np
import scipy.optimize as opt

#Returns the distance from a point to the list of spheres
def calc_distance(point):
    return np.power(np.sum(np.power(centers-point,2),axis=1),.5)-rad

#Latitude/longitude to carteisan
def geo2cart(lat,lon):
    lat=np.deg2rad(lat)
    lon=np.deg2rad(lon)
    points=np.vstack((earth_radius*np.cos(lat)*np.cos(lon),
           earth_radius*np.cos(lat)*np.sin(lon),
           earth_radius*np.sin(lat))).T
    return points

#Cartesian to lat/lon
def cart2geo(xyz):
    if xyz.ndim==1: xyz=xyz[None,:]
    lat=np.arcsin(xyz[:,2]/earth_radius)
    lon=np.arctan2(xyz[:,1],xyz[:,0])
    return np.rad2deg(lat),np.rad2deg(lon)

#Minimization function. 
def minimize(point):
    dist= calc_distance(point)
    #Here you can change the minimization parameter, here the distances
    #from a sphere to a point is divided by its radius for linear weighting.
    err=np.linalg.norm(dist/rad)
    return err

earth_radius = 6378
p1 = (31.2297, 121.4734, 3335.65)
p2 = (34.539, 69.171, 2477.17)
p3 = (47.907, 106.91, 1719.65)
p4 = (50.43, 80.25, 1242.27)

points = np.vstack((p1,p2,p3,p4))
lat    = points[:,0]
lon    = points[:,1]
rad    = points[:,2]

centers = geo2cart(lat,lon)

out=[]
for x in range(30):
    latrand=np.average(lat/rad)*np.random.rand(1)*np.sum(rad)
    lonrand=np.average(lon/rad)*np.random.rand(1)*np.sum(rad)
    start=geo2cart(latrand,lonrand)
    end_pos=opt.fmin_powell(minimize,start)
    out.append([cart2geo(end_pos),np.linalg.norm(end_pos-geo2cart(36.989,91464))])


out = sorted(out, key=lambda x: x[1])
print 'Latitude:',out[0][0][0],'Longitude:',out[0][0][1],'Distance:',out[0][1]

We obtain:

First set of points:  lat 40.1105092 lon 88.07068701
Second set of points: lat 40.36636421 lon 88.84527729

Im sure there is a better way, but at least you can play around with weights and error functions to see what happens. Of course there are several serious issues, one is you can get stuck in a local minima. There is probably a least squares way to do this- I just dont see it at the moment.

Just to double check this works:

p0=np.random.rand(2)*90+20
p1=np.random.rand(2)*-10+20+p0
p2=np.random.rand(2)*-10+20+p0
p3=np.random.rand(2)*-10+20+p0
p4=np.random.rand(2)*-10+20+p0

target=geo2cart(p0[0],p0[1])
points=np.vstack((p1,p2,p3,p4))
lat    = points[:,0]
lon    = points[:,1]

centers=geo2cart(lat,lon)
#You can change the random at the end to tune the amount of noise
rad =  np.power(np.sum(np.power(centers-target,2),axis=1),.5)#+np.random.rand(4)*10    

print '------------'
start=geo2cart(np.average(lat),np.average(lon))
end_pos=opt.fmin_powell(minimize,start)
print 'Exact',p0
print 'Start guess',cart2geo(start)
print 'Found',cart2geo(end_pos)
print 'Distance',np.linalg.norm(end_pos-target)

Exact [  45.21292244  101.85151772]
Start guess (array([ 60.63554123]), array([ 115.08426225]))
Found (array([ 45.21292244]), array([ 101.85151772]))
Distance 5.30420680512e-11

Thanks for this great work. One thing I don't understand is where you are getting the answers to the first and second sets of points. For ex., for the first set of points you get a solution of `40.1105092, 88.07068701`, which is far (~500km) from the actual location. Also, when I run your script, `cart2geo()` gives a lat, lon of the prediction at `54.85960821, 89.49203514`, not `40.1105092, 88.07068701`. Do I understand this correctly? Can you elaborate your explanation? — turtle, Jul 20 '13 at 08:08
I was playing with starting points- as mentioned you can easily get stuck in a local minima. What kind of noise is in the original data? Small changes to noise can change the answer quite a bit. Also your second set of points under SVD gives a radically different result. — Daniel, Jul 20 '13 at 14:22

Finding an unknown point using weighted multilateration

1 Answers1