FGFS-4.1/gr-air-modes
4
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed your mlat bug -- you'd removed the time error column from H and it happened to work with the extra information gathered by having timestamp[0] equal to the originating time -- i.e., zero time offset.

Browse Source 
There's currently a bug in the solver returning near-ground-level reports for aircraft at altitude. Also the Arne Saknussemm station doesn't work.
This commit is contained in:
Nick Foster 2012-12-06 19:17:40 -08:00
parent c7e216bca0
commit 1f0ef143a0
1 changed files with 16 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

38
python/mlat.py
View File

@ -95,27 +95,29 @@ c = 299792458 / 1.0003 #modified for refractive index of air, why not
#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
#TODO: this fails to converge for some seriously advantageous geometry
def mlat_iter(rel_stations, nearest, prange_obs, xguess = [0,0,0], limit = 20, maxrounds = 100):
xerr = [1e9, 1e9, 1e9]
#THIS WORKS PLEASE DON'T MESS WITH IT
def mlat_iter(rel_stations, prange_obs, guess = [0,0,0], limit = 20, maxrounds = 100):
print prange_obs
xerr = [1e9, 1e9, 1e9, 1e9]
rounds = 0
actual = numpy.array(llh2ecef([37.617175,-122.400843, testalt]))-nearest #DEBUG
while numpy.linalg.norm(xerr) > limit:
while numpy.linalg.norm(xerr[:3]) > 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]
prange_est = [[numpy.linalg.norm(station - guess)] for station in rel_stations]
#get the difference d_p^ between the observed and calculated pseudoranges
dphat = prange_obs - prange_est
#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))])
H = numpy.array([(numpy.array(-rel_stations[row,:])+guess) / prange_est[row] for row in range(len(rel_stations))])
H = numpy.append(H, numpy.ones(len(prange_obs)).reshape(len(prange_obs),1), axis=1)
print "H: ", H
#now we have H, the Jacobian, and can solve for residual error
xerr = numpy.linalg.lstsq(H, dphat)[0].flatten()
xguess += xerr
print "Estimated position and change: ", xguess, numpy.linalg.norm(xerr)
print "Actual error: ", numpy.linalg.norm(xguess - actual)
print "xerr: ", xerr
guess += xerr[:3] #we ignore the time error for xguess
print "Estimated position and change: ", guess, numpy.linalg.norm(xerr[:3])
rounds += 1
if rounds > maxrounds:
raise Exception("Failed to converge!")
return xguess
return guess
#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
@ -164,8 +166,8 @@ def mlat(replies, altitude):
prange_obs = numpy.array(prange_obs)
#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)
#xguess = numpy.array(llh2ecef([nearest_llh[0], nearest_llh[1], altguess])) - numpy.array(nearest_xyz)
xyzpos = mlat_iter(rel_stations, prange_obs)
llhpos = ecef2llh(xyzpos+nearest_xyz)
#now, we could return llhpos right now and be done with it.
@ -180,14 +182,6 @@ def mlat(replies, 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
# 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
@ -203,13 +197,11 @@ if __name__ == '__main__':
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[3]))) / c,
]
# 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]))
# 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))