laikad: fixes to run on device (#24879)

* Always run laikad on device!

* Update laika

* Update laika

* Fix gps week and time of week in msg

* Reset kalman filter if pos_fix or last_known_position

* put behind file

* move pr parsing into common file

Co-authored-by: Willem Melching <willem.melching@gmail.com>
old-commit-hash: dc98511b7a

laika_repo

@@ -1 +1 @@
-Subproject commit 951ab080b998ee3edde6229654d1a4cb63cda6a9
+Subproject commit 44f048bc1f58ae9e28dfdeb98e40aea3e0f2b699

release/files_common

@@ -209,15 +209,20 @@ selfdrive/locationd/generated/ubx.h
 selfdrive/locationd/generated/gps.cpp
 selfdrive/locationd/generated/gps.h
 
+selfdrive/locationd/laikad.py
+selfdrive/locationd/laikad_helpers.py
+selfdrive/locationd/locationd.cc
 selfdrive/locationd/locationd.h
 selfdrive/locationd/locationd.cc
 selfdrive/locationd/paramsd.py
 selfdrive/locationd/models/.gitignore
-selfdrive/locationd/models/live_kf.py
 selfdrive/locationd/models/car_kf.py
-selfdrive/locationd/models/constants.py
+selfdrive/locationd/models/gnss_kf.py
+selfdrive/locationd/models/live_kf.py
 selfdrive/locationd/models/live_kf.h
 selfdrive/locationd/models/live_kf.cc
+selfdrive/locationd/models/constants.py
+selfdrive/locationd/models/gnss_helpers.py
 
 selfdrive/locationd/calibrationd.py
 

selfdrive/locationd/laikad.py

@@ -1,23 +1,22 @@
 #!/usr/bin/env python3
 import json
 import time
+from collections import defaultdict
 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 cereal import log, messaging
 from common.params import Params, put_nonblocking
 from laika import AstroDog
-from laika.constants import EARTH_ROTATION_RATE, SECS_IN_HR, SECS_IN_MIN, SPEED_OF_LIGHT
+from laika.constants import SECS_IN_HR, SECS_IN_MIN
 from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
+from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun
 from selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
 from selfdrive.locationd.models.gnss_kf import GNSSKalman
 from selfdrive.locationd.models.gnss_kf import States as GStates
@@ -91,6 +90,8 @@ class Laikad:
       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=pos_fix, std=pos_fix_residual, valid=len(pos_fix) > 0),
@@ -115,7 +116,12 @@ 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")
-      self.init_gnss_localizer(est_pos)
+      if est_pos is not None:
+        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:
@@ -130,8 +136,7 @@ class Laikad:
 
   def init_gnss_localizer(self, est_pos):
     x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))
-    if est_pos is not None:
+    x_initial[GStates.ECEF_POS] = est_pos
-      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)
 
@@ -235,90 +240,6 @@ def deserialize_hook(dct):
   return dct
 
 
-def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6):
-  '''
-  Calculates gps fix using gauss newton method
-  To solve the problem a minimal of 4 measurements are required.
-    If Glonass is included 5 are required to solve for the additional free variable.
-  returns:
-  0 -> list with positions
-  '''
-  if x0 is None:
-    x0 = [0, 0, 0, 0, 0]
-  n = len(measurements)
-  if n < min_measurements:
-    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.tolist(), residual.tolist()
-
-
-def pr_residual(measurements, posfix_functions, signal='C1C'):
-  def Fx_pos(inp, weight=None):
-    vals, gradients = [], []
-
-    for meas in measurements:
-      pr = meas.observables[signal]
-      pr += meas.sat_clock_err * SPEED_OF_LIGHT
-
-      w = (1 / meas.observables_std[signal]) if weight is None else weight
-
-      val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w)
-      vals.append(val)
-      gradients.append(gradient)
-    return np.asarray(vals), np.asarray(gradients)
-
-  return Fx_pos
-
-
-def gauss_newton(fun, b, xtol=1e-8, max_n=25):
-  for _ in range(max_n):
-    # Compute function and jacobian on current estimate
-    r, J = fun(b)
-
-    # Update estimate
-    delta = np.linalg.pinv(J) @ r
-    b -= delta
-
-    # Check step size for stopping condition
-    if np.linalg.norm(delta) < xtol:
-      break
-  return b
-
-
-def get_posfix_sympy_fun(constellation):
-  # Unknowns
-  x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
-  bc = sympy.Symbol('bc')
-  bg = sympy.Symbol('bg')
-  var = [x, y, z, bc, bg]
-
-  # Knowns
-  pr = sympy.Symbol('pr')
-  sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
-  weight = sympy.Symbol('weight')
-
-  theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT
-  val = sympy.sqrt(
-    (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
-    (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
-    (sat_z - z) ** 2
-  )
-
-  if constellation == ConstellationId.GLONASS:
-    res = weight * (val - (pr - bc - bg))
-  elif constellation == ConstellationId.GPS:
-    res = weight * (val - (pr - bc))
-  else:
-    raise NotImplementedError(f"Constellation {constellation} not supported")
-
-  res = [res] + [sympy.diff(res, v) for v in var]
-
-  return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res)
-
-
 class EphemerisSourceType(IntEnum):
   nav = 0
   nasaUltraRapid = 1

selfdrive/locationd/laikad_helpers.py

@@ -0,0 +1,89 @@
+import numpy as np
+import sympy
+
+from laika.constants import EARTH_ROTATION_RATE, SPEED_OF_LIGHT
+from laika.helpers import ConstellationId
+
+
+def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C1C', min_measurements=6):
+  '''
+  Calculates gps fix using gauss newton method
+  To solve the problem a minimal of 4 measurements are required.
+    If Glonass is included 5 are required to solve for the additional free variable.
+  returns:
+  0 -> list with positions
+  '''
+  if x0 is None:
+    x0 = [0, 0, 0, 0, 0]
+  n = len(measurements)
+  if n < min_measurements:
+    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.tolist(), residual.tolist()
+
+
+def pr_residual(measurements, posfix_functions, signal='C1C'):
+  def Fx_pos(inp, weight=None):
+    vals, gradients = [], []
+
+    for meas in measurements:
+      pr = meas.observables[signal]
+      pr += meas.sat_clock_err * SPEED_OF_LIGHT
+
+      w = (1 / meas.observables_std[signal]) if weight is None else weight
+
+      val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, w)
+      vals.append(val)
+      gradients.append(gradient)
+    return np.asarray(vals), np.asarray(gradients)
+
+  return Fx_pos
+
+
+def gauss_newton(fun, b, xtol=1e-8, max_n=25):
+  for _ in range(max_n):
+    # Compute function and jacobian on current estimate
+    r, J = fun(b)
+
+    # Update estimate
+    delta = np.linalg.pinv(J) @ r
+    b -= delta
+
+    # Check step size for stopping condition
+    if np.linalg.norm(delta) < xtol:
+      break
+  return b
+
+
+def get_posfix_sympy_fun(constellation):
+  # Unknowns
+  x, y, z = sympy.Symbol('x'), sympy.Symbol('y'), sympy.Symbol('z')
+  bc = sympy.Symbol('bc')
+  bg = sympy.Symbol('bg')
+  var = [x, y, z, bc, bg]
+
+  # Knowns
+  pr = sympy.Symbol('pr')
+  sat_x, sat_y, sat_z = sympy.Symbol('sat_x'), sympy.Symbol('sat_y'), sympy.Symbol('sat_z')
+  weight = sympy.Symbol('weight')
+
+  theta = EARTH_ROTATION_RATE * (pr - bc) / SPEED_OF_LIGHT
+  val = sympy.sqrt(
+    (sat_x * sympy.cos(theta) + sat_y * sympy.sin(theta) - x) ** 2 +
+    (sat_y * sympy.cos(theta) - sat_x * sympy.sin(theta) - y) ** 2 +
+    (sat_z - z) ** 2
+  )
+
+  if constellation == ConstellationId.GLONASS:
+    res = weight * (val - (pr - bc - bg))
+  elif constellation == ConstellationId.GPS:
+    res = weight * (val - (pr - bc))
+  else:
+    raise NotImplementedError(f"Constellation {constellation} not supported")
+
+  res = [res] + [sympy.diff(res, v) for v in var]
+
+  return sympy.lambdify([x, y, z, bc, bg, pr, sat_x, sat_y, sat_z, weight], res)

selfdrive/locationd/models/gnss_helpers.py

@@ -0,0 +1,21 @@
+import numpy as np
+
+def parse_prr(m):
+  from laika.raw_gnss import GNSSMeasurement
+  sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
+                                  m[GNSSMeasurement.SAT_VEL]))
+  R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
+  z_i = m[GNSSMeasurement.PRR]
+  return z_i, R_i, sat_pos_vel_i
+
+
+def parse_pr(m):
+  from laika.raw_gnss import GNSSMeasurement
+  pseudorange = m[GNSSMeasurement.PR]
+  pseudorange_stdev = m[GNSSMeasurement.PR_STD]
+  sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
+                                   np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
+  z_i = np.atleast_1d(pseudorange)
+  R_i = np.atleast_2d(pseudorange_stdev**2)
+  return z_i, R_i, sat_pos_freq_i
+

selfdrive/locationd/models/gnss_kf.py

@@ -7,7 +7,7 @@ import sympy as sp
 
 from rednose.helpers.ekf_sym import EKF_sym, gen_code
 from selfdrive.locationd.models.constants import ObservationKind
-from selfdrive.locationd.models.loc_kf import parse_pr, parse_prr
+from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
 
 
 class States():

selfdrive/locationd/models/loc_kf.py

@@ -5,33 +5,14 @@ import sys
 import numpy as np
 import sympy as sp
 
-from selfdrive.locationd.models.constants import ObservationKind
 from rednose.helpers.ekf_sym import EKF_sym, gen_code
 from rednose.helpers.lst_sq_computer import LstSqComputer
 from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
 
-EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
+from selfdrive.locationd.models.constants import ObservationKind
-
+from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr
-
-def parse_prr(m):
-  from laika.raw_gnss import GNSSMeasurement
-  sat_pos_vel_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
-                                  m[GNSSMeasurement.SAT_VEL]))
-  R_i = np.atleast_2d(m[GNSSMeasurement.PRR_STD]**2)
-  z_i = m[GNSSMeasurement.PRR]
-  return z_i, R_i, sat_pos_vel_i
-
-
-def parse_pr(m):
-  from laika.raw_gnss import GNSSMeasurement
-  pseudorange = m[GNSSMeasurement.PR]
-  pseudorange_stdev = m[GNSSMeasurement.PR_STD]
-  sat_pos_freq_i = np.concatenate((m[GNSSMeasurement.SAT_POS],
-                                   np.array([m[GNSSMeasurement.GLONASS_FREQ]])))
-  z_i = np.atleast_1d(pseudorange)
-  R_i = np.atleast_2d(pseudorange_stdev**2)
-  return z_i, R_i, sat_pos_freq_i
 
+EARTH_GM = 3.986005e14  # m^3/s^2 (gravitational constant * mass of earth)
 
 class States():
   ECEF_POS = slice(0, 3)  # x, y and z in ECEF in meters

selfdrive/manager/process_config.py

@@ -57,6 +57,7 @@ procs = [
 
   # Experimental
   PythonProcess("rawgpsd", "selfdrive.sensord.rawgps.rawgpsd", enabled=os.path.isfile("/persist/comma/use-quectel-rawgps")),
+  PythonProcess("laikad", "selfdrive.locationd.laikad", enabled=os.path.isfile("/persist/comma/use-laikad")),
 ]
 
 managed_processes = {p.name: p for p in procs}