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