From d76002e7d45d78c09efe15aace32fd048119e360 Mon Sep 17 00:00:00 2001 From: Kurt Nistelberger Date: Fri, 16 Dec 2022 20:11:04 -0800 Subject: [PATCH] move laikad helpers to laika --- laika_repo | 2 +- release/files_common | 1 - selfdrive/locationd/laikad.py | 8 +- selfdrive/locationd/laikad_helpers.py | 166 -------------------------- 4 files changed, 6 insertions(+), 171 deletions(-) delete mode 100644 selfdrive/locationd/laikad_helpers.py diff --git a/laika_repo b/laika_repo index 3d8a1ff78e..c11017e731 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 3d8a1ff78ec4c095238e9a49717646acbc58bfa0 +Subproject commit c11017e731a8917dcabefed6fb38db365f8aaff5 diff --git a/release/files_common b/release/files_common index f438904118..a06543abbf 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 8e7a0a09e0..f71383b480 100755 --- a/selfdrive/locationd/laikad.py +++ b/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 diff --git a/selfdrive/locationd/laikad_helpers.py b/selfdrive/locationd/laikad_helpers.py deleted file mode 100644 index 61f09732c8..0000000000 --- a/selfdrive/locationd/laikad_helpers.py +++ /dev/null @@ -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() \ No newline at end of file