diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 57aecb22e7..41b80a1c41 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -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]