mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 21:14:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

Laikad: Fix getting covariances for pos and velocity (#25084)

Browse Source 
* Fix getting covariances for pos and velocity

* ref commit
old-commit-hash: cf862b6576
This commit is contained in:
Gijs Koning
2022-07-08 19:03:42 +02:00
committed by GitHub
parent c20b96764c
commit 954559f01d
2 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
selfdrive/locationd/laikad.py
View File

@@ -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

2
selfdrive/test/process_replay/ref_commit
View File

@@ -1 +1 @@
0da0928230d11dd4c76293b9e77b027eb4a1e291
dab90772097a0dd4706677ba4fe5e84b10232099