move laikad helpers to laika

laika_repo

@@ -1 +1 @@
-Subproject commit 3d8a1ff78ec4c095238e9a49717646acbc58bfa0
+Subproject commit c11017e731a8917dcabefed6fb38db365f8aaff5

release/files_common

@@ -233,7 +233,6 @@ selfdrive/locationd/generated/gps.cpp
 selfdrive/locationd/generated/gps.h
 
 selfdrive/locationd/laikad.py
-selfdrive/locationd/laikad_helpers.py
 selfdrive/locationd/locationd.h
 selfdrive/locationd/locationd.cc
 selfdrive/locationd/paramsd.py

selfdrive/locationd/laikad.py

@@ -20,7 +20,7 @@ from laika.ephemeris import Ephemeris, EphemerisType, convert_ublox_ephem, parse
 from laika.gps_time import GPSTime
 from laika.helpers import ConstellationId
 from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
-from selfdrive.locationd.laikad_helpers import calc_pos_fix_gauss_newton, get_posfix_sympy_fun, calc_vel_fix
+from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
 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
@@ -58,6 +58,7 @@ class Laikad:
     self.load_cache()
 
     self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)}
+    self.velfix_function = get_velfix_sympy_func()
     self.last_fix_pos = None
     self.last_fix_t = None
     self.gps_week = None
@@ -94,13 +95,14 @@ class Laikad:
   def get_lsq_fix(self, t, measurements):
     if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
       min_measurements = 6 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 5
-      position_solution, pr_residuals = calc_pos_fix_gauss_newton(measurements, self.posfix_functions, min_measurements=min_measurements)
+      position_solution, pr_residuals = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
+
       if len(position_solution) < 3:
         return None
       position_estimate = position_solution[:3]
       #TODO median abs residual is decent estimate of std, can be improved with measurements stds and/or DOP
       position_std = np.median(np.abs(pr_residuals)) * np.ones(3)
-      velocity_solution, prr_residuals = calc_vel_fix(measurements, position_estimate, min_measurements=min_measurements)
+      velocity_solution, prr_residuals = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements)
       if len(velocity_solution) < 3:
         return None
 

selfdrive/locationd/laikad_helpers.py

@@ -1,166 +0,0 @@
-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 get_prr_sympy_func():
-  # implementing this without sympy.Matrix gives a 2x speedup at generation
-
-  # knowns, receiver position, satellite position, satellite velocity
-  ep_x, ep_y, ep_z = sympy.Symbol('ep_x'), sympy.Symbol('ep_y'), sympy.Symbol('ep_z')
-  est_pos = np.array([ep_x, ep_y, ep_z])
-  sp_x, sp_y, sp_z = sympy.Symbol('sp_x'), sympy.Symbol('sp_y'), sympy.Symbol('sp_z')
-  sat_pos = np.array([sp_x, sp_y, sp_z])
-  sv_x, sv_y, sv_z = sympy.Symbol('sv_x'), sympy.Symbol('sv_y'), sympy.Symbol('sv_z')
-  sat_vel = np.array([sv_x, sv_y, sv_z])
-  observables = sympy.Symbol('observables')
-  weight = sympy.Symbol('weight')
-
-  # unknown, receiver velocity
-  v_x, v_y, v_z = sympy.Symbol('v_x'), sympy.Symbol('v_y'), sympy.Symbol('v_z')
-  vel = np.array([v_x, v_y, v_z])
-  vel_o = sympy.Symbol('vel_o')
-
-  loss = sat_pos - est_pos
-  loss /= sympy.sqrt(loss.dot(loss))
-
-  nv = loss.dot(sat_vel - vel)
-  ov = (observables - vel_o)
-  res = (nv - ov)*weight
-
-  res = [res] + [sympy.diff(res, v) for v in [v_x, v_y, v_z, vel_o]]
-
-  return sympy.lambdify([
-      ep_x, ep_y, ep_z, sp_x, sp_y, sp_z,
-      sv_x, sv_y, sv_z, observables, weight,
-      v_x, v_y, v_z, vel_o
-    ],
-    res, modules=["numpy"])
-
-
-def prr_residual(measurements, est_pos, no_weight=False, signal='D1C'):
-  loss_func = get_prr_sympy_func()
-
-  def Fx_vel(vel, no_weight=no_weight):
-    vals, gradients = [], []
-
-    for meas in measurements:
-      if signal not in meas.observables or not np.isfinite(meas.observables[signal]):
-        continue
-
-      sat_pos = meas.sat_pos_final if meas.corrected else meas.sat_pos
-      weight = 1 if no_weight or meas.observables_std[signal] == 0 else (1 / meas.observables_std[signal])
-
-      val, *gradient = loss_func(est_pos[0], est_pos[1], est_pos[2],
-                                 sat_pos[0], sat_pos[1], sat_pos[2],
-                                 meas.sat_vel[0], meas.sat_vel[1], meas.sat_vel[2],
-                                 meas.observables[signal], weight,
-                                 vel[0], vel[1], vel[2], vel[3])
-      vals.append(val)
-      gradients.append(gradient)
-
-    return np.asarray(vals), np.asarray(gradients)
-
-  return Fx_vel
-
-
-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, modules=["numpy"])
-
-
-def calc_vel_fix(measurements, est_pos, min_measurements=6):
-  '''
-  Calculates gps velocity fix with WLS optimizer
-  returns:
-  0 -> list with velocities
-  1 -> pseudorange_rate errs
-  '''
-  if len(measurements) < min_measurements:
-    return [], []
-
-  Fx_vel = prr_residual(measurements, est_pos)
-  opt_vel = gauss_newton(Fx_vel, [0, 0, 0, 0])
-  residual, _ = Fx_vel(opt_vel, no_weight=True)
-  return opt_vel.tolist(), residual.tolist()