def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), save_ephemeris=False): """ valid_const: GNSS constellation which can be used auto_fetch_orbits: If true fetch orbits from internet when needed auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere. valid_ephem_types: Valid ephemeris types to be used by AstroDog save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. """ self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True) self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True) self.auto_fetch_orbits = auto_fetch_orbits self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None self.got_first_ublox_msg = False self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None
def kf_add_observations(gnss_kf: GNSSKalman, t: float, measurements: List[GNSSMeasurement]): ekf_data = defaultdict(list) for m in measurements: m_arr = m.as_array() if m.constellation_id == ConstellationId.GPS: ekf_data[ObservationKind.PSEUDORANGE_GPS].append(m_arr) elif m.constellation_id == ConstellationId.GLONASS: ekf_data[ObservationKind.PSEUDORANGE_GLONASS].append(m_arr) ekf_data[ObservationKind.PSEUDORANGE_RATE_GPS] = ekf_data[ObservationKind.PSEUDORANGE_GPS] ekf_data[ObservationKind.PSEUDORANGE_RATE_GLONASS] = ekf_data[ObservationKind.PSEUDORANGE_GLONASS] for kind, data in ekf_data.items(): if len(data) >0: gnss_kf.predict_and_observe(t, kind, data)
def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), save_ephemeris=False, last_known_position=None): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True) self.gnss_kf = GNSSKalman(GENERATED_DIR) self.orbit_fetch_executor = ProcessPoolExecutor() self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} self.last_pos_fix = last_known_position if last_known_position is not None else [] self.last_pos_residual = [] self.last_pos_fix_t = None
class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_fetch_orbits=True, auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), save_ephemeris=False, use_qcom=False): """ valid_const: GNSS constellation which can be used auto_fetch_orbits: If true fetch orbits from internet when needed auto_update: If true download AstroDog will download all files needed. This can be ephemeris or correction data like ionosphere. valid_ephem_types: Valid ephemeris types to be used by AstroDog save_ephemeris: If true saves and loads nav and orbit ephemeris to cache. """ self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True, cache_dir=DOWNLOADS_CACHE_FOLDER) self.gnss_kf = GNSSKalman(GENERATED_DIR, cython=True, erratic_clock=use_qcom) self.auto_fetch_orbits = auto_fetch_orbits self.orbit_fetch_executor: Optional[ProcessPoolExecutor] = None self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None self.got_first_gnss_msg = False self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = { constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS) } self.last_pos_fix = [] self.last_pos_residual = [] self.last_pos_fix_t = None self.use_qcom = use_qcom def load_cache(self): if not self.save_ephemeris: return cache = Params().get(EPHEMERIS_CACHE) if not cache: return try: cache = json.loads(cache, object_hook=deserialize_hook) self.astro_dog.add_orbits(cache['orbits']) self.astro_dog.add_navs(cache['nav']) self.last_fetch_orbits_t = cache['last_fetch_orbits_t'] except json.decoder.JSONDecodeError: cloudlog.exception("Error parsing cache") timestamp = self.last_fetch_orbits_t.as_datetime( ) if self.last_fetch_orbits_t is not None else 'Nan' cloudlog.debug( f"Loaded nav ({sum([len(v) for v in cache['nav']])}) and orbits ({sum([len(v) for v in cache['orbits']])}) cache with timestamp: {timestamp}. Unique orbit and nav sats: {list(cache['orbits'].keys())} {list(cache['nav'].keys())} " + f"With time range: {[f'{start.as_datetime()}, {end.as_datetime()}' for (start,end) in self.astro_dog.orbit_fetched_times._ranges]}" ) def cache_ephemeris(self, t: GPSTime): if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): put_nonblocking( EPHEMERIS_CACHE, json.dumps( { 'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav }, cls=CacheSerializer)) cloudlog.debug("Cache saved") self.last_cached_t = t def get_est_pos(self, t, processed_measurements): if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2: min_measurements = 6 if any( p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 5 pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton( processed_measurements, self.posfix_functions, min_measurements=min_measurements) if len(pos_fix) > 0: self.last_pos_fix_t = t residual_median = np.median(np.abs(pos_fix_residual)) if np.median( np.abs(pos_fix_residual)) < POS_FIX_RESIDUAL_THRESHOLD: cloudlog.debug( f"Pos fix is within threshold with median: {residual_median.round()}" ) self.last_pos_fix = pos_fix[:3] self.last_pos_residual = pos_fix_residual else: cloudlog.debug( f"Pos fix failed with median: {residual_median.round()}. All residuals: {np.round(pos_fix_residual)}" ) return self.last_pos_fix def is_good_report(self, gnss_msg): if gnss_msg.which == 'drMeasurementReport' and self.use_qcom: constellation_id = ConstellationId.from_qcom_source( gnss_msg.drMeasurementReport.source) # TODO support GLONASS return constellation_id in [ ConstellationId.GPS, ConstellationId.SBAS ] elif gnss_msg.which == 'measurementReport' and not self.use_qcom: return True else: return False def read_report(self, gnss_msg): if self.use_qcom: report = gnss_msg.drMeasurementReport week = report.gpsWeek tow = report.gpsMilliseconds / 1000.0 new_meas = read_raw_qcom(report) else: report = gnss_msg.measurementReport week = report.gpsWeek tow = report.rcvTow new_meas = read_raw_ublox(report) return week, tow, new_meas def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False): if self.is_good_report(gnss_msg): week, tow, new_meas = self.read_report(gnss_msg) t = gnss_mono_time * 1e-9 if week > 0: self.got_first_gnss_msg = True latest_msg_t = GPSTime(week, tow) if self.auto_fetch_orbits: self.fetch_orbits(latest_msg_t, block) # Filter measurements with unexpected pseudoranges for GPS and GLONASS satellites new_meas = [ m for m in new_meas if 1e7 < m.observables['C1C'] < 3e7 ] processed_measurements = process_measurements( new_meas, self.astro_dog) est_pos = self.get_est_pos(t, processed_measurements) corrected_measurements = correct_measurements( processed_measurements, est_pos, self.astro_dog) if len(est_pos) > 0 else [] if gnss_mono_time % 10 == 0: cloudlog.debug( f"Measurements Incoming/Processed/Corrected: {len(new_meas), len(processed_measurements), len(corrected_measurements)}" ) self.update_localizer(est_pos, t, corrected_measurements) kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS] ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY] p = self.gnss_kf.P.diagonal() pos_std = np.sqrt(p[GStates.ECEF_POS]) vel_std = np.sqrt(p[GStates.ECEF_VELOCITY]) meas_msgs = [ create_measurement_msg(m) for m in corrected_measurements ] dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { "gpsWeek": week, "gpsTimeOfWeek": tow, "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), "ubloxMonoTime": gnss_mono_time, "correctedMeasurements": meas_msgs } return dat # TODO this only works on GLONASS, qcom needs live ephemeris parsing too elif gnss_msg.which == 'ephemeris': ephem = convert_ublox_ephem(gnss_msg.ephemeris) self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) #elif gnss_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid valid = self.kf_valid(t) if not all(valid): if not valid[0]: # Filter not initialized pass elif not valid[1]: cloudlog.error( "Time gap of over 10s detected, gnss kalman reset") elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") if len(est_pos) > 0: cloudlog.info(f"Reset kalman filter with {est_pos}") self.init_gnss_localizer(est_pos) else: return if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) def kf_valid(self, t: float) -> List[bool]: filter_time = self.gnss_kf.filter.get_filter_time() return [ not math.isnan(filter_time), abs(t - filter_time) < MAX_TIME_GAP, all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) ] def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy( np.diagonal(GNSSKalman.P_initial)) x_initial[GStates.ECEF_POS] = est_pos p_initial_diag[GStates.ECEF_POS] = 1000**2 self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) def fetch_orbits(self, t: GPSTime, block): # Download new orbits if 1 hour of orbits data left if t + SECS_IN_HR not in self.astro_dog.orbit_fetched_times and ( self.last_fetch_orbits_t is None or abs(t - self.last_fetch_orbits_t) > SECS_IN_MIN): astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types, self.astro_dog.cache_dir ret = None if block: # Used for testing purposes ret = get_orbit_data(t, *astro_dog_vars) elif self.orbit_fetch_future is None: self.orbit_fetch_executor = ProcessPoolExecutor(max_workers=1) self.orbit_fetch_future = self.orbit_fetch_executor.submit( get_orbit_data, t, *astro_dog_vars) elif self.orbit_fetch_future.done(): ret = self.orbit_fetch_future.result() self.orbit_fetch_executor = self.orbit_fetch_future = None if ret is not None: if ret[0] is None: self.last_fetch_orbits_t = ret[2] else: self.astro_dog.orbits, self.astro_dog.orbit_fetched_times, self.last_fetch_orbits_t = ret self.cache_ephemeris(t=t)
class Laikad: def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV), save_ephemeris=False, last_known_position=None): self.astro_dog = AstroDog(valid_const=valid_const, auto_update=auto_update, valid_ephem_types=valid_ephem_types, clear_old_ephemeris=True) self.gnss_kf = GNSSKalman(GENERATED_DIR) self.orbit_fetch_executor = ProcessPoolExecutor() self.orbit_fetch_future: Optional[Future] = None self.last_fetch_orbits_t = None self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} self.last_pos_fix = last_known_position if last_known_position is not None else [] self.last_pos_residual = [] self.last_pos_fix_t = None def load_cache(self): cache = Params().get(EPHEMERIS_CACHE) if not cache: return try: cache = json.loads(cache, object_hook=deserialize_hook) self.astro_dog.add_orbits(cache['orbits']) self.astro_dog.add_navs(cache['nav']) self.last_fetch_orbits_t = cache['last_fetch_orbits_t'] except json.decoder.JSONDecodeError: cloudlog.exception("Error parsing cache") def cache_ephemeris(self, t: GPSTime): if self.save_ephemeris and (self.last_cached_t is None or t - self.last_cached_t > SECS_IN_MIN): put_nonblocking(EPHEMERIS_CACHE, json.dumps( {'version': CACHE_VERSION, 'last_fetch_orbits_t': self.last_fetch_orbits_t, 'orbits': self.astro_dog.orbits, 'nav': self.astro_dog.nav}, cls=CacheSerializer)) self.last_cached_t = t def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if ublox_msg.which == 'measurementReport': t = ublox_mono_time * 1e-9 report = ublox_msg.measurementReport if report.gpsWeek > 0: latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) self.fetch_orbits(latest_msg_t + SECS_IN_MIN, block) new_meas = read_raw_ublox(report) processed_measurements = process_measurements(new_meas, self.astro_dog) if self.last_pos_fix_t is None or abs(self.last_pos_fix_t - t) >= 2: min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 pos_fix, pos_fix_residual = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) if len(pos_fix) > 0: self.last_pos_fix = pos_fix[:3] self.last_pos_residual = pos_fix_residual self.last_pos_fix_t = t corrected_measurements = correct_measurements(processed_measurements, self.last_pos_fix, self.astro_dog) if self.last_pos_fix_t is not None else [] self.update_localizer(self.last_pos_fix, t, corrected_measurements) kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { "gpsWeek": report.gpsWeek, "gpsTimeOfWeek": report.rcvTow, "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) self.astro_dog.add_navs({ephem.prn: [ephem]}) self.cache_ephemeris(t=ephem.epoch) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid valid = self.kf_valid(t) if not all(valid): if not valid[0]: cloudlog.info("Init gnss kalman filter") elif not valid[1]: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") if len(est_pos) > 0: cloudlog.info(f"Reset kalman filter with {est_pos}") self.init_gnss_localizer(est_pos) else: cloudlog.info("Could not reset kalman filter") return if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) def kf_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time return [filter_time is not None, filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS]))] def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial)) x_initial[GStates.ECEF_POS] = est_pos p_initial_diag[GStates.ECEF_POS] = 1000 ** 2 self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag) def fetch_orbits(self, t: GPSTime, block): if t not in self.astro_dog.orbit_fetched_times and (self.last_fetch_orbits_t is None or t - self.last_fetch_orbits_t > SECS_IN_HR): astro_dog_vars = self.astro_dog.valid_const, self.astro_dog.auto_update, self.astro_dog.valid_ephem_types if self.orbit_fetch_future is None: self.orbit_fetch_future = self.orbit_fetch_executor.submit(get_orbit_data, t, *astro_dog_vars) if block: self.orbit_fetch_future.result() if self.orbit_fetch_future.done(): ret = self.orbit_fetch_future.result() self.last_fetch_orbits_t = t if ret: self.astro_dog.orbits, self.astro_dog.orbit_fetched_times = ret self.cache_ephemeris(t=t) self.orbit_fetch_future = None
def __init__(self, use_internet): self.astro_dog = AstroDog(use_internet=use_internet) self.gnss_kf = GNSSKalman(GENERATED_DIR)
class Laikad: def __init__(self, use_internet): self.astro_dog = AstroDog(use_internet=use_internet) self.gnss_kf = GNSSKalman(GENERATED_DIR) def process_ublox_msg(self, ublox_msg, ublox_mono_time: int): if ublox_msg.which == 'measurementReport': report = ublox_msg.measurementReport new_meas = read_raw_ublox(report) measurements = process_measurements(new_meas, self.astro_dog) pos_fix = calc_pos_fix(measurements, min_measurements=4) # To get a position fix a minimum of 5 measurements are needed. # Each report can contain less and some measurements can't be processed. corrected_measurements = [] if len(pos_fix) > 0 and linalg.norm(pos_fix[1]) < 100: corrected_measurements = correct_measurements( measurements, pos_fix[0][:3], self.astro_dog) t = ublox_mono_time * 1e-9 self.update_localizer(pos_fix, t, corrected_measurements) localizer_valid = self.localizer_valid(t) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() pos_std = float(np.linalg.norm(self.gnss_kf.P[GStates.ECEF_POS])) vel_std = float( np.linalg.norm(self.gnss_kf.P[GStates.ECEF_VELOCITY])) bearing_deg, bearing_std = get_bearing_from_gnss( ecef_pos, ecef_vel, vel_std) meas_msgs = [ create_measurement_msg(m) for m in corrected_measurements ] dat = messaging.new_message("gnssMeasurements") measurement_msg = log.GnssMeasurements.Measurement.new_message dat.gnssMeasurements = { "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid), "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid), "bearingDeg": measurement_msg(value=[bearing_deg], std=bearing_std, valid=localizer_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } return dat elif ublox_msg.which == 'ephemeris': ephem = convert_ublox_ephem(ublox_msg.ephemeris) self.astro_dog.add_ephem(ephem, self.astro_dog.orbits) # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid if not self.localizer_valid(t): # A position fix is needed when resetting the kalman filter. if len(pos_fix) == 0: return post_est = pos_fix[0][:3].tolist() filter_time = self.gnss_kf.filter.filter_time if filter_time is None: cloudlog.info("Init gnss kalman filter") elif (t - filter_time) > MAX_TIME_GAP: cloudlog.error( "Time gap of over 10s detected, gnss kalman reset") else: cloudlog.error("Gnss kalman filter state is nan") self.init_gnss_localizer(post_est) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) def localizer_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and \ all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy( np.diagonal(GNSSKalman.P_initial)) x_initial[GStates.ECEF_POS] = est_pos p_initial_diag[GStates.ECEF_POS] = 1000**2 self.gnss_kf.init_state(x_initial, covs_diag=p_initial_diag)