mlat test now creates a cheesy moving simulated aircraft. mlat is broken though due to incorrect assumptions in the solver.
This commit is contained in:
Nick Foster
parent
e771c21730
commit
c7e216bca0
@ -43,8 +43,8 @@ import bisect
|
||||
|
||||
#change this to 0 for ASCII format for debugging. use HIGHEST_PROTOCOL
|
||||
#for actual use to keep the pickle size down.
|
||||
pickle_prot = 0
|
||||
#pickle_prot = pickle.HIGHEST_PROTOCOL
|
||||
#pickle_prot = 0
|
||||
pickle_prot = pickle.HIGHEST_PROTOCOL
|
||||
|
||||
class rx_data:
|
||||
def __init__(self):
|
||||
@ -53,8 +53,8 @@ class rx_data:
|
||||
self.data = None
|
||||
|
||||
class stamp:
|
||||
def __init__(self, addr, secs, frac_secs):
|
||||
self.addr = addr
|
||||
def __init__(self, clientinfo, secs, frac_secs):
|
||||
self.clientinfo = clientinfo
|
||||
self.secs = secs
|
||||
self.frac_secs = frac_secs
|
||||
def __lt__(self, other):
|
||||
@ -81,9 +81,7 @@ def ordered_insert(a, item):
|
||||
class client_info:
|
||||
def __init__(self):
|
||||
self.name = ""
|
||||
self.latitude = 0.0
|
||||
self.longitude = 0.0
|
||||
self.altitude = 0.0
|
||||
self.position = []
|
||||
self.offset_secs = 0
|
||||
self.offset_frac_secs = 0.0
|
||||
|
||||
@ -117,51 +115,53 @@ class mlat_server:
|
||||
except socket.error:
|
||||
self._conns.remove(conn)
|
||||
if not pkt: break
|
||||
#try:
|
||||
msglist = pickle.loads(pkt)
|
||||
for msg in msglist:
|
||||
#DEBUG: change conn.clientinfo.name to conn.addr for production
|
||||
st = stamp(conn.clientinfo.name, msg.secs, msg.frac_secs)
|
||||
if msg.data not in self._reports:
|
||||
self._reports[msg.data] = []
|
||||
try:
|
||||
msglist = pickle.loads(pkt)
|
||||
for msg in msglist:
|
||||
st = stamp(conn.clientinfo, msg.secs, msg.frac_secs)
|
||||
if msg.data not in self._reports:
|
||||
self._reports[msg.data] = []
|
||||
|
||||
#ordered insert
|
||||
ordered_insert(self._reports[msg.data], st)
|
||||
self._lastreport = st.tofloat()
|
||||
# for report in self._reports.values():
|
||||
# for st in report:
|
||||
# print st.addr, st.secs, st.frac_secs
|
||||
#ordered insert
|
||||
ordered_insert(self._reports[msg.data], st)
|
||||
if st.tofloat() > self._lastreport:
|
||||
self._lastreport = st.tofloat()
|
||||
|
||||
#except Exception as e:
|
||||
# print "Invalid message from %s: %s" % (conn.addr, pkt)
|
||||
# print e
|
||||
except Exception as e:
|
||||
print "Invalid message from %s: %s" % (conn.addr, pkt)
|
||||
print e
|
||||
|
||||
#self.prune()
|
||||
|
||||
#prune should delete all reports in self._reports older than 5s.
|
||||
#how do we get the appropriate time? we either trust the reporting
|
||||
#stations, or we use UTC.
|
||||
#if we assume all stations are using UTC, we can prune on UTC, but
|
||||
#this computer has to be closely synchronized as well
|
||||
#not really tested well
|
||||
def prune(self):
|
||||
for report in self._reports:
|
||||
if self._reports[report][-1].tofloat() - self._lastreport > 5:
|
||||
self._reports.remove(report)
|
||||
for data, stamps in self._reports.iteritems():
|
||||
#check the last stamp first so we don't iterate over
|
||||
#the whole list if we don't have to
|
||||
if self._lastreport - stamps[-1].tofloat() > 5:
|
||||
del self._reports[data]
|
||||
else:
|
||||
for i,st in enumerate(stamps):
|
||||
if self._lastreport - st.tofloat() > 5:
|
||||
del self._reports[data][i]
|
||||
if len(self._reports[data]) == 0:
|
||||
del self._reports[data]
|
||||
|
||||
#return a list of eligible messages for multilateration
|
||||
#eligible reports are:
|
||||
#1. bit-identical
|
||||
#2. from distinct stations (at least 3)
|
||||
#3. within 0.001 seconds of each other
|
||||
#3. within 0.003 seconds of each other
|
||||
#traverse the reports for each data pkt (hash) looking for >3 reports
|
||||
#within 0.001s, then check for unique IPs (this should pass 99% of the time)
|
||||
#within 0.003s, then check for unique IPs (this should pass 99% of the time)
|
||||
#let's break a record for most nested loops. this one goes four deep.
|
||||
#it's loop-ception!
|
||||
def get_eligible_reports(self):
|
||||
groups = []
|
||||
for data,stamps in self._reports.iteritems():
|
||||
if len(stamps) > 2: #quick check before we do a set()
|
||||
stations = set([st.addr for st in stamps])
|
||||
stations = set([st.clientinfo for st in stamps])
|
||||
if len(stations) > 2:
|
||||
i=0
|
||||
#it's O(n) since the list is already sorted
|
||||
@ -169,13 +169,13 @@ class mlat_server:
|
||||
while(i < len(stamps)):
|
||||
refstamp = stamps[i].tofloat()
|
||||
reps = []
|
||||
while (i<len(stamps)) and (stamps[i].tofloat() < (refstamp + 0.001)):
|
||||
while (i<len(stamps)) and (stamps[i].tofloat() < (refstamp + 0.003)):
|
||||
reps.append(stamps[i])
|
||||
i+=1
|
||||
deduped = []
|
||||
for addr in stations:
|
||||
for cinfo in stations:
|
||||
for st in reps[::-1]:
|
||||
if st.addr == addr:
|
||||
if st.clientinfo == cinfo:
|
||||
deduped.append(st)
|
||||
break
|
||||
if len(deduped) > 2:
|
||||
@ -220,6 +220,21 @@ class mlat_server:
|
||||
print "New connection from %s: %s" % (addr[0], clientinfo.name)
|
||||
except socket.error:
|
||||
pass
|
||||
|
||||
#retrieve altitude from a mode S packet or None if not available
|
||||
#returns alt in meters
|
||||
def get_modes_altitude(data):
|
||||
df = data["df"] #reply type
|
||||
f2m = 0.3048
|
||||
if df == 0 or df == 4:
|
||||
return air_modes.altitude.decode_alt(data["ac"], True)
|
||||
elif df == 17:
|
||||
bds = data["me"].get_type()
|
||||
if bds == 0x05:
|
||||
#return f2m*air_modes.altitude.decode_alt(data["me"]["alt"], False)
|
||||
return 8000
|
||||
else:
|
||||
return None
|
||||
|
||||
if __name__=="__main__":
|
||||
srv = mlat_server("nothin'", 31337)
|
||||
@ -232,6 +247,24 @@ if __name__=="__main__":
|
||||
for rep in reps:
|
||||
print "Report with data %x" % rep["data"]
|
||||
for st in rep["stamps"]:
|
||||
print "Stamp from %s: %f" % (st.addr, st.tofloat())
|
||||
print "Stamp from %s: %f" % (st.clientinfo.name, st.tofloat())
|
||||
srv.prune()
|
||||
|
||||
#now format the reports to get them ready for multilateration
|
||||
#it's expecting a list of tuples [(station[], timestamp)...]
|
||||
#also have to parse the data to pull altitude out of the mix
|
||||
if reps:
|
||||
for rep in reps:
|
||||
alt = get_modes_altitude(air_modes.modes_reply(rep["data"]))
|
||||
if (alt is None and len(rep["stamps"]) > 3) or alt is not None:
|
||||
mlat_list = [(st.clientinfo.position, st.tofloat()) for st in rep["stamps"]]
|
||||
print mlat_list
|
||||
#multilaterate!
|
||||
try:
|
||||
pos = air_modes.mlat.mlat(mlat_list, alt)
|
||||
if pos is not None:
|
||||
print pos
|
||||
except Exception as e:
|
||||
print e
|
||||
|
||||
time.sleep(0.3)
|
||||
|
||||
@ -2,7 +2,8 @@
|
||||
|
||||
#test stuff for mlat server
|
||||
|
||||
import socket, pickle, time, random, sys
|
||||
import socket, pickle, time, random, sys, math, numpy
|
||||
import air_modes
|
||||
|
||||
class rx_data:
|
||||
secs = 0
|
||||
@ -12,17 +13,13 @@ class rx_data:
|
||||
class client_info:
|
||||
def __init__(self):
|
||||
self.name = ""
|
||||
self.latitude = 0.0
|
||||
self.longitude = 0.0
|
||||
self.altitude = 0.0
|
||||
self.position = []
|
||||
self.offset_secs = 0
|
||||
self.offset_frac_secs = 0.0
|
||||
|
||||
info = client_info()
|
||||
info.name = sys.argv[1]
|
||||
info.latitude = float(sys.argv[2])
|
||||
info.longitude = float(sys.argv[3])
|
||||
info.altitude = 123
|
||||
info.position = [float(sys.argv[2]), float(sys.argv[3]), 100]
|
||||
info.offset_secs = 0
|
||||
info.offset_frac_secs = 0.0
|
||||
|
||||
@ -30,15 +27,34 @@ data1 = rx_data()
|
||||
data1.secs = 0
|
||||
data1.data = int("0x8da81f875857f10eb65b10cb66f3", 16)
|
||||
|
||||
ac_starting_pos = [37.617175, -122.400843, 8000]
|
||||
ac_hdg = 130.
|
||||
ac_spd = 0.00008
|
||||
def get_pos(time):
|
||||
return [ac_starting_pos[0] + ac_spd * time * math.cos(ac_hdg*math.pi/180.), \
|
||||
ac_starting_pos[1] + ac_spd * time * math.sin(ac_hdg*math.pi/180.), \
|
||||
ac_starting_pos[2]]
|
||||
|
||||
def get_simulated_timestamp(time, position):
|
||||
return time + numpy.linalg.norm(numpy.array(air_modes.mlat.llh2ecef(position))-numpy.array(air_modes.mlat.llh2geoid(info.position))) / air_modes.mlat.c
|
||||
|
||||
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||||
sock.setblocking(1)
|
||||
sock.connect(("localhost", 31337))
|
||||
sock.send(pickle.dumps(info))
|
||||
print sock.recv(1024)
|
||||
ts = 0.0
|
||||
while 1:
|
||||
time.sleep(0.05)
|
||||
data1.frac_secs = random.random()
|
||||
pos = get_pos(ts)
|
||||
stamp = get_simulated_timestamp(ts, pos)
|
||||
print "Timestamp: %.10f" % (stamp)
|
||||
print "Position: ", pos
|
||||
data1.secs = int(stamp)
|
||||
data1.frac_secs = float(stamp)
|
||||
data1.frac_secs -= int(data1.frac_secs)
|
||||
sock.send(pickle.dumps([data1]))
|
||||
ts+=1
|
||||
time.sleep(1)
|
||||
|
||||
sock.close()
|
||||
sock = None
|
||||
|
||||
@ -96,23 +96,37 @@ c = 299792458 / 1.0003 #modified for refractive index of air, why not
|
||||
#basically 20 meters is way less than the anticipated error of the system so it doesn't make sense to continue
|
||||
#it's possible this could fail in situations where the solution converges slowly
|
||||
#TODO: this fails to converge for some seriously advantageous geometry
|
||||
def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 100):
|
||||
def mlat_iter(rel_stations, nearest, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 100):
|
||||
xerr = [1e9, 1e9, 1e9]
|
||||
rounds = 0
|
||||
actual = numpy.array(llh2ecef([37.617175,-122.400843, testalt]))-nearest #DEBUG
|
||||
while numpy.linalg.norm(xerr) > limit:
|
||||
#get p_i, the estimated pseudoranges based on the latest position guess
|
||||
prange_est = [[numpy.linalg.norm(station - xguess)] for station in rel_stations]
|
||||
#get the difference d_p^ between the observed and calculated pseudoranges
|
||||
dphat = prange_obs - prange_est
|
||||
H = numpy.array([(numpy.array(-rel_stations[row,:])+xguess) / prange_est[row] for row in range(0,len(rel_stations))])
|
||||
#create a matrix of partial differentials to find the slope of the error in X,Y,Z directions
|
||||
H = numpy.array([(numpy.array(-rel_stations[row,:])+xguess) / prange_est[row] for row in range(len(rel_stations))])
|
||||
#now we have H, the Jacobian, and can solve for residual error
|
||||
xerr = numpy.linalg.lstsq(H, dphat)[0].flatten()
|
||||
xguess += xerr
|
||||
#print xguess, xerr
|
||||
print "Estimated position and change: ", xguess, numpy.linalg.norm(xerr)
|
||||
print "Actual error: ", numpy.linalg.norm(xguess - actual)
|
||||
rounds += 1
|
||||
if rounds > maxrounds:
|
||||
raise Exception("Failed to converge!")
|
||||
break
|
||||
return xguess
|
||||
|
||||
#gets the emulated Arne Saknussemm Memorial Radio Station report
|
||||
#here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid
|
||||
#in other words, we say "if the aircraft were directly overhead of me, this is the pseudorange to the center of the earth"
|
||||
#if the dang earth were actually round this wouldn't be an issue
|
||||
#this lets us use the altitude of the mode S reply as info to construct an additional reporting station
|
||||
#i haven't really thought about it but I think the geometry (re: *DOP) of this "station" is pretty lousy
|
||||
#but it lets us solve with 3 stations
|
||||
def get_fake_station(surface_position, altitude):
|
||||
return [numpy.linalg.norm(llh2ecef((surface_position[0], surface_position[1], altitude)))] #use ECEF not geoid since alt is MSL not GPS
|
||||
|
||||
#func mlat:
|
||||
#uses a modified GPS pseudorange solver to locate aircraft by multilateration.
|
||||
#replies is a list of reports, in ([lat, lon, alt], timestamp) format
|
||||
@ -125,32 +139,34 @@ def mlat(replies, altitude):
|
||||
stations = [sorted_reply[0] for sorted_reply in sorted_replies]
|
||||
timestamps = [sorted_reply[1] for sorted_reply in sorted_replies]
|
||||
|
||||
me_llh = stations[0]
|
||||
me = llh2geoid(stations[0])
|
||||
|
||||
nearest_llh = stations[0]
|
||||
nearest_xyz = llh2geoid(stations[0])
|
||||
|
||||
#list of stations in XYZ relative to me
|
||||
rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(me) for station in stations[1:]]
|
||||
rel_stations.append([0,0,0] - numpy.array(me))
|
||||
#list of stations in XYZ relative to the closest station
|
||||
rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(nearest_xyz) for station in stations[1:]]
|
||||
|
||||
#add in a center-of-the-earth station if we have altitude
|
||||
if altitude is not None:
|
||||
rel_stations.append([0,0,0] - numpy.array(nearest_xyz))
|
||||
|
||||
rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array
|
||||
|
||||
#differentiate the timestamps to get TDOA, multiply by c to get pseudorange
|
||||
#get TDOA relative to station 0, multiply by c to get pseudorange
|
||||
prange_obs = [[c*(stamp-timestamps[0])] for stamp in timestamps[1:]]
|
||||
print "Initial pranges: ", prange_obs
|
||||
|
||||
if altitude is not None:
|
||||
prange_obs.append(get_fake_station(stations[0], altitude))
|
||||
altguess = altitude
|
||||
else:
|
||||
altguess = nearest_llh[2]
|
||||
|
||||
#so here we calc the estimated pseudorange to the center of the earth, using station[0] as a reference point for the geoid
|
||||
#in other words, we say "if the aircraft were directly overhead of station[0], this is the prange to the center of the earth"
|
||||
#this is a necessary approximation since we don't know the location of the aircraft yet
|
||||
#if the dang earth were actually round this wouldn't be an issue
|
||||
prange_obs.append( [numpy.linalg.norm(llh2ecef((me_llh[0], me_llh[1], altitude)))] ) #use ECEF not geoid since alt is MSL not GPS
|
||||
prange_obs = numpy.array(prange_obs)
|
||||
|
||||
#xguess = llh2ecef([37.617175,-122.400843, 8000])-numpy.array(me)
|
||||
#xguess = [0,0,0]
|
||||
#start our guess directly overhead, who cares
|
||||
xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
|
||||
|
||||
xyzpos = mlat_iter(rel_stations, prange_obs, xguess)
|
||||
llhpos = ecef2llh(xyzpos+me)
|
||||
#initial guess is atop nearest station
|
||||
xguess = numpy.array(llh2ecef([nearest_llh[0], nearest_llh[1], altguess])) - numpy.array(nearest_xyz)
|
||||
xyzpos = mlat_iter(rel_stations, nearest_xyz, prange_obs, xguess)
|
||||
llhpos = ecef2llh(xyzpos+nearest_xyz)
|
||||
|
||||
#now, we could return llhpos right now and be done with it.
|
||||
#but the assumption we made above, namely that the aircraft is directly above the
|
||||
@ -158,9 +174,11 @@ def mlat(replies, altitude):
|
||||
#so now we solve AGAIN, but this time with the corrected pseudorange of the aircraft altitude
|
||||
#this might not be really useful in practice but the sim shows >50m errors without it
|
||||
#and <4cm errors with it, not that we'll get that close in reality but hey let's do it right
|
||||
prange_obs[-1] = [numpy.linalg.norm(llh2ecef((llhpos[0], llhpos[1], altitude)))]
|
||||
xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
|
||||
llhpos = ecef2llh(xyzpos_corr+me)
|
||||
|
||||
if altitude is not None:
|
||||
prange_obs[-1] = [numpy.linalg.norm(llh2ecef((llhpos[0], llhpos[1], altitude)))]
|
||||
xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
|
||||
llhpos = ecef2llh(xyzpos_corr+nearest_xyz)
|
||||
|
||||
#and now, what the hell, let's try to get dilution of precision data
|
||||
#avec is the unit vector of relative ranges to the aircraft from each of the stations
|
||||
@ -179,18 +197,20 @@ if __name__ == '__main__':
|
||||
testalt = 8000
|
||||
testplane = numpy.array(llh2ecef([37.617175,-122.400843, testalt]))
|
||||
testme = llh2geoid(teststations[0])
|
||||
teststamps = [10,
|
||||
teststamps = [10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[0]))) / c,
|
||||
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[1]))) / c,
|
||||
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[2]))) / c,
|
||||
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[3]))) / c,
|
||||
]
|
||||
|
||||
print teststamps
|
||||
# print teststamps
|
||||
print "Actual pranges: ", sorted([numpy.linalg.norm(testplane - numpy.array(llh2geoid(station))) for station in teststations])
|
||||
|
||||
replies = []
|
||||
for i in range(0, len(teststations)):
|
||||
replies.append((teststations[i], teststamps[i]))
|
||||
ans = mlat(replies, testalt)
|
||||
# print (replies, testalt)
|
||||
ans = mlat(replies, None)
|
||||
error = numpy.linalg.norm(numpy.array(llh2ecef(ans))-numpy.array(testplane))
|
||||
range = numpy.linalg.norm(llh2geoid(ans)-numpy.array(testme))
|
||||
print testplane-testme
|
||||
|
||||
Loading…
Reference in New Issue
Block a user