Finding an unknown point using weighted multilateration
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)
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
上一篇: Xyz距离坐标
下一篇: 使用加权多边测量找到未知点