Example #1
0
    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)
Example #2
0
    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
Example #3
0
    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
Example #4
0
    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")
Example #5
0
    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")
Example #6
0
    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")
Example #7
0
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
Example #8
0
    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)})
Example #9
0
    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
Example #10
0
    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
Example #11
0
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
Example #12
0
    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)
Example #13
0
    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")
Example #14
0
    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")
Example #15
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')
Example #16
0
    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")
Example #17
0
    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")
Example #18
0
    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')
Example #19
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')