Beispiel #1
0
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)
Beispiel #2
0
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