diff --git a/SConstruct b/SConstruct index 0aeb382c23..b5c4edc99b 100644 --- a/SConstruct +++ b/SConstruct @@ -359,6 +359,7 @@ Export('cereal', 'messaging', 'visionipc') rednose_config = { 'generated_folder': '#selfdrive/locationd/models/generated', 'to_build': { + 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'live': ('#selfdrive/locationd/models/live_kf.py', True, ['live_kf_constants.h']), 'car': ('#selfdrive/locationd/models/car_kf.py', True, []), }, @@ -366,7 +367,6 @@ rednose_config = { if arch != "larch64": rednose_config['to_build'].update({ - 'gnss': ('#selfdrive/locationd/models/gnss_kf.py', True, []), 'loc_4': ('#selfdrive/locationd/models/loc_kf.py', True, []), 'pos_computer_4': ('#rednose/helpers/lst_sq_computer.py', False, []), 'pos_computer_5': ('#rednose/helpers/lst_sq_computer.py', False, []), diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 01457f4681..42c4156795 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -6,6 +6,8 @@ from typing import List, Optional import numpy as np from collections import defaultdict +from numpy.linalg import linalg + from cereal import log, messaging from laika import AstroDog from laika.constants import SECS_IN_MIN @@ -37,24 +39,33 @@ class Laikad: latest_msg_t = GPSTime(report.gpsWeek, report.rcvTow) 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) - measurements = process_measurements(new_meas, self.astro_dog) - pos_fix = calc_pos_fix(measurements, min_measurements=4) - # To get a position fix a minimum of 5 measurements are needed. - # Each report can contain less and some measurements can't be processed. + t = ublox_mono_time * 1e-9 + kf_pos_std = None + if all(self.kf_valid(t)): + self.gnss_kf.predict(t) + kf_pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())) + # 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] + else: + est_pos = None corrected_measurements = [] - if len(pos_fix) > 0 and abs(np.array(pos_fix[1])).mean() < 1000: - corrected_measurements = correct_measurements(measurements, pos_fix[0][:3], self.astro_dog) + if est_pos is not None: + corrected_measurements = correct_measurements(processed_measurements, est_pos, self.astro_dog) - t = ublox_mono_time * 1e-9 self.update_localizer(pos_fix, t, corrected_measurements) - localizer_valid = self.localizer_valid(t) + kf_valid = all(self.kf_valid(t)) ecef_pos = self.gnss_kf.x[GStates.ECEF_POS].tolist() ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() - pos_std = self.gnss_kf.P[GStates.ECEF_POS].flatten().tolist() - vel_std = self.gnss_kf.P[GStates.ECEF_VELOCITY].flatten().tolist() + pos_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_POS].diagonal())).tolist() + vel_std = np.sqrt(abs(self.gnss_kf.P[GStates.ECEF_VELOCITY].diagonal())).tolist() bearing_deg, bearing_std = get_bearing_from_gnss(ecef_pos, ecef_vel, vel_std) @@ -62,9 +73,9 @@ class Laikad: dat = messaging.new_message("gnssMeasurements") measurement_msg = log.LiveLocationKalman.Measurement.new_message dat.gnssMeasurements = { - "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=localizer_valid), - "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=localizer_valid), - "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=localizer_valid), + "positionECEF": measurement_msg(value=ecef_pos, std=pos_std, valid=kf_valid), + "velocityECEF": measurement_msg(value=ecef_vel, std=vel_std, valid=kf_valid), + "bearingDeg": measurement_msg(value=[bearing_deg], std=[bearing_std], valid=kf_valid), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs } @@ -77,18 +88,21 @@ class Laikad: def update_localizer(self, pos_fix, t: float, measurements: List[GNSSMeasurement]): # Check time and outputs are valid - if not self.localizer_valid(t): - # A position fix is needed when resetting the kalman filter. - if len(pos_fix) == 0: - return - post_est = pos_fix[0][:3].tolist() - filter_time = self.gnss_kf.filter.filter_time - if filter_time is None: + valid = self.kf_valid(t) + if not all(valid): + if not valid[0]: cloudlog.info("Init gnss kalman filter") - elif abs(t - filter_time) > MAX_TIME_GAP: + elif not valid[1]: cloudlog.error("Time gap of over 10s detected, gnss kalman reset") - else: + elif not valid[2]: cloudlog.error("Gnss kalman filter state is nan") + else: + cloudlog.error("Gnss kalman std too far") + + if len(pos_fix) == 0: + cloudlog.error("Position fix not available when resetting kalman filter") + return + post_est = pos_fix[0][:3].tolist() self.init_gnss_localizer(post_est) if len(measurements) > 0: kf_add_observations(self.gnss_kf, t, measurements) @@ -96,9 +110,12 @@ class Laikad: # Ensure gnss filter is updated even with no new measurements self.gnss_kf.predict(t) - def localizer_valid(self, t: float): + def kf_valid(self, t: float): filter_time = self.gnss_kf.filter.filter_time - return filter_time is not None and (t - filter_time) < MAX_TIME_GAP and all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])) + return [filter_time is not None, + filter_time is not None and abs(t - filter_time) < MAX_TIME_GAP, + all(np.isfinite(self.gnss_kf.x[GStates.ECEF_POS])), + linalg.norm(self.gnss_kf.P[GStates.ECEF_POS]) < 1e5] def init_gnss_localizer(self, est_pos): x_initial, p_initial_diag = np.copy(GNSSKalman.x_initial), np.copy(np.diagonal(GNSSKalman.P_initial))