From 5f2a41f6480e3188e071acfb3bec2e314623f7ce Mon Sep 17 00:00:00 2001
From: Nick Foster <nick@ettus.com>
Date: Wed, 13 Jul 2011 00:10:26 -0700
Subject: [PATCH] mlat: improvements/simplifications to solver, basic DOP
 pseudocode

---
 src/python/mlat.py | 29 +++++++++++++++++++++--------
 1 file changed, 21 insertions(+), 8 deletions(-)

diff --git a/src/python/mlat.py b/src/python/mlat.py
index d8b368a..c410d83 100755
--- a/src/python/mlat.py
+++ b/src/python/mlat.py
@@ -4,6 +4,11 @@ import numpy
 from scipy.ndimage import map_coordinates
 
 #functions for multilateration.
+#this library is more or less based around the so-called "GPS equation", the canonical
+#iterative method for getting position from GPS satellite time difference of arrival data.
+#here, instead of multiple orbiting satellites with known time reference and position,
+#we have multiple fixed stations with known time references (GPSDO, hopefully) and known
+#locations (again, GPSDO).
 
 #NB: because of the way this solver works, at least 3 stations and timestamps
 #are required. this function will not return hyperbolae for underconstrained systems.
@@ -156,8 +161,6 @@ teststamps   = [10,
 #we use limit as a goal to stop solving when we get "close enough" (error magnitude in meters for that iteration)
 #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
-#because the change in ERROR is not necessarily the error itself
-#still, it should converge quickly when close, so it SHOULDN'T give more than 2-3x the precision in total error
 def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 50):
     xerr = [1e9, 1e9, 1e9]
     rounds = 0
@@ -171,7 +174,8 @@ def mlat_iter(rel_stations, prange_obs, xguess = [0,0,0], limit = 20, maxrounds
             H.append((numpy.array(-rel_stations[row,:])-xguess) / prange_est[row])
         H = numpy.array(H)
         #now we have H, the Jacobian, and can solve for residual error
-        xerr = numpy.dot(numpy.linalg.solve(numpy.dot(H.T,H), H.T), dphat).flatten()
+        #xerr = numpy.dot(numpy.linalg.solve(numpy.dot(H.T,H), H.T), dphat).flatten()
+        xerr = numpy.linalg.lstsq(H, dphat)[0].flatten() #let's not get crazy here
         xguess += xerr
         rounds += 1
         if rounds > maxrounds:
@@ -191,10 +195,10 @@ def mlat(replies, altitude):
     stations = []
     timestamps = []
     #i really couldn't figure out how to unpack this, sorry
-    for i in range(0, len(replies)):
-        stations.append( sorted_replies[i][0] )
-        timestamps.append( sorted_replies[i][1] )
-    
+    for sorted_reply in sorted_replies:
+        stations.append(sorted_reply[0])
+        timestamps.append(sorted_reply[1])
+
     me_llh = stations[0]
     me = llh2geoid(stations[0])
     
@@ -203,7 +207,8 @@ def mlat(replies, altitude):
         rel_stations.append(numpy.array(llh2geoid(station)) - numpy.array(me))
     rel_stations.append([0,0,0]-numpy.array(me)) #arne saknussemm, reporting in
     rel_stations = numpy.array(rel_stations) #convert list of arrays to 2d array
-    
+
+    #differentiate the timestamps to get TDOA
     tdoa = []
     for stamp in timestamps[1:]:
         tdoa.append(stamp - timestamps[0])
@@ -231,5 +236,13 @@ def mlat(replies, altitude):
     
     xyzpos_corr = mlat_iter(rel_stations, prange_obs, xyzpos) #start off with a really close guess
     llhpos = ecef2llh(xyzpos_corr+me)
+
+    #and now, what the hell, let's try to get dilution of precision data
+    #avec is the vector of relative ranges to the aircraft from each of the stations
+#    for i in range(len(avec)):
+#        avec[i] = numpy.array(avec[i]) / numpy.linalg.norm(numpy.array(avec[i]))
+#    numpy.append(avec, [[-1],[-1],[-1],[-1]], 1) #must be # of stations
+#    doparray = numpy.linalg.inv(avec.T*avec)
+#the diagonal elements of doparray will be the x, y, z DOPs.
     
     return llhpos