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 _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')