Laikad: Use filter for correcting measurements (#24824)

* Update laikad.

* Update log error

SConstruct

@@ -359,6 +359,7 @@ Export('cereal', 'messaging', 'visionipc')
 rednose_config = {
   'generated_folder': '#selfdrive/locationd/models/generated',
   'to_build': {
+    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
     'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']),
     'car': ('#selfdrive/locationd/models/car_kf.py', True, []),
   },
@@ -366,7 +367,6 @@ rednose_config = {
 
 if arch != "larch64":
   rednose_config['to_build'].update({
-    'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []),
     'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []),
     'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []),
     'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []),

selfdrive/locationd/laikad.py

@@ -6,6 +6,8 @@ from typing import List, Optional
 import numpy as np
 from collections import defaultdict
 
+from numpy.linalg import linalg
+
 from cereal import log, messaging
 from laika import AstroDog
 from laika.constants import SECS_IN_MIN
@@ -37,24 +39,33 @@ class Laikad:
         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)
+      pos_fix = calc_pos_fix(processed_measurements, min_measurements=4)
 
-      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.
+      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 len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000:
-        corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog)
+      if est_pos is not None:
+        corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog)
 
-      t = ublox_mono_time * 1e-9
       self.update_localizer(pos_fix, t, corrected_measurements)
-      localizer_valid = self.localizer_valid(t)
+      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 = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist()
-      vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().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()
 
       bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std)
 
@@ -62,9 +73,9 @@ class Laikad:
       dat = messaging.new_message("gnssMeasurements")
       measurement_msg = log.LiveLocationKalman.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),
+        "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid),
+        "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid),
+        "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid),
         "ubloxMonoTime": ublox_mono_time,
         "correctedMeasurements": meas_msgs
       }
@@ -77,18 +88,21 @@ class Laikad:
 
   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:
+    valid = self.kf_valid(t)
+    if not all(valid):
+      if not valid[0]:
         cloudlog.info("Init gnss kalman filter")
-      elif abs(t - filter_time) > MAX_TIME_GAP:
+      elif not valid[1]:
         cloudlog.error("Time gap of over 10s detected, gnss kalman reset")
-      else:
+      elif not valid[2]:
         cloudlog.error("Gnss kalman filter state is nan")
+      else:
+        cloudlog.error("Gnss kalman std too far")
+
+      if len(pos_fix) == 0:
+        cloudlog.error("Position fix not available when resetting kalman filter")
+        return
+      post_est = pos_fix[0][:3].tolist()
       self.init_gnss_localizer(post_est)
     if len(measurements) > 0:
       kf_add_observations(self.gnss_kf, t, measurements)
@@ -96,9 +110,12 @@ class Laikad:
       # Ensure gnss filter is updated even with no new measurements
       self.gnss_kf.predict(t)
 
-  def localizer_valid(self, t: float):
+  def kf_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]))
+    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]
 
   def init_gnss_localizer(self, est_pos):
     x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))