def _do_sync(self, address, posA, posB, r0, t0A, t0B, r1, t1A, t1B): # find or create clock pair k = (r0, r1) pairing = self.clock_pairs.get(k) if pairing is None: self.clock_pairs[k] = pairing = clocksync.ClockPairing(r0, r1) # propagation delays, in clock units delay0A = geodesy.ecef_distance( posA, r0.position) * r0.clock.freq / constants.Cair delay0B = geodesy.ecef_distance( posB, r0.position) * r0.clock.freq / constants.Cair delay1A = geodesy.ecef_distance( posA, r1.position) * r1.clock.freq / constants.Cair delay1B = geodesy.ecef_distance( posB, r1.position) * r1.clock.freq / constants.Cair # compute intervals, adjusted for transmitter motion i0 = (t0B - delay0B) - (t0A - delay0A) i1 = (t1B - delay1B) - (t1A - delay1A) if not pairing.is_new(t0B - delay0B): return True # timestamp is in the past or duplicated, don't use this # do the update return pairing.update(address, t0B - delay0B, t1B - delay1B, i0, i1)
def _add_to_existing_syncpoint(self, syncpoint, r0, t0A, t0B): # add a new receiver and timestamps to an existing syncpoint # new state for the syncpoint: receiver, timestamp A, timestamp B, # and a flag indicating if this receiver actually managed to sync # with another receiver using this syncpoint (used for stats) receiverDistA = geodesy.ecef_distance(syncpoint.posA, r0.position) receiverDistB = geodesy.ecef_distance(syncpoint.posB, r0.position) # add receiver distance check here if receiverDistA > config.MAX_RANGE or receiverDistB > config.MAX_RANGE: r0.sync_range_exceeded = 1 return r0.sync_range_exceeded = 0 # propagation delays, in clock units delay0A = receiverDistA * r0.clock.freq / constants.Cair delay0B = receiverDistB * r0.clock.freq / constants.Cair td0A = t0A - delay0A td0B = t0B - delay0B # compute interval, adjusted for transmitter motion i0 = td0B - td0A r0l = [r0, td0B, i0, False] # try to sync the new receiver with all receivers that previously # saw the same pair for r1l in syncpoint.receivers: r1, td1B, i1, r1sync = r1l if r1.dead: # receiver went away before we started resolving this continue if r0 is r1: # odd, but could happen continue now = time.monotonic() # order the clockpair so that the receiver that sorts lower is the base clock if r0 < r1: if self._do_sync(syncpoint.address, now, r0, td0B, i0, r1, td1B, i1): # sync worked, note it for stats r0l[3] = r1l[3] = True else: if self._do_sync(syncpoint.address, now, r1, td1B, i1, r0, td0B, i0): # sync worked, note it for stats r0l[3] = r1l[3] = True # update syncpoint with the new receiver and we're done syncpoint.receivers.append(r0l)
def _compute_interstation_distances(self, receiver): """compute inter-station distances for a receiver""" for other_receiver in self.receivers.values(): if other_receiver is receiver: distance = 0 else: distance = geodesy.ecef_distance(receiver.position, other_receiver.position) receiver.distance[other_receiver] = distance other_receiver.distance[receiver] = distance
def _do_sync(self, address, posA, posB, r0, t0A, t0B, r1, t1A, t1B): # find or create clock pair k = (r0, r1) pairing = self.clock_pairs.get(k) if pairing is None: self.clock_pairs[k] = pairing = clocksync.ClockPairing(r0, r1) # propagation delays, in clock units delay0A = geodesy.ecef_distance(posA, r0.position) * r0.clock.freq / constants.Cair delay0B = geodesy.ecef_distance(posB, r0.position) * r0.clock.freq / constants.Cair delay1A = geodesy.ecef_distance(posA, r1.position) * r1.clock.freq / constants.Cair delay1B = geodesy.ecef_distance(posB, r1.position) * r1.clock.freq / constants.Cair # compute intervals, adjusted for transmitter motion i0 = (t0B - delay0B) - (t0A - delay0A) i1 = (t1B - delay1B) - (t1A - delay1A) if not pairing.is_new(t0B - delay0B): return True # timestamp is in the past or duplicated, don't use this # do the update return pairing.update(address, t0B - delay0B, t1B - delay1B, i0, i1)
def _residuals(x_guess, pseudorange_data, altitude, altitude_error): """Return an array of residuals for a position guess at x_guess versus actual measurements pseudorange_data and altitude.""" (*position_guess, offset) = x_guess res = [] # compute pseudoranges at the current guess vs. measured pseudorange for receiver_position, pseudorange, error in pseudorange_data: pseudorange_guess = geodesy.ecef_distance(receiver_position, position_guess) - offset res.append((pseudorange - pseudorange_guess) / error) # compute altitude at the current guess vs. measured altitude if altitude is not None: _, _, altitude_guess = geodesy.ecef2llh(position_guess) res.append((altitude - altitude_guess) / altitude_error) return res
def receiver_sync(self, receiver, even_time, odd_time, even_message, odd_message): """ Called by the coordinator to handle a sync message from a receiver. Looks for a suitable existing sync point and, if there is one, does synchronization between this receiver and the existing receivers associated with the sync point. Otherwise, validates the message pair and, if it is suitable, creates a new sync point for it. receiver: the receiver reporting the sync message even_message: a DF17 airborne position message with F=0 odd_message: a DF17 airborne position message with F=1 even_time: the time of arrival of even_message, as seen by receiver.clock odd_time: the time of arrival of odd_message, as seen by receiver.clock """ # Do sanity checks. # Messages must be within 5 seconds of each other. if abs(even_time - odd_time) / receiver.clock.freq > 5.0: return # compute key and interval if even_time < odd_time: tA = even_time tB = odd_time key = (even_message, odd_message) else: tA = odd_time tB = even_time key = (odd_message, even_message) interval = (tB - tA) / receiver.clock.freq # do we have a suitable existing match? syncpointlist = self.sync_points.get(key) if syncpointlist: for candidate in syncpointlist: if abs(candidate.interval - interval) < 1e-3: # interval matches within 1ms, close enough. self._add_to_existing_syncpoint(candidate, receiver, tA, tB) return # No existing match. Validate the messages and maybe create a new sync point # basic validity even_message = modes.message.decode(even_message) if ((not even_message or even_message.DF != 17 or not even_message.crc_ok or even_message.estype != modes.message.ESType.airborne_position or even_message.F)): return odd_message = modes.message.decode(odd_message) if ((not odd_message or odd_message.DF != 17 or not odd_message.crc_ok or odd_message.estype != modes.message.ESType.airborne_position or not odd_message.F)): return if even_message.address != odd_message.address: return # quality checks if even_message.nuc < 6 or even_message.altitude is None: return if odd_message.nuc < 6 or odd_message.altitude is None: return if abs(even_message.altitude - odd_message.altitude) > 5000: return # find global positions try: even_lat, even_lon, odd_lat, odd_lon = modes.cpr.decode(even_message.LAT, even_message.LON, odd_message.LAT, odd_message.LON) except ValueError: # CPR failed return # convert to ECEF, do range checks even_ecef = geodesy.llh2ecef((even_lat, even_lon, even_message.altitude * constants.FTOM)) if geodesy.ecef_distance(even_ecef, receiver.position) > config.MAX_RANGE: logging.info("{a:06X}: receiver range check (even) failed".format(a=even_message.address)) return odd_ecef = geodesy.llh2ecef((odd_lat, odd_lon, odd_message.altitude * constants.FTOM)) if geodesy.ecef_distance(odd_ecef, receiver.position) > config.MAX_RANGE: logging.info("{a:06X}: receiver range check (odd) failed".format(a=odd_message.address)) return if geodesy.ecef_distance(even_ecef, odd_ecef) > config.MAX_INTERMESSAGE_RANGE: logging.info("{a:06X}: intermessage range check failed".format(a=even_message.address)) return # valid. Create a new sync point. if even_time < odd_time: syncpoint = SyncPoint(even_message.address, even_ecef, odd_ecef, interval) else: syncpoint = SyncPoint(even_message.address, odd_ecef, even_ecef, interval) syncpoint.receivers.append([receiver, tA, tB, False]) if not syncpointlist: syncpointlist = self.sync_points[key] = [] syncpointlist.append(syncpoint) # schedule cleanup of the syncpoint after 2 seconds - # we should have seen all copies of those messages by # then. asyncio.get_event_loop().call_later( 2.0, functools.partial(self._cleanup_syncpoint, key=key, syncpoint=syncpoint))
def solve(measurements, altitude, altitude_error, initial_guess): """Given a set of receive timestamps, multilaterate the position of the transmitter. measurements: a list of (receiver, timestamp, error) tuples. Should be sorted by timestamp. receiver.position should be the ECEF position of the receiver timestamp should be a reception time in seconds (with an arbitrary epoch) variance should be the estimated variance of timestamp altitude: the reported altitude of the transmitter in _meters_, or None altitude_error: the estimated error in altitude in meters, or None initial_guess: an ECEF position to start the solver from Returns None on failure, or (ecef, ecef_cov) on success, with: ecef: the multilaterated ECEF position of the transmitter ecef_cov: an estimate of the covariance matrix of ecef """ if len(measurements) + (0 if altitude is None else 1) < 4: raise ValueError('Not enough measurements available') glat, glng, galt = geodesy.ecef2llh(initial_guess) if galt < config.MIN_ALT: galt = -galt initial_guess = geodesy.llh2ecef([glat, glng, galt]) if galt > config.MAX_ALT: galt = config.MAX_ALT initial_guess = geodesy.llh2ecef([glat, glng, galt]) base_timestamp = measurements[0][1] pseudorange_data = [(receiver.position, (timestamp - base_timestamp) * constants.Cair, math.sqrt(variance) * constants.Cair) for receiver, timestamp, variance in measurements] x_guess = [initial_guess[0], initial_guess[1], initial_guess[2], 0.0] x_est, cov_x, infodict, mesg, ler = scipy.optimize.leastsq( _residuals, x_guess, args=(pseudorange_data, altitude, altitude_error), full_output=True, maxfev=config.SOLVER_MAXFEV) if ler in (1, 2, 3, 4): #glogger.info("solver success: {0} {1}".format(ler, mesg)) # Solver found a result. Validate that it makes # some sort of physical sense. (*position_est, offset_est) = x_est if offset_est < 0 or offset_est > config.MAX_RANGE: #glogger.info("solver: bad offset: {0}".formaT(offset_est)) # implausible range offset to closest receiver return None for receiver, timestamp, variance in measurements: d = geodesy.ecef_distance(receiver.position, position_est) if d > config.MAX_RANGE: # too far from this receiver #glogger.info("solver: bad range: {0}".format(d)) return None if cov_x is None: return position_est, None else: return position_est, cov_x[0:3, 0:3] else: # Solver failed #glogger.info("solver: failed: {0} {1}".format(ler, mesg)) return None
def 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') 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