def report_mlat_position_old(self, receiver,
receive_timestamp, address, ecef, ecef_cov, receivers, distinct,
dof, kalman_state, result_new_old):
# old client, use the old format (somewhat incomplete)
if result_new_old[1]:
self.send(result=result_new_old[1])
return
lat, lon, alt = geodesy.ecef2llh(ecef)
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
result = {'@': round(receive_timestamp, 3),
'addr': '{0:06x}'.format(address),
'lat': round(lat, 5),
'lon': round(lon, 5),
'alt': round(alt * constants.MTOF, 0),
'callsign': callsign,
'squawk': squawk,
'hdop': 0.0,
'vdop': 0.0,
'tdop': 0.0,
'gdop': 0.0,
'nstations': len(receivers)}
result_new_old[1] = result
self.send(result=result)
def _update_derived(self):
"""Update derived values from self._mean and self._cov"""
self.position = self._mean[0:3]
self.velocity = self._mean[3:6]
pe = numpy.trace(self._cov[0:3, 0:3])
self.position_error = 1e6 if pe < 0 else math.sqrt(pe)
ve = numpy.trace(self._cov[3:6, 3:6])
self.velocity_error = 1e6 if ve < 0 else math.sqrt(ve)
lat, lon, alt = self.position_llh = geodesy.ecef2llh(self.position)
# rotate velocity into the local tangent plane
lat_r = lat * constants.DTOR
lon_r = lon * constants.DTOR
C = numpy.array([[-math.sin(lon_r), math.cos(lon_r), 0],
[math.sin(-lat_r) * math.cos(lon_r), math.sin(-lat_r) * math.sin(lon_r), math.cos(-lat_r)],
[math.cos(-lat_r) * math.cos(lon_r), math.cos(-lat_r) * math.sin(lon_r), -math.sin(-lat_r)]])
east, north, up = self.velocity_enu = numpy.dot(C, self.velocity.T).T
# extract speeds, headings
self.heading = math.atan2(east, north) * 180.0 / math.pi
if self.heading < 0:
self.heading += 360
self.ground_speed = math.sqrt(north**2 + east**2)
self.vertical_speed = up
self.valid = True
def _update_derived(self):
"""Update derived values from self._mean and self._cov"""
self.position = self._mean[0:3]
self.velocity = self._mean[3:6]
pe = numpy.trace(self._cov[0:3, 0:3])
self.position_error = 1e6 if pe < 0 else math.sqrt(pe)
ve = numpy.trace(self._cov[3:6, 3:6])
self.velocity_error = 1e6 if ve < 0 else math.sqrt(ve)
lat, lon, alt = self.position_llh = geodesy.ecef2llh(self.position)
# rotate velocity into the local tangent plane
lat_r = lat * constants.DTOR
lon_r = lon * constants.DTOR
C = numpy.array([[-math.sin(lon_r), math.cos(lon_r), 0],
[math.sin(-lat_r) * math.cos(lon_r), math.sin(-lat_r) * math.sin(lon_r), math.cos(-lat_r)],
[math.cos(-lat_r) * math.cos(lon_r), math.cos(-lat_r) * math.sin(lon_r), -math.sin(-lat_r)]])
east, north, up = self.velocity_enu = numpy.dot(C, self.velocity.T).T
# extract speeds, headings
self.heading = math.atan2(east, north) * 180.0 / math.pi
if self.heading < 0:
self.heading += 360
self.ground_speed = math.sqrt(north**2 + east**2)
self.vertical_speed = up
self.valid = True
def write_result(self, receive_timestamp, address, ecef, ecef_cov, receivers, distinct, dof, kalman_state):
try:
lat, lon, alt = geodesy.ecef2llh(ecef)
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
if ecef_cov is None:
err_est = -1
else:
var_est = numpy.sum(numpy.diagonal(ecef_cov))
if var_est >= 0:
err_est = math.sqrt(var_est)
else:
err_est = -1
if kalman_state.valid and kalman_state.last_update >= receive_timestamp:
line = self.KTEMPLATE.format(
t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=alt * constants.MTOF,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join([receiver.uuid for receiver in receivers])),
klat=kalman_state.position_llh[0],
klon=kalman_state.position_llh[1],
kalt=kalman_state.position_llh[2] * constants.MTOF,
kheading=kalman_state.heading,
kspeed=kalman_state.ground_speed * constants.MS_TO_KTS,
kvrate=kalman_state.vertical_speed * constants.MS_TO_FPM,
kerr=kalman_state.position_error)
else:
line = self.TEMPLATE.format(
t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=alt * constants.MTOF,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join([receiver.uuid for receiver in receivers])))
self.f.write(line)
except Exception:
self.logger.exception("Failed to write result")
def write_result(self, receive_timestamp, address, ecef, ecef_cov,
receivers, distinct, dof, kalman_data):
try:
if self.use_kalman_data:
if not kalman_data.valid or kalman_data.last_update < receive_timestamp:
return
lat, lon, alt = kalman_data.position_llh
speed = int(
round(kalman_data.ground_speed * constants.MS_TO_KTS))
heading = int(round(kalman_data.heading))
vrate = int(
round(kalman_data.vertical_speed * constants.MS_TO_FPM))
else:
lat, lon, alt = geodesy.ecef2llh(ecef)
speed = ''
heading = ''
vrate = ''
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
altitude = int(round(alt * constants.MTOF))
send_timestamp = time.time()
if ac.altitude is None:
vrate = ''
line = self.TEMPLATE.format(
mtype=3,
addr=address,
rcv_date=format_date(receive_timestamp),
rcv_time=format_time(receive_timestamp),
now_date=format_date(send_timestamp),
now_time=format_time(send_timestamp),
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=round(lat, 6),
lon=round(lon, 6),
altitude=altitude,
speed=speed,
heading=heading,
vrate=vrate,
fs='',
emerg='',
ident='',
aog='')
self.writer.write(line.encode('ascii'))
self.last_output = time.monotonic()
except Exception:
self.logger.exception("Failed to write result")
def write_result(self, receive_timestamp, address, ecef, ecef_cov, receivers, distinct, dof, kalman_data):
try:
if self.use_kalman_data:
if not kalman_data.valid or kalman_data.last_update < receive_timestamp:
return
lat, lon, alt = kalman_data.position_llh
speed = int(round(kalman_data.ground_speed * constants.MS_TO_KTS))
heading = int(round(kalman_data.heading))
vrate = int(round(kalman_data.vertical_speed * constants.MS_TO_FPM))
else:
lat, lon, alt = geodesy.ecef2llh(ecef)
speed = ''
heading = ''
vrate = ''
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
altitude = int(round(alt * constants.MTOF))
send_timestamp = time.time()
line = self.TEMPLATE.format(mtype=3,
addr=address,
rcv_date=format_date(receive_timestamp),
rcv_time=format_time(receive_timestamp),
now_date=format_date(send_timestamp),
now_time=format_time(send_timestamp),
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=round(lat, 4),
lon=round(lon, 4),
altitude=altitude,
speed=speed,
heading=heading,
vrate=vrate,
fs='',
emerg='',
ident='',
aog='')
self.writer.write(line.encode('ascii'))
self.last_output = time.monotonic()
except Exception:
self.logger.exception("Failed to write result")
def _residuals(x_guess, pseudorange_data, altitude, altitude_error):
"""Return an array of residuals for a position guess at x_guess versus
actual measurements pseudorange_data and altitude."""
(*position_guess, offset) = x_guess
res = []
# compute pseudoranges at the current guess vs. measured pseudorange
for receiver_position, pseudorange, error in pseudorange_data:
pseudorange_guess = geodesy.ecef_distance(receiver_position, position_guess) - offset
res.append((pseudorange - pseudorange_guess) / error)
# compute altitude at the current guess vs. measured altitude
if altitude is not None:
_, _, altitude_guess = geodesy.ecef2llh(position_guess)
res.append((altitude - altitude_guess) / altitude_error)
return res
def report_mlat_position_old(self, receiver,
receive_timestamp, address, ecef, ecef_cov, receivers, distinct,
dof, kalman_state):
# old client, use the old format (somewhat incomplete)
lat, lon, alt = geodesy.ecef2llh(ecef)
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
self.send(result={'@': round(receive_timestamp, 3),
'addr': '{0:06x}'.format(address),
'lat': round(lat, 4),
'lon': round(lon, 4),
'alt': round(alt * constants.MTOF, 0),
'callsign': callsign,
'squawk': squawk,
'hdop': 0.0,
'vdop': 0.0,
'tdop': 0.0,
'gdop': 0.0,
'nstations': len(receivers)})
def observation_function_with_altitude(self, state, *, positions):
"""Kalman filter observation function.
Given state (position,...) and a list of N receiver positions,
return an altitude observation and N-1 pseudorange observations; the
pseudoranges are relative to the first receiver's pseudorange."""
x, y, z = state[0:3]
n = len(positions)
obs = numpy.zeros(n)
_, _, obs[0] = geodesy.ecef2llh((x, y, z))
rx, ry, rz = positions[0]
zero_range = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5
for i in range(1, n):
rx, ry, rz = positions[i]
obs[i] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range
return obs
def observation_function_with_altitude(self, state, *, positions):
"""Kalman filter observation function.
Given state (position,...) and a list of N receiver positions,
return an altitude observation and N-1 pseudorange observations; the
pseudoranges are relative to the first receiver's pseudorange."""
x, y, z = state[0:3]
n = len(positions)
obs = numpy.zeros(n)
_, _, obs[0] = geodesy.ecef2llh((x, y, z))
rx, ry, rz = positions[0]
zero_range = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5
for i in range(1, n):
rx, ry, rz = positions[i]
obs[i] = ((rx - x)**2 + (ry - y)**2 + (rz - z)**2)**0.5 - zero_range
return obs
def solve(measurements, altitude, altitude_error, initial_guess):
"""Given a set of receive timestamps, multilaterate the position of the transmitter.
measurements: a list of (receiver, timestamp, error) tuples. Should be sorted by timestamp.
receiver.position should be the ECEF position of the receiver
timestamp should be a reception time in seconds (with an arbitrary epoch)
variance should be the estimated variance of timestamp
altitude: the reported altitude of the transmitter in _meters_, or None
altitude_error: the estimated error in altitude in meters, or None
initial_guess: an ECEF position to start the solver from
Returns None on failure, or (ecef, ecef_cov) on success, with:
ecef: the multilaterated ECEF position of the transmitter
ecef_cov: an estimate of the covariance matrix of ecef
"""
if len(measurements) + (0 if altitude is None else 1) < 4:
raise ValueError('Not enough measurements available')
glat, glng, galt = geodesy.ecef2llh(initial_guess)
if galt < config.MIN_ALT:
galt = -galt
initial_guess = geodesy.llh2ecef([glat, glng, galt])
if galt > config.MAX_ALT:
galt = config.MAX_ALT
initial_guess = geodesy.llh2ecef([glat, glng, galt])
base_timestamp = measurements[0][1]
pseudorange_data = [(receiver.position,
(timestamp - base_timestamp) * constants.Cair,
math.sqrt(variance) * constants.Cair)
for receiver, timestamp, variance in measurements]
x_guess = [initial_guess[0], initial_guess[1], initial_guess[2], 0.0]
x_est, cov_x, infodict, mesg, ler = scipy.optimize.leastsq(
_residuals,
x_guess,
args=(pseudorange_data, altitude, altitude_error),
full_output=True,
maxfev=config.SOLVER_MAXFEV)
if ler in (1, 2, 3, 4):
#glogger.info("solver success: {0} {1}".format(ler, mesg))
# Solver found a result. Validate that it makes
# some sort of physical sense.
(*position_est, offset_est) = x_est
if offset_est < 0 or offset_est > config.MAX_RANGE:
#glogger.info("solver: bad offset: {0}".formaT(offset_est))
# implausible range offset to closest receiver
return None
for receiver, timestamp, variance in measurements:
d = geodesy.ecef_distance(receiver.position, position_est)
if d > config.MAX_RANGE:
# too far from this receiver
#glogger.info("solver: bad range: {0}".format(d))
return None
if cov_x is None:
return position_est, None
else:
return position_est, cov_x[0:3, 0:3]
else:
# Solver failed
#glogger.info("solver: failed: {0} {1}".format(ler, mesg))
return None
def _write_state(self):
aircraft_state = {}
ac_count_mlat = len(self.tracker.mlat_wanted)
ac_count_sync = 0
now = time.time()
for ac in self.tracker.aircraft.values():
elapsed_seen = now - ac.seen
#if elapsed_seen > 3 * 3600:
# continue
icao_string = '{0:06X}'.format(ac.icao)
s = {}
aircraft_state[icao_string] = s
s['icao'] = icao_string
s['elapsed_seen'] = round(elapsed_seen, 1)
s['interesting'] = 1 if ac.interesting else 0
s['allow_mlat'] = 1 if ac.allow_mlat else 0
s['tracking'] = len(ac.tracking)
s['sync_interest'] = len(ac.sync_interest)
s['mlat_interest'] = len(ac.mlat_interest)
s['adsb_seen'] = len(ac.adsb_seen)
s['mlat_message_count'] = ac.mlat_message_count
s['mlat_result_count'] = ac.mlat_result_count
s['mlat_kalman_count'] = ac.mlat_kalman_count
sync_count = round(ac.sync_good + ac.sync_bad)
s['sync_count_1min'] = sync_count
sync_bad_percent = round(100 * ac.sync_bad / (sync_count + 0.01), 1)
s['sync_bad_percent'] = sync_bad_percent
ac.sync_bad_percent = sync_bad_percent
if ac.sync_bad > 3 and sync_bad_percent > 15:
ac.sync_dont_use = 1
else:
ac.sync_dont_use = 0
ac.sync_good *= 0.8
ac.sync_bad *= 0.8
if ac.last_result_time is not None:
s['last_result'] = round(now - ac.last_result_time, 1)
lat, lon, alt = geodesy.ecef2llh(ac.last_result_position)
s['lat'] = round(lat, 4)
s['lon'] = round(lon, 4)
alt = ac.altitude
if alt is not None:
s['alt'] = round(alt)
if ac.kalman.valid:
s['heading'] = round(ac.kalman.heading, 0)
s['speed'] = round(ac.kalman.ground_speed, 0)
if elapsed_seen > 600:
s['tracking_receivers'] = [receiver.uid for receiver in ac.tracking]
if ac.sync_interest:
ac_count_sync += 1
sync = {}
clients = {}
receiver_states = self.clock_tracker.dump_receiver_state()
bad_receivers = 0
outlier_sum = 0
sync_sum = 0
# blacklist receivers with bad clock
# note this section of code runs every 15 seconds
for r in self.receivers.values():
bad_peers = 0
# count how many peers we have bad sync with
# don't count peers who have been timed out (state[3] > 0)
# 1.5 microseconds error or more are considered a bad sync (state[1] > 3)
num_peers = 8
# start with 8 peers extra, so low peer receivers
# aren't timed out by the percentage threshold
# of bad_peers as easily.
# iterate over sync state with all peers
# state = [ 0: pairing sync count, 1: offset, 2: drift,
# 3: bad_syncs, 4: pairing.jumped]
peers = receiver_states.get(r.user, {})
bad_peer_list = []
for username, state in peers.items():
if state[3] > 0:
# skip peers which have bad sync
continue
num_peers += 1
if state[4] or (state[0] > 10 and state[1] > 1.2) or (state[0] > 3 and state[1] > 1.8) or state[1] > 2.4:
bad_peers += 1
bad_peer_list.append(username)
outlier_sum += r.num_outliers
sync_sum += r.num_syncs
outlier_percent = 100 * r.num_outliers / (r.num_syncs + 0.1)
# running average for the outlier percent
r.outlier_percent_rolling -= 0.1 * (r.outlier_percent_rolling - outlier_percent)
# If your sync with more than 10 percent of peers is bad,
# it's likely you are the reason.
# You get 0.5 to 2 to your bad_sync score and timed out.
if bad_peers/num_peers > 0.15:
r.bad_syncs += min(0.5, 2*bad_peers/num_peers) + 0.1
outlier_percent_limit = 12
if r.num_syncs > 100 and outlier_percent > outlier_percent_limit:
r.bad_syncs += 0.15
r.bad_syncs -= 0.1
# If your sync mostly looks good, your bad_sync score is decreased.
# If you had a score before, once it goes down to zero you are
# no longer timed out
# Limit bad_sync score to the range of 0 to 6
r.bad_syncs = max(0, min(6, r.bad_syncs))
if r.bad_syncs > 0:
bad_receivers += 1
if r.focus:
glogger.warning("{u}: bad_syncs: {bs:0.1f} outlier percent: {pe:0.1f} bad peers: {bp} ratio: {r} list: {l}".format(
u=r.user, bs=r.bad_syncs, pe=r.outlier_percent_rolling,
bp=bad_peers, r=round(bad_peers/num_peers, 2), l=str(bad_peer_list)))
r.recent_pair_jumps = 0
# almost reset num_outlier / num_syns for each receiver, keep a bit of the last iteration
r.num_outliers *= 0.25
r.num_syncs *= 0.25
# fudge positions, set retained precision as a fraction of a degree:
precision = 20
if r.privacy:
rlat = None
rlon = None
ralt = None
else:
rlat = round(round(r.position_llh[0] * precision) / precision + r.offX, 2)
rlon = round(round(r.position_llh[1] * precision) / precision + r.offY, 2)
ralt = 50 * round(r.position_llh[2]/50)
sync[r.user] = {
'peers': receiver_states.get(r.user, {}),
'bad_syncs': r.bad_syncs,
'lat': rlat,
'lon': rlon
}
r.peer_count = len(sync[r.user]['peers'])
clients[r.user] = {
'user': r.user,
'uid': r.uid,
'uuid': r.uuid,
'coords': "{0:.6f},{1:.6f}".format(r.position_llh[0], r.position_llh[1]),
'lat': r.position_llh[0],
'lon': r.position_llh[1],
'alt': r.position_llh[2],
'privacy': r.privacy,
'connection': r.connection_info,
'source_ip': r.connection.source_ip,
'source_port': r.connection.source_port,
'message_rate': round(r.connection.message_counter / 15.0),
'peer_count': sum(r.sync_peers),
'bad_sync_timeout': round(r.bad_syncs * 15 / 0.1),
'outlier_percent': round(r.outlier_percent_rolling, 1),
'bad_peer_list': str(bad_peer_list),
'sync_interest': [format(a.icao, '06x') for a in r.sync_interest],
'mlat_interest': [format(a.icao, '06x') for a in r.mlat_interest]
}
# reset message counter
r.connection.message_counter = 0
# The sync matrix json can be large. This means it might take a little time to write out.
# This therefore means someone could start reading it before it has completed writing...
# So, write out to a temp file first, and then call os.replace(), which is ATOMIC, to overwrite the real file.
# (Do this for each file, because why not?)
syncfile = self.work_dir + '/sync.json'
clientsfile = self.work_dir + '/clients.json'
aircraftfile = self.work_dir + '/aircraft.json'
# sync.json
tmpfile = syncfile + '.tmp'
with closing(open(tmpfile, 'w')) as f:
ujson.dump(sync, f)
# We should probably check for errors here, but let's fire-and-forget, instead...
os.replace(tmpfile, syncfile)
# clients.json
tmpfile = clientsfile + '.tmp'
with closing(open(tmpfile, 'w')) as f:
ujson.dump(clients, f)
os.replace(tmpfile, clientsfile)
# aircraft.json
tmpfile = aircraftfile + '.tmp'
with closing(open(tmpfile, 'w')) as f:
ujson.dump(aircraft_state, f)
os.replace(tmpfile, aircraftfile)
total_outlier_percent = 100 * outlier_sum / (sync_sum + 0.1)
cpu_time = time.clock_gettime(time.CLOCK_PROCESS_CPUTIME_ID)
cpu_time_us_per_sec = round((cpu_time - self.last_cpu_time) * (1e6 / 15))
self.last_cpu_time = cpu_time
try:
with open('/run/node_exporter/mlat-server.prom', 'w', encoding='utf-8') as f:
out = ''
out += 'mlat_server_cpu_ppm ' + str(cpu_time_us_per_sec) + '\n'
out += 'mlat_server_receivers ' + str(len(self.receivers)) + '\n'
out += 'mlat_server_ac_mlat ' + str(ac_count_mlat) + '\n'
out += 'mlat_server_ac_sync ' + str(ac_count_sync) + '\n'
out += 'mlat_server_ac_total ' + str(len(self.tracker.aircraft)) + '\n'
out += 'mlat_server_outlier_ppm ' + "{0:.0f}".format(total_outlier_percent * 1000) + '\n'
out += 'mlat_server_sync_points ' + "{0:.0f}".format(self.stats_sync_points / self.main_interval) + '\n'
out += 'mlat_server_sync_msgs ' + "{0:.0f}".format(self.stats_sync_msgs / self.main_interval) + '\n'
out += 'mlat_server_mlat_msgs ' + "{0:.0f}".format(self.stats_mlat_msgs / self.main_interval) + '\n'
out += 'mlat_server_valid_groups ' + "{0:.0f}".format(self.stats_valid_groups / self.main_interval) + '\n'
out += 'mlat_server_normalize_called ' + "{0:.0f}".format(self.stats_normalize / self.main_interval) + '\n'
out += 'mlat_server_solve_attempt ' + "{0:.0f}".format(self.stats_solve_attempt / self.main_interval) + '\n'
out += 'mlat_server_solve_success ' + "{0:.0f}".format(self.stats_solve_success / self.main_interval) + '\n'
out += 'mlat_server_solve_used ' + "{0:.0f}".format(self.stats_solve_used / self.main_interval) + '\n'
f.write(out)
except OSError:
pass
except:
glogger.exception("prom stats")
# reset stats
self.stats_sync_points = 0
self.stats_sync_msgs = 0
self.stats_mlat_msgs = 0
self.stats_valid_groups = 0
self.stats_normalize = 0
self.stats_solve_attempt = 0
self.stats_solve_success = 0
self.stats_solve_used = 0
if self.partition[1] > 1:
title_string = 'Status: {i}/{n} ({r} clients) ({m} mlat {s} sync {t} tracked)'.format(
i=self.partition[0],
n=self.partition[1],
r=len(self.receivers),
m=ac_count_mlat,
s=ac_count_sync,
t=len(self.tracker.aircraft))
else:
title_string = 'Status: ({r} clients {b} bad sync) ({o:.2f} outlier_percentage) ({m} mlat {s} sync {t} tracked)'.format(
r=len(self.receivers),
b=bad_receivers,
o=total_outlier_percent,
m=ac_count_mlat,
s=ac_count_sync,
t=len(self.tracker.aircraft))
util.setproctitle(title_string)
if now > self.next_status:
self.next_status = now + self.status_interval
glogger.warning(title_string)
def write_result(self, receive_timestamp, address, ecef, ecef_cov,
receivers, distinct, dof, kalman_state):
try:
lat, lon, alt = geodesy.ecef2llh(ecef)
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
if ecef_cov is None:
err_est = -1
else:
var_est = numpy.sum(numpy.diagonal(ecef_cov))
if var_est >= 0:
err_est = math.sqrt(var_est)
else:
err_est = -1
if kalman_state.valid and kalman_state.last_update >= receive_timestamp:
line = self.KTEMPLATE.format(
t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=alt * constants.MTOF,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join(
[receiver.uuid for receiver in receivers])),
klat=kalman_state.position_llh[0],
klon=kalman_state.position_llh[1],
kalt=kalman_state.position_llh[2] * constants.MTOF,
kheading=kalman_state.heading,
kspeed=kalman_state.ground_speed * constants.MS_TO_KTS,
kvrate=kalman_state.vertical_speed * constants.MS_TO_FPM,
kerr=kalman_state.position_error)
else:
line = self.TEMPLATE.format(t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=alt * constants.MTOF,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join([
receiver.uuid
for receiver in receivers
])))
self.f.write(line)
except Exception:
self.logger.exception("Failed to write result")
def write_result(self, receive_timestamp, address, ecef, ecef_cov,
receivers, distinct, dof, kalman_data):
try:
ac = self.coordinator.tracker.aircraft[address]
speed = ''
heading = ''
vrate = ''
if self.use_kalman_data:
if not kalman_data.valid or kalman_data.last_update < receive_timestamp:
if receive_timestamp - ac.last_kalman_output > 13 or receive_timestamp == ac.last_kalman_output:
#self.logger.info("{icao:06X} noKalman".format(icao=address))
lat, lon, alt = geodesy.ecef2llh(ecef)
if ac.last_altitude_time is not None and receive_timestamp - ac.last_altitude_time < 15:
alt = ac.altitude
else:
return
else:
lat, lon, alt = kalman_data.position_llh
speed = int(
round(kalman_data.ground_speed * constants.MS_TO_KTS))
heading = int(round(kalman_data.heading))
vrate = int(
round(kalman_data.vertical_speed *
constants.MS_TO_FPM))
ac.last_kalman_output = receive_timestamp
# as a test: always use non kalman position, only speed, heading, vertical speed are used from kalman
#else:
lat, lon, alt = geodesy.ecef2llh(ecef)
callsign = ac.callsign
squawk = ac.squawk
altitude = int(round(alt * constants.MTOF))
send_timestamp = time.time()
if ac.last_altitude_time is None or receive_timestamp - ac.last_altitude_time > 5:
vrate = ''
line = self.TEMPLATE.format(
mtype=3,
addr=address,
rcv_date=format_date(receive_timestamp),
rcv_time=format_time(receive_timestamp),
now_date=format_date(send_timestamp),
now_time=format_time(send_timestamp),
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=round(lat, 6),
lon=round(lon, 6),
altitude=altitude,
speed=speed,
heading=heading,
vrate=vrate,
fs=len(receivers),
emerg='',
ident='',
aog='')
self.writer.write(line.encode('ascii'))
self.last_output = time.monotonic()
except Exception:
self.logger.exception("Failed to write result")
def _resolve(self, group):
del self.pending[group.message]
# less than 3 messages -> no go
if len(group.copies) < 3:
return
decoded = modes.message.decode(group.message)
ac = self.tracker.aircraft.get(decoded.address)
if not ac:
return
ac.mlat_message_count += 1
if not ac.allow_mlat:
glogger.info("not doing mlat for {0:06x}, wrong partition!".format(
ac.icao))
return
# When we've seen a few copies of the same message, it's
# probably correct. Update the tracker with newly seen
# altitudes, squawks, callsigns.
if decoded.altitude is not None:
ac.altitude = decoded.altitude
ac.last_altitude_time = group.first_seen
if decoded.squawk is not None:
ac.squawk = decoded.squawk
if decoded.callsign is not None:
ac.callsign = decoded.callsign
# find old result, if present
if ac.last_result_position is None or (group.first_seen -
ac.last_result_time) > 120:
last_result_position = None
last_result_var = 1e9
last_result_dof = 0
last_result_time = group.first_seen - 120
else:
last_result_position = ac.last_result_position
last_result_var = ac.last_result_var
last_result_dof = ac.last_result_dof
last_result_time = ac.last_result_time
elapsed = group.first_seen - last_result_time
# find altitude
if ac.altitude is None:
altitude = None
altitude_dof = 0
else:
altitude = ac.altitude * constants.FTOM
altitude_dof = 1
if altitude < config.MIN_ALT or altitude > config.MAX_ALT:
altitude = None
altitude_dof = 0
# rate limiting
if elapsed < 1.5:
return
len_copies = len(group.copies)
if elapsed < 3 and len_copies + altitude_dof < last_result_dof and len_copies < 8:
return
# construct a map of receiver -> list of timestamps
timestamp_map = {}
for receiver, timestamp, utc in group.copies:
timestamp_map.setdefault(receiver, []).append((timestamp, utc))
# check for minimum needed receivers
dof = len(timestamp_map) + altitude_dof - 4
if dof < 0:
return
# basic ratelimit before we do more work
if elapsed < 3 and dof < last_result_dof and dof < 5:
return
# normalize timestamps. This returns a list of timestamp maps;
# within each map, the timestamp values are comparable to each other.
components = clocknorm.normalize(clocktracker=self.clock_tracker,
timestamp_map=timestamp_map)
# cluster timestamps into clusters that are probably copies of the
# same transmission.
clusters = []
min_component_size = 4 - altitude_dof
for component in components:
if len(
component
) >= min_component_size: # don't bother with orphan components at all
clusters.extend(
_cluster_timestamps(component, min_component_size))
if not clusters:
return
# start from the most recent, largest, cluster
result = None
clusters.sort(key=lambda x: (x[0], x[1]))
while clusters and not result:
distinct, cluster_utc, cluster = clusters.pop()
# accept fewer receivers after 10s
# accept more receivers immediately
elapsed = cluster_utc - last_result_time
dof = distinct + altitude_dof - 4
if elapsed < 3 and dof < last_result_dof and dof < 5:
return
# assume 250ft accuracy at the time it is reported
# (this bundles up both the measurement error, and
# that we don't adjust for local pressure)
#
# Then degrade the accuracy over time at ~4000fpm
if decoded.altitude is not None:
altitude_error = 250 * constants.FTOM
elif altitude is not None:
altitude_error = (250 + (cluster_utc - ac.last_altitude_time) *
70) * constants.FTOM
else:
altitude_error = None
cluster.sort(
key=operator.itemgetter(1)
) # sort by increasing timestamp (todo: just assume descending..)
r = solver.solve(
cluster, altitude, altitude_error, last_result_position
if last_result_position else cluster[0][0].position)
if r:
# estimate the error
ecef, ecef_cov = r
if ecef_cov is not None:
var_est = numpy.trace(ecef_cov)
else:
# this result is suspect
var_est = 100e6
if var_est > 100e6:
# more than 10km, too inaccurate
continue
if elapsed < 8 and var_est > last_result_var + elapsed * 4e6:
# less accurate than a recent position
continue
#if elapsed < 10.0 and var_est > last_result_var * 2.25:
# # much less accurate than a recent-ish position
# continue
# accept it
result = r
if not result:
return
ecef, ecef_cov = result
ac.last_result_position = ecef
ac.last_result_var = var_est
ac.last_result_dof = dof
ac.last_result_time = cluster_utc
ac.mlat_result_count += 1
if altitude is None:
_, _, solved_alt = geodesy.ecef2llh(ecef)
glogger.info(
"{addr:06x} solved altitude={solved_alt:.0f}ft with dof={dof}".
format(addr=decoded.address,
solved_alt=solved_alt * constants.MTOF,
dof=dof))
if altitude is None:
if ac.kalman.update(cluster_utc, cluster, solved_alt,
4000 / Math.sqrt(dof + 1), ecef, ecef_cov,
distinct, dof):
ac.mlat_kalman_count += 1
else:
if ac.kalman.update(cluster_utc, cluster, altitude, altitude_error,
ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
for handler in self.coordinator.output_handlers:
handler(cluster_utc, decoded.address, ecef, ecef_cov,
[receiver for receiver, timestamp, error in cluster],
distinct, dof, ac.kalman)
if self.pseudorange_file:
cluster_state = []
t0 = cluster[0][1]
for receiver, timestamp, variance in cluster:
cluster_state.append([
round(receiver.position[0], 0),
round(receiver.position[1], 0),
round(receiver.position[2], 0),
round((timestamp - t0) * 1e6, 1),
round(variance * 1e12, 2)
])
state = {
'icao': '{a:06x}'.format(a=decoded.address),
'time': round(cluster_utc, 3),
'ecef':
[round(ecef[0], 0),
round(ecef[1], 0),
round(ecef[2], 0)],
'distinct': distinct,
'dof': dof,
'cluster': cluster_state
}
if ecef_cov is not None:
state['ecef_cov'] = [
round(ecef_cov[0, 0], 0),
round(ecef_cov[0, 1], 0),
round(ecef_cov[0, 2], 0),
round(ecef_cov[1, 0], 0),
round(ecef_cov[1, 1], 0),
round(ecef_cov[1, 2], 0),
round(ecef_cov[2, 0], 0),
round(ecef_cov[2, 1], 0),
round(ecef_cov[2, 2], 0)
]
if altitude is not None:
state['altitude'] = round(altitude, 0)
state['altitude_error'] = round(altitude_error, 0)
json.dump(state, self.pseudorange_file)
self.pseudorange_file.write('\n')
def write_result(self, receive_timestamp, address, ecef, ecef_cov,
receivers, distinct, dof, kalman_data, error):
try:
ac = self.coordinator.tracker.aircraft[address]
speed = ''
heading = ''
vrate = ''
altitude = ''
if self.use_kalman_data:
if not kalman_data.valid and dof < 1:
if receive_timestamp - ac.last_crappy_output > 60:
# ignore the first crappy output
ac.last_crappy_output = receive_timestamp - 1
return
if receive_timestamp - ac.last_crappy_output > 30 or receive_timestamp == ac.last_crappy_output:
ac.last_crappy_output = receive_timestamp
else:
return
if not kalman_data.valid or kalman_data.last_update < receive_timestamp:
lat, lon, alt = geodesy.ecef2llh(ecef)
else:
# always use non kalman position, only speed, heading, vertical speed are used from kalman
# lat, lon, alt = kalman_data.position_llh
lat, lon, alt = geodesy.ecef2llh(ecef)
speed = int(
round(kalman_data.ground_speed * constants.MS_TO_KTS))
heading = int(round(kalman_data.heading))
vrate = int(
round(kalman_data.vertical_speed *
constants.MS_TO_FPM))
else:
lat, lon, alt = geodesy.ecef2llh(ecef)
callsign = ac.callsign
squawk = ac.squawk
send_timestamp = time.time()
# never use MLAT calculated altitude, most of the time it's so inaccurate that it's useless
# better have the altitude be undefined
if ac.last_altitude_time and receive_timestamp - ac.last_altitude_time < 5:
# ft
altitude = ac.altitude
else:
altitude = ''
if ac.vrate_time and receive_timestamp - ac.vrate_time < 5:
# fpm
vrate = ac.vrate
else:
vrate = ''
line = self.TEMPLATE.format(
mtype=3,
addr=address,
rcv_date=format_date(receive_timestamp),
rcv_time=format_time(receive_timestamp),
now_date=format_date(send_timestamp),
now_time=format_time(send_timestamp),
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=round(lat, 6),
lon=round(lon, 6),
altitude=altitude,
speed=speed,
heading=heading,
vrate=vrate,
fs=len(receivers),
emerg=(error if error is not None else ''),
ident='',
aog='')
self.writer.write(line.encode('ascii'))
self.last_output = time.time()
except Exception:
self.logger.exception("Failed to write result")
def write_result(self, receive_timestamp, address, ecef, ecef_cov,
receivers, distinct, dof, kalman_state, error):
try:
lat, lon, alt = geodesy.ecef2llh(ecef)
ac = self.coordinator.tracker.aircraft[address]
callsign = ac.callsign
squawk = ac.squawk
if ecef_cov is None:
err_est = -1
else:
var_est = numpy.sum(numpy.diagonal(ecef_cov))
if var_est >= 0:
err_est = math.sqrt(var_est)
else:
err_est = -1
# never use MLAT calculated altitude, most of the time it's so inaccurate that it's useless
# better have the altitude be undefined
if ac.last_altitude_time and receive_timestamp - ac.last_altitude_time < 5:
# ft
altitude = str(ac.altitude)
else:
altitude = ''
if ac.vrate_time and receive_timestamp - ac.vrate_time < 5:
# fpm
vrate = str(ac.vrate)
else:
vrate = ''
if kalman_state.valid and kalman_state.last_update >= receive_timestamp:
line = self.KTEMPLATE.format(
t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=altitude,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join(
[receiver.user for receiver in receivers])),
klat=kalman_state.position_llh[0],
klon=kalman_state.position_llh[1],
kalt=kalman_state.position_llh[2] * constants.MTOF,
kheading=kalman_state.heading,
kspeed=kalman_state.ground_speed * constants.MS_TO_KTS,
vrate=vrate,
kerr=kalman_state.position_error)
else:
line = self.TEMPLATE.format(t=receive_timestamp,
address=address,
callsign=csv_quote(callsign),
squawk=csv_quote(squawk),
lat=lat,
lon=lon,
alt=altitude,
err=err_est,
n=len(receivers),
d=distinct,
dof=dof,
receivers=csv_quote(','.join([
receiver.user
for receiver in receivers
])),
vrate=vrate)
self.pos_logger.debug(line)
except Exception:
self.logger.exception("Failed to write result")
def _resolve(self, group):
del self.pending[group.message]
# less than 3 messages -> no go
if len(group.copies) < 3:
return
decoded = modes_cython.message.decode(group.message)
if not decoded or not decoded.address:
return
ac = self.tracker.aircraft.get(decoded.address)
if not ac:
return
now = time.time()
ac.seen = now
ac.mlat_message_count += 1
self.coordinator.stats_valid_groups += 1
if not ac.allow_mlat:
glogger.info("not doing mlat for {0:06x}, wrong partition!".format(ac.icao))
return
# When we've seen a few copies of the same message, it's
# probably correct. Update the tracker with newly seen
# altitudes, squawks, callsigns.
if decoded.altitude is not None and decoded.altitude > -1500 and decoded.altitude < 75000:
if (not ac.last_altitude_time
or (group.first_seen > ac.last_altitude_time
and (group.first_seen - ac.last_altitude_time > 15 or abs(ac.altitude - decoded.altitude) < 4000)
)
):
ac.altitude = decoded.altitude
ac.last_altitude_time = group.first_seen
new_hist = []
for ts, old_alt in ac.alt_history:
if group.first_seen - ts < 20.0:
new_hist.append((ts, old_alt))
ac.alt_history = new_hist
ac.alt_history.append((group.first_seen, decoded.altitude))
ts_diff = group.first_seen - new_hist[0][0]
if ts_diff > 10:
# fpm
new_vrate = (decoded.altitude - new_hist[0][1]) / (ts_diff / 60.0)
if ac.vrate and group.first_seen - ac.vrate_time < 15:
ac.vrate = int(ac.vrate + 0.3 * (new_vrate - ac.vrate))
else:
ac.vrate = int(new_vrate)
ac.vrate_time = group.first_seen
if decoded.squawk is not None:
ac.squawk = decoded.squawk
if decoded.callsign is not None:
ac.callsign = decoded.callsign
if now - ac.last_resolve_attempt < config.RESOLVE_INTERVAL:
return
ac.last_resolve_attempt = now
# find old result, if present
if ac.last_result_position is None or (group.first_seen - ac.last_result_time) > 120:
last_result_position = None
last_result_var = 1e9
last_result_dof = 0
last_result_time = group.first_seen - 120
else:
last_result_position = ac.last_result_position
last_result_var = ac.last_result_var
last_result_dof = ac.last_result_dof
last_result_time = ac.last_result_time
elapsed = group.first_seen - last_result_time
if elapsed < 0:
elapsed = 0
if elapsed < config.RESOLVE_BACKOFF:
return
# find altitude
if (
ac.altitude is None
or ac.altitude < config.MIN_ALT
or ac.altitude > config.MAX_ALT
or group.first_seen > ac.last_altitude_time + 45
):
altitude = None
altitude_dof = 0
else:
altitude = ac.altitude * constants.FTOM
altitude_dof = 1
len_copies = len(group.copies)
max_dof = len_copies + altitude_dof - 4
if max_dof < 0:
return
if elapsed < 2 * config.RESOLVE_BACKOFF and max_dof < last_result_dof - elapsed + 0.5:
return
# construct a map of receiver -> list of timestamps
timestamp_map = {}
for receiver, timestamp, utc in group.copies:
timestamp_map.setdefault(receiver, []).append((timestamp, utc))
# check for minimum needed receivers
dof = len(timestamp_map) + altitude_dof - 4
if dof < 0:
return
if elapsed < 2 * config.RESOLVE_BACKOFF and dof < last_result_dof - elapsed + 0.5:
return
self.coordinator.stats_normalize += 1
# normalize timestamps. This returns a list of timestamp maps;
# within each map, the timestamp values are comparable to each other.
try:
components = clocktrack.normalize2(clocktracker=self.clock_tracker,
timestamp_map=timestamp_map)
except Exception as e:
traceback.print_exc()
return
# cluster timestamps into clusters that are probably copies of the
# same transmission.
clusters = []
min_component_size = 4 - altitude_dof
for component in components:
if len(component) >= min_component_size: # don't bother with orphan components at all
clusters.extend(_cluster_timestamps(component, min_component_size))
if not clusters:
return
# start from the most recent, largest, cluster
result = None
error = None
clusters.sort(key=lambda x: (x[0], x[1]))
while clusters and not result:
distinct, cluster_utc, cluster = clusters.pop()
# accept fewer receivers after 10s
# accept more receivers immediately
elapsed = cluster_utc - last_result_time
dof = distinct + altitude_dof - 4
if elapsed < 2 and dof < last_result_dof - elapsed + 0.5:
return
# assume 250ft accuracy at the time it is reported
# (this bundles up both the measurement error, and
# that we don't adjust for local pressure)
#
# Then degrade the accuracy over time at ~4000fpm
if decoded.altitude is not None:
altitude_error = 250 * constants.FTOM
elif altitude is not None:
altitude_error = (250 + (cluster_utc - ac.last_altitude_time) * 70) * constants.FTOM
else:
altitude_error = None
cluster.sort(key=operator.itemgetter(1)) # sort by increasing timestamp (todo: just assume descending..)
if elapsed > 30 and dof == 0:
continue
if elapsed < 60:
initial_guess = last_result_position
else:
initial_guess = cluster[0][0].position
self.coordinator.stats_solve_attempt += 1
r = solver.solve(cluster, altitude, altitude_error, initial_guess)
if r:
# estimate the error
ecef, ecef_cov = r
max_error = 10e3 # 10 km
if ecef_cov is not None:
var_est = numpy.trace(ecef_cov)
else:
# this result is suspect
var_est = max_error * max_error
#glogger.warn('{a:06X} {e:7.3f} '.format(a=decoded.address, e=999.999) + str([line[0].user for line in cluster]))
# don't use this
continue
error = int(math.sqrt(abs(var_est)))
if False and elapsed > 30 and error < 1e9:
lat, lon, alt = geodesy.ecef2llh(ecef)
ecef, ecef_cov = r
glogger.warn('{a:06X} {e:8.1f} {lat:7.3f},{lon:7.3f},{alt:5.0f} '.format(a=decoded.address, e=error/1000, lat=lat, lon=lon, alt=alt) + str([line[0].user for line in cluster]))
if error > max_error:
continue
self.coordinator.stats_solve_success += 1
# the higher the accuracy, the higher the freqency of positions that is output
if elapsed / 20 < error / max_error:
continue
self.coordinator.stats_solve_used += 1
#if elapsed < 10.0 and var_est > last_result_var * 2.25:
# # much less accurate than a recent-ish position
# continue
# accept it
result = r
if not result:
return
ecef, ecef_cov = result
ac.last_result_position = ecef
ac.last_result_var = var_est
ac.last_result_dof = dof
ac.last_result_time = cluster_utc
ac.mlat_result_count += 1
if altitude is not None:
lat, lon, _ = geodesy.ecef2llh(ecef)
# replace ecef altitude with reported altitude
ecef = geodesy.llh2ecef([lat, lon, altitude])
if ac.kalman.update(cluster_utc, cluster, altitude, altitude_error, ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
else:
_, _, solved_alt = geodesy.ecef2llh(ecef)
#glogger.info("{addr:06x} solved altitude={solved_alt:.0f}ft with dof={dof}".format(
# addr=decoded.address,
# solved_alt=solved_alt*constants.MTOF,
# dof=dof))
if ac.kalman.update(cluster_utc, cluster, solved_alt, 4000 / math.sqrt(dof + 1), ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
for handler in self.coordinator.output_handlers:
handler(cluster_utc, decoded.address,
ecef, ecef_cov,
[receiver for receiver, timestamp, error in cluster], distinct, dof,
ac.kalman, error)
# forward result to all receivers that received the raw message the result is based on
self.coordinator.forward_results(cluster_utc, decoded.address,
ecef, ecef_cov,
list(group.receivers), distinct, dof,
ac.kalman, error)
if self.pseudorange_file:
cluster_state = []
t0 = cluster[0][1]
for receiver, timestamp, variance in cluster:
cluster_state.append([round(receiver.position[0], 0),
round(receiver.position[1], 0),
round(receiver.position[2], 0),
round((timestamp-t0)*1e6, 1),
round(variance*1e12, 2)])
state = {'icao': '{a:06x}'.format(a=decoded.address),
'time': round(cluster_utc, 3),
'ecef': [round(ecef[0], 0),
round(ecef[1], 0),
round(ecef[2], 0)],
'distinct': distinct,
'dof': dof,
'cluster': cluster_state}
if ecef_cov is not None:
state['ecef_cov'] = [round(ecef_cov[0, 0], 0),
round(ecef_cov[0, 1], 0),
round(ecef_cov[0, 2], 0),
round(ecef_cov[1, 0], 0),
round(ecef_cov[1, 1], 0),
round(ecef_cov[1, 2], 0),
round(ecef_cov[2, 0], 0),
round(ecef_cov[2, 1], 0),
round(ecef_cov[2, 2], 0)]
if altitude is not None:
state['altitude'] = round(altitude, 0)
state['altitude_error'] = round(altitude_error, 0)
ujson.dump(state, self.pseudorange_file)
self.pseudorange_file.write('\n')
def _resolve(self, group):
del self.pending[group.message]
# less than 3 messages -> no go
if len(group.copies) < 3:
return
decoded = modes.message.decode(group.message)
ac = self.tracker.aircraft.get(decoded.address)
if not ac:
return
ac.mlat_message_count += 1
if not ac.allow_mlat:
glogger.info("not doing mlat for {0:06x}, wrong partition!".format(ac.icao))
return
# When we've seen a few copies of the same message, it's
# probably correct. Update the tracker with newly seen
# altitudes, squawks, callsigns.
if decoded.altitude is not None:
ac.altitude = decoded.altitude
ac.last_altitude_time = group.first_seen
if decoded.squawk is not None:
ac.squawk = decoded.squawk
if decoded.callsign is not None:
ac.callsign = decoded.callsign
# find old result, if present
if ac.last_result_position is None or (group.first_seen - ac.last_result_time) > 120:
last_result_position = None
last_result_var = 1e9
last_result_dof = 0
last_result_time = group.first_seen - 120
else:
last_result_position = ac.last_result_position
last_result_var = ac.last_result_var
last_result_dof = ac.last_result_dof
last_result_time = ac.last_result_time
# find altitude
if ac.altitude is None:
altitude = None
altitude_dof = 0
else:
altitude = ac.altitude * constants.FTOM
altitude_dof = 1
# construct a map of receiver -> list of timestamps
timestamp_map = {}
for receiver, timestamp, utc in group.copies:
if receiver.user not in self.blacklist:
timestamp_map.setdefault(receiver, []).append((timestamp, utc))
# check for minimum needed receivers
dof = len(timestamp_map) + altitude_dof - 4
if dof < 0:
return
# basic ratelimit before we do more work
elapsed = group.first_seen - last_result_time
if elapsed < 15.0 and dof < last_result_dof:
return
if elapsed < 2.0 and dof == last_result_dof:
return
# normalize timestamps. This returns a list of timestamp maps;
# within each map, the timestamp values are comparable to each other.
components = clocknorm.normalize(clocktracker=self.clock_tracker,
timestamp_map=timestamp_map)
# cluster timestamps into clusters that are probably copies of the
# same transmission.
clusters = []
min_component_size = 4 - altitude_dof
for component in components:
if len(component) >= min_component_size: # don't bother with orphan components at all
clusters.extend(_cluster_timestamps(component, min_component_size))
if not clusters:
return
# start from the most recent, largest, cluster
result = None
clusters.sort(key=lambda x: (x[0], x[1]))
while clusters and not result:
distinct, cluster_utc, cluster = clusters.pop()
# accept fewer receivers after 10s
# accept the same number of receivers after MLAT_DELAY - 0.5s
# accept more receivers immediately
elapsed = cluster_utc - last_result_time
dof = distinct + altitude_dof - 4
if elapsed < 10.0 and dof < last_result_dof:
break
if elapsed < (config.MLAT_DELAY - 0.5) and dof == last_result_dof:
break
# assume 250ft accuracy at the time it is reported
# (this bundles up both the measurement error, and
# that we don't adjust for local pressure)
#
# Then degrade the accuracy over time at ~4000fpm
if decoded.altitude is not None:
altitude_error = 250 * constants.FTOM
elif altitude is not None:
altitude_error = (250 + (cluster_utc - ac.last_altitude_time) * 70) * constants.FTOM
else:
altitude_error = None
cluster.sort(key=operator.itemgetter(1)) # sort by increasing timestamp (todo: just assume descending..)
r = solver.solve(cluster, altitude, altitude_error,
last_result_position if last_result_position else cluster[0][0].position)
if r:
# estimate the error
ecef, ecef_cov = r
if ecef_cov is not None:
var_est = numpy.trace(ecef_cov)
else:
# this result is suspect
var_est = 100e6
if var_est > 100e6:
# more than 10km, too inaccurate
continue
if elapsed < 2.0 and var_est > last_result_var * 1.1:
# less accurate than a recent position
continue
#if elapsed < 10.0 and var_est > last_result_var * 2.25:
# # much less accurate than a recent-ish position
# continue
# accept it
result = r
if not result:
return
ecef, ecef_cov = result
ac.last_result_position = ecef
ac.last_result_var = var_est
ac.last_result_dof = dof
ac.last_result_time = cluster_utc
ac.mlat_result_count += 1
if ac.kalman.update(cluster_utc, cluster, altitude, altitude_error, ecef, ecef_cov, distinct, dof):
ac.mlat_kalman_count += 1
if altitude is None:
_, _, solved_alt = geodesy.ecef2llh(ecef)
glogger.info("{addr:06x} solved altitude={solved_alt:.0f}ft with dof={dof}".format(
addr=decoded.address,
solved_alt=solved_alt*constants.MTOF,
dof=dof))
for handler in self.coordinator.output_handlers:
handler(cluster_utc, decoded.address,
ecef, ecef_cov,
[receiver for receiver, timestamp, error in cluster], distinct, dof,
ac.kalman)
if self.pseudorange_file:
cluster_state = []
t0 = cluster[0][1]
for receiver, timestamp, variance in cluster:
cluster_state.append([round(receiver.position[0], 0),
round(receiver.position[1], 0),
round(receiver.position[2], 0),
round((timestamp-t0)*1e6, 1),
round(variance*1e12, 2)])
state = {'icao': '{a:06x}'.format(a=decoded.address),
'time': round(cluster_utc, 3),
'ecef': [round(ecef[0], 0),
round(ecef[1], 0),
round(ecef[2], 0)],
'distinct': distinct,
'dof': dof,
'cluster': cluster_state}
if ecef_cov is not None:
state['ecef_cov'] = [round(ecef_cov[0, 0], 0),
round(ecef_cov[0, 1], 0),
round(ecef_cov[0, 2], 0),
round(ecef_cov[1, 0], 0),
round(ecef_cov[1, 1], 0),
round(ecef_cov[1, 2], 0),
round(ecef_cov[2, 0], 0),
round(ecef_cov[2, 1], 0),
round(ecef_cov[2, 2], 0)]
if altitude is not None:
state['altitude'] = round(altitude, 0)
state['altitude_error'] = round(altitude_error, 0)
json.dump(state, self.pseudorange_file)
self.pseudorange_file.write('\n')