mlat test now creates a cheesy moving simulated aircraft. mlat is broken though due to incorrect assumptions in the solver.

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 = 0
-#pickle_prot = pickle.HIGHEST_PROTOCOL
+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):
+    def __init__(self, clientinfo, secs, frac_secs):
-        self.addr = addr
+        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.position = []
-        self.longitude = 0.0
-        self.altitude = 0.0
         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:
+            try:
-            msglist = pickle.loads(pkt)
+                msglist = pickle.loads(pkt)
-            for msg in msglist:
+                for msg in msglist:
-                #DEBUG: change conn.clientinfo.name to conn.addr for production
+                    st = stamp(conn.clientinfo, msg.secs, msg.frac_secs)
-                st = stamp(conn.clientinfo.name, msg.secs, msg.frac_secs)
+                    if msg.data not in self._reports:
-                if msg.data not in self._reports:
+                        self._reports[msg.data] = []
-                    self._reports[msg.data] = []
 
-                #ordered insert
+                    #ordered insert
-                ordered_insert(self._reports[msg.data], st)
+                    ordered_insert(self._reports[msg.data], st)
-                self._lastreport = st.tofloat()
+                    if st.tofloat() > self._lastreport:
-#            for report in self._reports.values():
+                        self._lastreport = st.tofloat()
-#                for st in report:
-#                    print st.addr, st.secs, st.frac_secs
 
-            #except Exception as e:
+            except Exception as e:
-            #    print "Invalid message from %s: %s" % (conn.addr, pkt)
+                print "Invalid message from %s: %s" % (conn.addr, pkt)
-            #    print e
+                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
+    #not really tested well
-    #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
     def prune(self):
-        for report in self._reports:
+        for data, stamps in self._reports.iteritems():
-            if self._reports[report][-1].tofloat() - self._lastreport > 5:
+            #check the last stamp first so we don't iterate over
-                self._reports.remove(report)
+            #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)

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.position = []
-        self.longitude = 0.0
-        self.altitude = 0.0
         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.position = [float(sys.argv[2]), float(sys.argv[3]), 100]
-info.longitude = float(sys.argv[3])
-info.altitude = 123
 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)
+    pos = get_pos(ts)
-    data1.frac_secs = random.random()
+    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

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]
+    nearest_llh = stations[0]
-    me = llh2geoid(stations[0])
+    nearest_xyz = llh2geoid(stations[0])
-
     
-    #list of stations in XYZ relative to me
+    #list of stations in XYZ relative to the closest station
-    rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(me) for station in stations[1:]]
+    rel_stations = [numpy.array(llh2geoid(station)) - numpy.array(nearest_xyz) for station in stations[1:]]
-    rel_stations.append([0,0,0] - numpy.array(me))
+
+    #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)
+    #initial guess is atop nearest station
-    #xguess = [0,0,0]
+    xguess = numpy.array(llh2ecef([nearest_llh[0], nearest_llh[1], altguess])) - numpy.array(nearest_xyz)
-    #start our guess directly overhead, who cares
+    xyzpos = mlat_iter(rel_stations, nearest_xyz, prange_obs, xguess)
-    xguess = numpy.array(llh2ecef([me_llh[0], me_llh[1], altitude])) - numpy.array(me)
+    llhpos = ecef2llh(xyzpos+nearest_xyz)
-    
-    xyzpos = mlat_iter(rel_stations, prange_obs, xguess)
-    llhpos = ecef2llh(xyzpos+me)
     
     #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
+    if altitude is not None:
-    llhpos = ecef2llh(xyzpos_corr+me)
+        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