From e3ac472a1a9e20f826a73ba305f1c82668ccb8c8 Mon Sep 17 00:00:00 2001 From: Nick Foster Date: Mon, 18 Jun 2012 18:43:53 -0700 Subject: [PATCH] Move multilateration test code into mlat.py --- python/mlat-test.py | 27 --------------------------- python/mlat.py | 26 ++++++++++++++++++++++++++ 2 files changed, 26 insertions(+), 27 deletions(-) delete mode 100755 python/mlat-test.py diff --git a/python/mlat-test.py b/python/mlat-test.py deleted file mode 100755 index 042e104..0000000 --- a/python/mlat-test.py +++ /dev/null @@ -1,27 +0,0 @@ -#!/usr/bin/python -import mlat -import numpy - -#here's some test data to validate the algorithm -teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]] -testalt = 8000 -testplane = numpy.array(mlat.llh2ecef([37.617175,-122.400843, 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, - 10 + numpy.linalg.norm(testplane-numpy.array(mlat.llh2geoid(teststations[3]))) / mlat.c, - ] - -print teststamps - -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(testme)) -print testplane-testme -print ans -print "Error: %.2fm" % (error) -print "Range: %.2fkm (from first station in list)" % (range/1000) diff --git a/python/mlat.py b/python/mlat.py index 7cffc9b..7c79a01 100755 --- a/python/mlat.py +++ b/python/mlat.py @@ -171,3 +171,29 @@ def mlat(replies, altitude): #the diagonal elements of doparray will be the x, y, z DOPs. return llhpos + + +if __name__ == '__main__': + #here's some test data to validate the algorithm + teststations = [[37.76225, -122.44254, 100], [37.680016,-121.772461, 100], [37.385844,-122.083082, 100], [37.701207,-122.309418, 100]] + testalt = 8000 + testplane = numpy.array(llh2ecef([37.617175,-122.400843, 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, + 10 + numpy.linalg.norm(testplane-numpy.array(llh2geoid(teststations[3]))) / c, + ] + + print teststamps + + replies = [] + for i in range(0, len(teststations)): + replies.append((teststations[i], teststamps[i])) + ans = mlat(replies, testalt) + error = numpy.linalg.norm(numpy.array(llh2ecef(ans))-numpy.array(testplane)) + range = numpy.linalg.norm(llh2geoid(ans)-numpy.array(testme)) + print testplane-testme + print ans + print "Error: %.2fm" % (error) + print "Range: %.2fkm (from first station in list)" % (range/1000)