From 4be4bd20709622c636a5e430658bfaaf096412ed Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 13:34:06 +0200 Subject: [PATCH 1/8] Replace posfix with gauss newton method --- selfdrive/locationd/laikad.py | 93 +++++++++++++++++++++++++++++++++-- 1 file changed, 90 insertions(+), 3 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index ebe7404e17618e..1351766f4a11e4 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -7,16 +7,17 @@ import numpy as np from collections import defaultdict +import sympy from numpy.linalg import linalg from cereal import log, messaging from common.params import Params, put_nonblocking from laika import AstroDog -from laika.constants import SECS_IN_HR, SECS_IN_MIN +from laika.constants import EARTH_ROTATION_RATE, SECS_IN_HR, SECS_IN_MIN, SPEED_OF_LIGHT 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, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox +from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox 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 @@ -38,6 +39,7 @@ def __init__(self, valid_const=("GPS", "GLONASS"), auto_update=False, valid_ephe self.last_cached_t = None self.save_ephemeris = save_ephemeris self.load_cache() + self.posfix_functions = {constellation: get_posfix_sympy_fun(constellation) for constellation in (ConstellationId.GPS, ConstellationId.GLONASS)} def load_cache(self): cache = Params().get(EPHEMERIS_CACHE) @@ -66,7 +68,7 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): 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) + pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=4) t = ublox_mono_time * 1e-9 kf_pos_std = None @@ -227,6 +229,91 @@ 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 + 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) + return gauss_newton(Fx_pos, x0) + + +def get_posfix_sympy_fun(constellation): + # Unknowns + x = sympy.Symbol('x') + y = sympy.Symbol('y') + z = sympy.Symbol('z') + bc = sympy.Symbol('bc') + bg = sympy.Symbol('bg') + var = [x, y, z, bc, bg] + + # Knowns + pr = sympy.Symbol('pr') + sat_x = sympy.Symbol('sat_x') + sat_y = sympy.Symbol('sat_y') + sat_z = 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) + + +def pr_residual(measurements, posfix_functions, signal='C1C'): + + def Fx_pos(inp): + vals, gradients = [], [] + + for meas in measurements: + pr = meas.observables[signal] + pr += meas.sat_clock_err * SPEED_OF_LIGHT + + weight = (1 / meas.observables_std[signal]) + + val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, weight) + 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 main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) From 13d46d5c109d325a2d61221a3060bfd44341c84e Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 13:45:56 +0200 Subject: [PATCH 2/8] Cleanup --- selfdrive/locationd/laikad.py | 67 ++++++++++++++++------------------- 1 file changed, 31 insertions(+), 36 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 1351766f4a11e4..9ef4863cf5fd7c 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -245,43 +245,7 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C return gauss_newton(Fx_pos, x0) -def get_posfix_sympy_fun(constellation): - # Unknowns - x = sympy.Symbol('x') - y = sympy.Symbol('y') - z = sympy.Symbol('z') - bc = sympy.Symbol('bc') - bg = sympy.Symbol('bg') - var = [x, y, z, bc, bg] - - # Knowns - pr = sympy.Symbol('pr') - sat_x = sympy.Symbol('sat_x') - sat_y = sympy.Symbol('sat_y') - sat_z = 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) - - def pr_residual(measurements, posfix_functions, signal='C1C'): - def Fx_pos(inp): vals, gradients = [], [] @@ -314,6 +278,37 @@ def gauss_newton(fun, b, xtol=1e-8, max_n=25): 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) + + def main(): sm = messaging.SubMaster(['ubloxGnss']) pm = messaging.PubMaster(['gnssMeasurements']) From 39d6e0749673461672253cd803b92dca5750edac Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 14:11:26 +0200 Subject: [PATCH 3/8] Check if glonass is in the list --- selfdrive/locationd/laikad.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 9ef4863cf5fd7c..e182bdc95b4309 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -68,7 +68,13 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): 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_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=4) + + min_measurements = 4 + for p in processed_measurements: + if p.constellation_id == ConstellationId.GLONASS > 0: + min_measurements += 1 + break + pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) t = ublox_mono_time * 1e-9 kf_pos_std = None @@ -232,6 +238,8 @@ def deserialize_hook(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 ''' From 9536d922d13c9c92672c44a0f82f1508656eb92e Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 14:45:25 +0200 Subject: [PATCH 4/8] Fix --- selfdrive/locationd/laikad.py | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index e182bdc95b4309..057fce02e22e25 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -70,6 +70,7 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): processed_measurements = process_measurements(new_meas, self.astro_dog) min_measurements = 4 + # If glonass is one of the measurements we need 5 measurements for calculating the pos fix for p in processed_measurements: if p.constellation_id == ConstellationId.GLONASS > 0: min_measurements += 1 @@ -84,15 +85,15 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): # 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] + elif pos_fix is not None: + est_pos = pos_fix[:3] else: est_pos = None corrected_measurements = [] if est_pos is not None: corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) - self.update_localizer(pos_fix, t, corrected_measurements) + self.update_localizer(est_pos, t, corrected_measurements) kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() @@ -118,7 +119,7 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): # elif ublox_msg.which == 'ionoData': # todo add this. Needed to better correct messages offline. First fix ublox_msg.cc to sent them. - def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): + def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid valid = self.kf_valid(t) if not all(valid): @@ -131,10 +132,10 @@ def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement else: cloudlog.error("Gnss kalman std too far") - if len(pos_fix) == 0: + if est_pos is None: cloudlog.info("Position fix not available when resetting kalman filter") return - post_est = pos_fix[0][:3].tolist() + post_est = est_pos.tolist() self.init_gnss_localizer(post_est) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) @@ -247,7 +248,7 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C x0 = [0, 0, 0, 0, 0] n = len(measurements) if n < min_measurements: - return [] + return None Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) return gauss_newton(Fx_pos, x0) From 0d6630deb55297ae0cf1d4d6d2258b30d53f0105 Mon Sep 17 00:00:00 2001 From: Willem Melching Date: Wed, 15 Jun 2022 15:25:53 +0200 Subject: [PATCH 5/8] also return residual --- selfdrive/locationd/laikad.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 057fce02e22e25..64a002d9a03403 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -75,7 +75,7 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): if p.constellation_id == ConstellationId.GLONASS > 0: min_measurements += 1 break - pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + pos_fix, _ = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) t = ublox_mono_time * 1e-9 kf_pos_std = None @@ -251,20 +251,22 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C return None Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) - return gauss_newton(Fx_pos, x0) + x = gauss_newton(Fx_pos, x0) + residual = Fx_pos(x, weight=1.0) + return x, residual def pr_residual(measurements, posfix_functions, signal='C1C'): - def Fx_pos(inp): + def Fx_pos(inp, weight=None): vals, gradients = [], [] for meas in measurements: pr = meas.observables[signal] pr += meas.sat_clock_err * SPEED_OF_LIGHT - weight = (1 / meas.observables_std[signal]) + w = (1 / meas.observables_std[signal]) if weight is None else weight - val, *gradient = posfix_functions[meas.constellation_id](*inp, pr, *meas.sat_pos, 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) From b76bd9625bd3bbc7444f7fc814595954b1d9f61c Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 15:40:37 +0200 Subject: [PATCH 6/8] Add residuals --- selfdrive/locationd/laikad.py | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 64a002d9a03403..88d262cf0466ce 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -69,13 +69,8 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): new_meas = read_raw_ublox(report) processed_measurements = process_measurements(new_meas, self.astro_dog) - min_measurements = 4 - # If glonass is one of the measurements we need 5 measurements for calculating the pos fix - for p in processed_measurements: - if p.constellation_id == ConstellationId.GLONASS > 0: - min_measurements += 1 - break - pos_fix, _ = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) + min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in processed_measurements) else 4 + pos_fix = calc_pos_fix_gauss_newton(processed_measurements, self.posfix_functions, min_measurements=min_measurements) t = ublox_mono_time * 1e-9 kf_pos_std = None @@ -85,8 +80,8 @@ def process_ublox_msg(self, ublox_msg, ublox_mono_time: int, block=False): # 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 pos_fix is not None: - est_pos = pos_fix[:3] + 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 = [] @@ -135,8 +130,7 @@ def update_localizer(self, est_pos, t: float, measurements: List[GNSSMeasurement if est_pos is None: cloudlog.info("Position fix not available when resetting kalman filter") return - post_est = est_pos.tolist() - self.init_gnss_localizer(post_est) + self.init_gnss_localizer(est_pos.tolist()) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) else: @@ -248,12 +242,13 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C x0 = [0, 0, 0, 0, 0] n = len(measurements) if n < min_measurements: - return None + 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, residual + print(residual[0]) + return x, residual[0] def pr_residual(measurements, posfix_functions, signal='C1C'): From 8c3540ed8b52a7b14350d2cd9eb5bb008a728825 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 07:50:31 -0700 Subject: [PATCH 7/8] Update selfdrive/locationd/laikad.py Co-authored-by: Willem Melching --- selfdrive/locationd/laikad.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 88d262cf0466ce..5dc4639644aa98 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -246,7 +246,7 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) x = gauss_newton(Fx_pos, x0) - residual = Fx_pos(x, weight=1.0) + residual, _ = Fx_pos(x, weight=1.0) print(residual[0]) return x, residual[0] From 406befa8d25a6c7d0c9971ae77f4061df102a84c Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Wed, 15 Jun 2022 16:50:57 +0200 Subject: [PATCH 8/8] Cleanup --- selfdrive/locationd/laikad.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 5dc4639644aa98..8eb81345ff1924 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -247,8 +247,7 @@ def calc_pos_fix_gauss_newton(measurements, posfix_functions, x0=None, signal='C Fx_pos = pr_residual(measurements, posfix_functions, signal=signal) x = gauss_newton(Fx_pos, x0) residual, _ = Fx_pos(x, weight=1.0) - print(residual[0]) - return x, residual[0] + return x, residual def pr_residual(measurements, posfix_functions, signal='C1C'):