diff --git a/apps/mlat_server.py b/apps/mlat_server.py index 8a758bb..e7213f0 100755 --- a/apps/mlat_server.py +++ b/apps/mlat_server.py @@ -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 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) diff --git a/apps/mlat_test.py b/apps/mlat_test.py index 5639196..26691aa 100755 --- a/apps/mlat_test.py +++ b/apps/mlat_test.py @@ -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 diff --git a/python/mlat.py b/python/mlat.py index 7c79a01..82087bd 100755 --- a/python/mlat.py +++ b/python/mlat.py @@ -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