Laikad: More logging and use last_pos_fix for correcting (#24868)

Use last available pos_fix for correcting measurements. Improve logging measurements old-commit-hash: 5958e78037
2026-06-27 05:12:06 +08:00 · 2022-06-16 15:21:33 +02:00
parent 4c1b0696c0
commit 4b338d69db
4 changed files with 101 additions and 58 deletions
@@ -2,13 +2,13 @@
 import json
 import time
 from concurrent.futures import Future, ProcessPoolExecutor
+from enum import IntEnum
 from typing import List, Optional

 import numpy as np
 from collections import defaultdict

 import sympy
-from numpy.linalg import linalg

 from cereal import log, messaging
 from common.params import Params, put_nonblocking
@@ -30,7 +30,7 @@ CACHE_VERSION = 0.1

 class Laikad:
  def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephem_types=(EphemerisType.ULTRA_RAPID_ORBIT, EphemerisType.NAV),
-               save_ephemeris=False):
+               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()
@@ -40,6 +40,7 @@ class Laikad:
    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

  def load_cache(self):
    cache = Params().get(EPHEMERIS_CACHE)
@@ -70,27 +71,16 @@ class Laikad:
      processed_measurements = process_measurements(new_meas, self.astro_dog)

      min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4
-      pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements)
+      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]
+      est_pos = self.last_pos_fix
+
+      corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) if est_pos is not None else []

      t = ublox_mono_time * 1e-9
-      kf_pos_std = None
-      if all(self.kf_valid(t)):
-        self.gnss_kf.predict(t)
-        kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal()))
-      # If localizer is valid use its position to correct measurements
-      if kf_pos_std is not None and linalg.norm(kf_pos_std) < 100:
-        est_pos = self.gnss_kf.x[GStates.ECEF_POS]
-      elif len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000:
-        est_pos = pos_fix[0][:3]
-      else:
-        est_pos = None
-      corrected_measurements = []
-      if est_pos is not None:
-        corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
-
      self.update_localizer(est_pos, 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()

@@ -103,6 +93,7 @@ class Laikad:
      dat.gnssMeasurements = {
        "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=pos_fix, std=pos_fix_residual, valid=len(pos_fix) > 0),
        "ubloxMonoTime": ublox_mono_time,
        "correctedMeasurements": meas_msgs
      }
@@ -124,13 +115,7 @@ class Laikad:
        cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
      elif not valid[2]:
        cloudlog.error("Gnss kalman filter state is nan")
-      else:
-        cloudlog.error("Gnss kalman std too far")
-
-      if est_pos is None:
-        cloudlog.info("Position fix not available when resetting kalman filter")
-        return
-      self.init_gnss_localizer(est_pos.tolist())
+      self.init_gnss_localizer(est_pos)
    if len(measurements) > 0:
      kf_add_observations(self.gnss_kf, t, measurements)
    else:
@@ -141,14 +126,13 @@ class Laikad:
    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])),
-            linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5]
+            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
+    if est_pos is not None:
+      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):
@@ -192,6 +176,27 @@ def create_measurement_msg(meas: GNSSMeasurement):
  c.pseudorangeRateStd = float(meas.observables_std['D1C'])
  c.satPos = meas.sat_pos_final.tolist()
  c.satVel = meas.sat_vel.tolist()
+  c.satVel = meas.sat_vel.tolist()
+  ephem = meas.sat_ephemeris
+  assert ephem is not None
+  if ephem.eph_type == EphemerisType.NAV:
+    source_type = EphemerisSourceType.nav
+    week, time_of_week = -1, -1
+  else:
+    assert ephem.file_epoch is not None
+    week = ephem.file_epoch.week
+    time_of_week = ephem.file_epoch.tow
+    file_src = ephem.file_source
+    if file_src == 'igu':  # example nasa: '2214/igu22144_00.sp3.Z'
+      source_type = EphemerisSourceType.nasaUltraRapid
+    elif file_src == 'Sta':  # example nasa: '22166/ultra/Stark_1D_22061518.sp3'
+      source_type = EphemerisSourceType.glonassIacUltraRapid
+    else:
+      raise Exception(f"Didn't expect file source {file_src}")
+
+  c.ephemerisSource.type = source_type.value
+  c.ephemerisSource.gpsWeek = week
+  c.ephemerisSource.gpsTimeOfWeek = time_of_week
  return c


@@ -242,12 +247,12 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C
    x0 = [0, 0, 0, 0, 0]
  n = len(measurements)
  if n < min_measurements:
-    return []
+    return [], []

  Fx_pos = pr_residual(measurements, posfix_functions, signal=signal)
  x = gauss_newton(Fx_pos, x0)
  residual, _ = Fx_pos(x, weight=1.0)
-  return x, residual
+  return x.tolist(), residual.tolist()


 def pr_residual(measurements, posfix_functions, signal='C1C'):
@@ -314,10 +319,16 @@ def get_posfix_sympy_fun(constellation):
  return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res)


+class EphemerisSourceType(IntEnum):
+  nav = 0
+  nasaUltraRapid = 1
+  glonassIacUltraRapid = 2
+
+
 def main():
  sm = messaging.SubMaster(['ubloxGnss'])
  pm = messaging.PubMaster(['gnssMeasurements'])
-
+  # todo get last_known_position
  laikad = Laikad(save_ephemeris=True)
  while True:
    sm.update()
@@ -1,16 +1,17 @@
 #!/usr/bin/env python3
 import time
 import unittest
+from collections import defaultdict
 from datetime import datetime
 from unittest import mock
 from unittest.mock import Mock, patch

 from common.params import Params
-from laika.ephemeris import EphemerisType
+from laika.ephemeris import EphemerisType, GPSEphemeris
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId, TimeRangeHolder
 from laika.raw_gnss import GNSSMeasurement, read_raw_ublox
-from selfdrive.locationd.laikad import EPHEMERIS_CACHE, Laikad, create_measurement_msg
+from selfdrive.locationd.laikad import EPHEMERIS_CACHE, EphemerisSourceType, Laikad, create_measurement_msg
 from selfdrive.test.openpilotci import get_url
 from tools.lib.logreader import LogReader

@@ -33,23 +34,62 @@ def verify_messages(lr, laikad, return_one_success=False):
  return good_msgs


+def get_first_gps_time(logs):
+  for m in logs:
+    if m.ubloxGnss.which == 'measurementReport':
+      new_meas = read_raw_ublox(m.ubloxGnss.measurementReport)
+      if len(new_meas) > 0:
+        return new_meas[0].recv_time
+
+
+def get_measurement_mock(gpstime, sat_ephemeris):
+  meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.})
+  # Fake measurement being processed
+  meas.observables_final = meas.observables
+  meas.sat_ephemeris = sat_ephemeris
+  return meas
+
+
 class TestLaikad(unittest.TestCase):

  @classmethod
  def setUpClass(cls):
-    cls.logs = get_log(range(1))
+    logs = get_log(range(1))
+    cls.logs = logs
+    first_gps_time = get_first_gps_time(logs)
+    cls.first_gps_time = first_gps_time

  def setUp(self):
    Params().delete(EPHEMERIS_CACHE)

-  def test_create_msg_without_errors(self):
-    gpstime = GPSTime.from_datetime(datetime.now())
-    meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.})
-    # Fake observables_final to be correct
-    meas.observables_final = meas.observables
-    msg = create_measurement_msg(meas)
+  def test_ephemeris_source_in_msg(self):
+    data_mock = defaultdict(str)
+    data_mock['sv_id'] = 1

-    self.assertEqual(msg.constellationId, 'gps')
+    gpstime = GPSTime.from_datetime(datetime(2022, month=3, day=1))
+    laikad = Laikad()
+    laikad.fetch_orbits(gpstime, block=True)
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
+    msg = create_measurement_msg(meas)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
+    # Verify gps satellite returns same source
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['R01'][0])
+    msg = create_measurement_msg(meas)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.glonassIacUltraRapid)
+
+    # Test nasa source by using older date
+    gpstime = GPSTime.from_datetime(datetime(2021, month=3, day=1))
+    laikad = Laikad()
+    laikad.fetch_orbits(gpstime, block=True)
+    meas = get_measurement_mock(gpstime, laikad.astro_dog.orbits['G01'][0])
+    msg = create_measurement_msg(meas)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nasaUltraRapid)
+
+    # Test nav source type
+    ephem = GPSEphemeris(data_mock, gpstime)
+    meas = get_measurement_mock(gpstime, ephem)
+    msg = create_measurement_msg(meas)
+    self.assertEqual(msg.ephemerisSource.type.raw, EphemerisSourceType.nav)

  def test_laika_online(self):
    laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.ULTRA_RAPID_ORBIT)
@@ -76,18 +116,10 @@ class TestLaikad(unittest.TestCase):
    self.assertEqual(256, len(correct_msgs))
    self.assertEqual(256, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))

-  def get_first_gps_time(self):
-    for m in self.logs:
-      if m.ubloxGnss.which == 'measurementReport':
-        new_meas = read_raw_ublox(m.ubloxGnss.measurementReport)
-        if len(new_meas) != 0:
-          return new_meas[0].recv_time
-
  def test_laika_get_orbits(self):
    laikad = Laikad(auto_update=False)
-    first_gps_time = self.get_first_gps_time()
    # Pretend process has loaded the orbits on startup by using the time of the first gps message.
-    laikad.fetch_orbits(first_gps_time, block=True)
+    laikad.fetch_orbits(self.first_gps_time, block=True)
    self.dict_has_values(laikad.astro_dog.orbits)

  @unittest.skip("Use to debug live data")
@@ -117,7 +149,6 @@ class TestLaikad(unittest.TestCase):

  def test_cache(self):
    laikad = Laikad(auto_update=True, save_ephemeris=True)
-    first_gps_time = self.get_first_gps_time()

    def wait_for_cache():
      max_time = 2
@@ -126,13 +157,14 @@ class TestLaikad(unittest.TestCase):
        max_time -= 0.1
        if max_time == 0:
          self.fail("Cache has not been written after 2 seconds")
+
    # Test cache with no ephemeris
    laikad.cache_ephemeris(t=GPSTime(0, 0))
    wait_for_cache()
    Params().delete(EPHEMERIS_CACHE)

-    laikad.astro_dog.get_navs(first_gps_time)
-    laikad.fetch_orbits(first_gps_time, block=True)
+    laikad.astro_dog.get_navs(self.first_gps_time)
+    laikad.fetch_orbits(self.first_gps_time, block=True)

    # Wait for cache to save
    wait_for_cache()
@@ -149,7 +181,7 @@ class TestLaikad(unittest.TestCase):
    with patch('selfdrive.locationd.laikad.get_orbit_data', return_value=None) as mock_method:
      # Verify no orbit downloads even if orbit fetch times is reset since the cache has recently been saved and we don't want to download high frequently
      laikad.astro_dog.orbit_fetched_times = TimeRangeHolder()
-      laikad.fetch_orbits(first_gps_time, block=False)
+      laikad.fetch_orbits(self.first_gps_time, block=False)
      mock_method.assert_not_called()

      # Verify cache is working for only orbits by running a segment