Exemplo n.º 1
0
    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')
Exemplo n.º 2
0
    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')