From 954559f01ddc6aaa01659bbdad474bcb48e90de2 Mon Sep 17 00:00:00 2001 From: Gijs Koning Date: Fri, 8 Jul 2022 19:03:42 +0200 Subject: [PATCH] Laikad: Fix getting covariances for pos and velocity (#25084) * Fix getting covariances for pos and velocity * ref commit old-commit-hash: cf862b6576a8b3f66e26d38080a1c8eec00f0793 --- selfdrive/locationd/laikad.py | 13 +++++++------ selfdrive/test/process_replay/ref_commit | 2 +- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/selfdrive/locationd/laikad.py b/selfdrive/locationd/laikad.py index 67867e82b..0954cb4c9 100755 --- a/selfdrive/locationd/laikad.py +++ b/selfdrive/locationd/laikad.py @@ -120,11 +120,12 @@ class Laikad: 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() - ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY].tolist() + ecef_pos = self.gnss_kf.x[GStates.ECEF_POS] + ecef_vel = self.gnss_kf.x[GStates.ECEF_VELOCITY] - 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() + p = self.gnss_kf.P.diagonal() + pos_std = np.sqrt(p[GStates.ECEF_POS]) + vel_std = np.sqrt(p[GStates.ECEF_VELOCITY]) meas_msgs = [create_measurement_msg(m) for m in corrected_measurements] dat = messaging.new_message("gnssMeasurements") @@ -132,8 +133,8 @@ class Laikad: 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), + "positionECEF": measurement_msg(value=ecef_pos.tolist(), std=pos_std.tolist(), valid=kf_valid), + "velocityECEF": measurement_msg(value=ecef_vel.tolist(), std=vel_std.tolist(), valid=kf_valid), "positionFixECEF": measurement_msg(value=self.last_pos_fix, std=self.last_pos_residual, valid=self.last_pos_fix_t == t), "ubloxMonoTime": ublox_mono_time, "correctedMeasurements": meas_msgs diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 7fced5ad6..0d61dbd73 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -0da0928230d11dd4c76293b9e77b027eb4a1e291 \ No newline at end of file +dab90772097a0dd4706677ba4fe5e84b10232099 \ No newline at end of file