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

Move mlat test data to mlat-test.py instead of mlat.py

Browse Source
This commit is contained in:
Nick Foster 2011-07-14 17:01:43 -07:00
parent 5f2a41f648
commit d7e153d281
2 changed files with 16 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/python/mlat-test.py
View File

@ -2,12 +2,22 @@
import mlat
import numpy
replies = []
for i in range(0, len(mlat.teststations)):
replies.append((mlat.teststations[i], mlat.teststamps[i]))
#here's some test data to validate the algorithm
teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.585085, -121.986395, 100]]
testalt = 8000
testplane = numpy.array(mlat.llh2ecef([37.617175,-122.380843, testalt]))
testme = mlat.llh2geoid(teststations[0])
teststamps = [10,
10 + numpy.linalg.norm(testplane-numpy.array(mlat.llh2geoid(teststations[1]))) / mlat.c,
10 + numpy.linalg.norm(testplane-numpy.array(mlat.llh2geoid(teststations[2]))) / mlat.c,
]
ans = mlat.mlat(replies, mlat.testalt)
error = numpy.linalg.norm(numpy.array(mlat.llh2ecef(ans))-numpy.array(mlat.testplane))
range = numpy.linalg.norm(mlat.llh2geoid(ans)-numpy.array(mlat.llh2geoid(mlat.teststations[0])))
replies = []
for i in range(0, len(teststations)):
replies.append((teststations[i], teststamps[i]))
ans = mlat.mlat(replies, testalt)
error = numpy.linalg.norm(numpy.array(mlat.llh2ecef(ans))-numpy.array(testplane))
range = numpy.linalg.norm(mlat.llh2geoid(ans)-numpy.array(mlat.llh2geoid(teststations[0])))
print "Error: %.2fm" % (error)
print "Range: %.2fkm (from first station in list)" % (range/1000)

11
src/python/mlat.py
View File

@ -147,16 +147,6 @@ def llh2geoid((lat, lon, alt)):
c = 299792458 / 1.0003 #modified for refractive index of air, why not
#here's some test data to validate the algorithm
teststations = [[37.76225, -122.44254, 100], [37.409044, -122.077748, 100], [37.585085, -121.986395, 100]]
testalt = 8000
testplane = numpy.array(llh2ecef([37.617175,-122.380843, testalt]))
testme = llh2geoid(teststations[0])
teststamps = [10,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[1]))) / c,
10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[2]))) / c,
]
#this function is the iterative solver core of the mlat function below
#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
@ -174,7 +164,6 @@ 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.linalg.lstsq(H, dphat)[0].flatten() #let's not get crazy here
xguess += xerr
rounds += 1