diff --git a/laika_repo b/laika_repo index 951ab080b9..44f048bc1f 160000 --- a/laika_repo +++ b/laika_repo @@ -1 +1 @@ -Subproject commit 951ab080b998ee3edde6229654d1a4cb63cda6a9 +Subproject commit 44f048bc1f58ae9e28dfdeb98e40aea3e0f2b699 diff --git a/release/files_common b/release/files_common index 6731ae4a12..b6cd4fa797 100644 --- a/release/files_common +++ b/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 diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 0e2838da6d..8ec570e2be 100755 --- a/selfdrive/locationd/laikad.py +++ b/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 diff --git a/selfdrive/locationd/laikad_helpers.py b/selfdrive/locationd/laikad_helpers.py new file mode 100644 index 0000000000..81f5ac3dd6 --- /dev/null +++ b/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) diff --git a/selfdrive/locationd/models/gnss_helpers.py b/selfdrive/locationd/models/gnss_helpers.py new file mode 100644 index 0000000000..a3bcbc0002 --- /dev/null +++ b/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 + diff --git a/selfdrive/locationd/models/gnss_kf.py b/selfdrive/locationd/models/gnss_kf.py index 3f4baab7b5..ce0ff84dc2 100755 --- a/selfdrive/locationd/models/gnss_kf.py +++ b/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(): diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py index 8391426dd1..6b08828695 100755 --- a/selfdrive/locationd/models/loc_kf.py +++ b/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) - - -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 +from selfdrive.locationd.models.constants import ObservationKind +from selfdrive.locationd.models.gnss_helpers import parse_pr, parse_prr +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 diff --git a/selfdrive/manager/process_config.py b/selfdrive/manager/process_config.py index b3efe7e2de..a9a5c78a7f 100644 --- a/selfdrive/manager/process_config.py +++ b/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}