mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-16 13:02:27 +08:00
Code Issues Packages Projects Releases Wiki Activity

A valid livePose should not carry a timestamp produced from uninitialized Kalman filter time (#38124)

Browse Source 
* locationd: require finite filter time for valid livePose

* cast to bool and rename
This commit is contained in:
brian
2026-06-03 19:25:39 -04:00
committed by GitHub
parent f02d134f40
commit 094591793a
1 changed files with 3 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/locationd/locationd.py
+3 -1
View File
@@ -206,9 +206,11 @@ class LocationEstimator:
self._finite_check(t, new_x, new_P)
return HandleLogResult.SUCCESS
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_valid: bool):
def get_msg(self, sensors_valid: bool, inputs_valid: bool, filter_initialized: bool):
state, cov = self.kf.x, self.kf.P
std = np.sqrt(np.diag(cov))
filter_time_valid = bool(np.isfinite(self.kf.t))
filter_valid = filter_initialized and filter_time_valid
orientation_ned, orientation_ned_std = state[States.NED_ORIENTATION], std[States.NED_ORIENTATION]
velocity_device, velocity_device_std = state[States.DEVICE_VELOCITY], std[States.DEVICE_VELOCITY]