You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
132 lines
6.1 KiB
132 lines
6.1 KiB
#!/usr/bin/env python
|
|
from air_modes import modes_parse, mlat
|
|
import numpy
|
|
import sys
|
|
|
|
#sffile = open("27augsf3.txt")
|
|
#rudifile = open("27augrudi3.txt")
|
|
|
|
#sfoutfile = open("sfout.txt", "w")
|
|
#rudioutfile = open("rudiout.txt", "w")
|
|
|
|
sfparse = modes_parse.modes_parse([37.762236,-122.442525])
|
|
|
|
sf_station = [37.762236,-122.442525, 100]
|
|
mv_station = [37.409348,-122.07732, 100]
|
|
bk_station = [37.854246, -122.266701, 100]
|
|
|
|
raw_stamps = []
|
|
|
|
#first iterate through both files to find the estimated time difference. doesn't have to be accurate to more than 1ms or so.
|
|
#to do this, look for type 17 position packets with the same data. assume they're unique. print the tdiff.
|
|
|
|
#collect a list of raw timestamps for each aircraft from each station
|
|
#the raw stamps have to be processed into corrected stamps OR distance has to be included in each
|
|
#then postprocess to find clock delay for each and determine drift rate for each aircraft separately
|
|
#then come up with an average clock drift rate
|
|
#then find an average drift-corrected clock delay
|
|
#then find rms error
|
|
|
|
#ok so get [ICAO, [raw stamps], [distance]] for each matched record
|
|
|
|
files = [open(arg) for arg in sys.argv[1:]]
|
|
|
|
#files = [sffile, rudifile]
|
|
stations = [sf_station, mv_station]#, bk_station]
|
|
|
|
records = []
|
|
|
|
for each_file in files:
|
|
recordlist = []
|
|
for line in each_file:
|
|
[msgtype, shortdata, longdata, parity, ecc, reference, timestamp] = line.split()
|
|
recordlist.append({"data": {"msgtype": long(msgtype, 10),\
|
|
"shortdata": long(shortdata, 16),\
|
|
"longdata": long(longdata, 16),\
|
|
"parity": long(parity, 16),\
|
|
"ecc": long(ecc, 16)},
|
|
"time": float(timestamp)\
|
|
})
|
|
records.append(recordlist)
|
|
|
|
#ok now we have records parsed into something usable that we can == with
|
|
|
|
def feet_to_meters(feet):
|
|
return feet * 0.3048006096012
|
|
|
|
all_heard = []
|
|
#gather list of reports which were heard by all stations
|
|
for station0_report in records[0]: #iterate over list of reports from station 0
|
|
for other_reports in records[1:]:
|
|
stamps = [station0_report["time"]]
|
|
stamp = [report["time"] for report in other_reports if report["data"] == station0_report["data"]]# for other_reports in records[1:]]
|
|
if len(stamp) > 0:
|
|
stamps.append(stamp[0])
|
|
if len(stamps) == len(records): #found same report in all records
|
|
all_heard.append({"data": station0_report["data"], "times": stamps})
|
|
|
|
#print all_heard
|
|
|
|
#ok, now let's pull out the location-bearing packets so we can find our time offset
|
|
position_reports = [x for x in all_heard if x["data"]["msgtype"] == 17 and 9 <= (x["data"]["longdata"] >> 51) & 0x1F <= 18]
|
|
offset_list = []
|
|
#there's probably a way to list-comprehension-ify this but it looks hard
|
|
for msg in position_reports:
|
|
data = msg["data"]
|
|
[alt, lat, lon, rng, bearing] = sfparse.parseBDS05(data["shortdata"], data["longdata"], data["parity"], data["ecc"])
|
|
ac_pos = [lat, lon, feet_to_meters(alt)]
|
|
rel_times = []
|
|
for time, station in zip(msg["times"], stations):
|
|
#here we get the estimated time at the aircraft when it transmitted
|
|
range_to_ac = numpy.linalg.norm(numpy.array(mlat.llh2ecef(station))-numpy.array(mlat.llh2ecef(ac_pos)))
|
|
timestamp_at_ac = time - range_to_ac / mlat.c
|
|
rel_times.append(timestamp_at_ac)
|
|
offset_list.append({"aircraft": data["shortdata"] & 0xffffff, "times": rel_times})
|
|
|
|
#this is a list of unique aircraft, heard by all stations, which transmitted position packets
|
|
#we do drift calcs separately for each aircraft in the set because mixing them seems to screw things up
|
|
#i haven't really sat down and figured out why that is yet
|
|
unique_aircraft = list(set([x["aircraft"] for x in offset_list]))
|
|
print "Aircraft heard for clock drift estimate: %s" % [str("%x" % ac) for ac in unique_aircraft]
|
|
print "Total reports used: %d over %.2f seconds" % (len(position_reports), position_reports[-1]["times"][0]-position_reports[0]["times"][0])
|
|
|
|
#get a list of reported times gathered by the unique aircraft that transmitted them
|
|
#abs_unique_times = [report["times"] for ac in unique_aircraft for report in offset_list if report["aircraft"] == ac]
|
|
#print abs_unique_times
|
|
#todo: the below can probably be done cleaner with nested list comprehensions
|
|
clock_rate_corrections = [0]
|
|
for i in range(1,len(stations)):
|
|
drift_error_limited = []
|
|
for ac in unique_aircraft:
|
|
times = [report["times"] for report in offset_list if report["aircraft"] == ac]
|
|
|
|
s0_times = [report[0] for report in times]
|
|
rel_times = [report[i]-report[0] for report in times]
|
|
|
|
#find drift error rate
|
|
drift_error = [(y-x)/(b-a) for x,y,a,b in zip(rel_times, rel_times[1:], s0_times[0:], s0_times[1:])]
|
|
drift_error_limited.append([x for x in drift_error if abs(x) < 1e-5])
|
|
|
|
#flatten the list of lists (tacky, there's a better way)
|
|
drift_error_limited = [x for sublist in drift_error_limited for x in sublist]
|
|
clock_rate_corrections.append(0-numpy.mean(drift_error_limited))
|
|
|
|
for i in range(len(clock_rate_corrections)):
|
|
print "drift from %d relative to station 0: %.3fppm" % (i, clock_rate_corrections[i] * 1e6)
|
|
|
|
#let's get the average clock offset (based on drift-corrected, TDOA-corrected derived timestamps)
|
|
clock_offsets = [[numpy.mean([x["times"][i]*(1+clock_rate_corrections[i])-x["times"][0] for x in offset_list])][0] for i in range(0,len(stations))]
|
|
for i in range(len(clock_offsets)):
|
|
print "mean offset from %d relative to station 0: %.3f seconds" % (i, clock_offsets[i])
|
|
|
|
#for the two-station case, let's now go back, armed with our clock drift and offset, and get the variance between expected and observed timestamps
|
|
error_list = []
|
|
for i in range(1,len(stations)):
|
|
for report in offset_list:
|
|
error = abs(((report["times"][i]*(1+clock_rate_corrections[i]) - report["times"][0]) - clock_offsets[i]) * mlat.c)
|
|
error_list.append(error)
|
|
#print error
|
|
|
|
rms_error = (numpy.mean([error**2 for error in error_list]))**0.5
|
|
print "RMS error in TDOA: %.1f meters" % rms_error
|