From 701b0a7060bc99eee7a81e43f4b20db017c75d95 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20R=C4=85czy?= Date: Fri, 4 Apr 2025 17:06:46 -0700 Subject: [PATCH] Delete lines --- selfdrive/locationd/locationd.py | 6 ++-- selfdrive/locationd/models/pose_kf.py | 41 +++++---------------------- 2 files changed, 10 insertions(+), 37 deletions(-) diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 31af940f08..e5377afa99 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -64,7 +64,7 @@ class LocationEstimator: self.observation_errors = {kind: np.zeros(3, dtype=np.float32) for kind in obs_kinds} def reset(self, t: float, x_initial: np.ndarray = PoseKalman.initial_x, P_initial: np.ndarray = PoseKalman.initial_P): - self.kf.reset(t, x_initial, P_initial) + self.kf.init_state(x_initial, covs=P_initial, filter_time=t) def _validate_sensor_source(self, source: log.SensorEventData.SensorSource): # some segments have two IMUs, ignore the second one @@ -185,8 +185,8 @@ class LocationEstimator: rot_device_noise = rot_device_std ** 2 trans_device_noise = trans_device_std ** 2 - cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, rot_device_noise) - cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, trans_device_noise) + cam_odo_rot_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_ROTATION, rot_device, np.array([np.diag(rot_device_noise)])) + cam_odo_trans_res = self.kf.predict_and_observe(t, ObservationKind.CAMERA_ODO_TRANSLATION, trans_device, np.array([np.diag(trans_device_noise)])) self.camodo_yawrate_distribution = np.array([rot_device[2], rot_device_std[2]]) if cam_odo_rot_res is not None: _, new_x, _, new_P, _, _, (cam_odo_rot_err,), _, _ = cam_odo_rot_res diff --git a/selfdrive/locationd/models/pose_kf.py b/selfdrive/locationd/models/pose_kf.py index df63518441..020e51ad6e 100755 --- a/selfdrive/locationd/models/pose_kf.py +++ b/selfdrive/locationd/models/pose_kf.py @@ -5,6 +5,8 @@ import numpy as np from openpilot.selfdrive.locationd.models.constants import ObservationKind +from rednose.helpers.kalmanfilter import KalmanFilter + if __name__=="__main__": import sympy as sp from rednose.helpers.ekf_sym import gen_code @@ -24,7 +26,7 @@ class States: ACCEL_BIAS = slice(15, 18) # Acceletometer bias in m/s**2 -class PoseKalman: +class PoseKalman(KalmanFilter): name = "pose" # state @@ -50,10 +52,10 @@ class PoseKalman: 3**2, 3**2, 3**2, 0.005**2, 0.005**2, 0.005**2]) - obs_noise = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]), - ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]), - ObservationKind.CAMERA_ODO_TRANSLATION: np.array([0.5**2, 0.5**2, 0.5**2]), - ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2])} + obs_noise = {ObservationKind.PHONE_GYRO: np.diag([0.025**2, 0.025**2, 0.025**2]), + ObservationKind.PHONE_ACCEL: np.diag([.5**2, .5**2, .5**2]), + ObservationKind.CAMERA_ODO_TRANSLATION: np.diag([0.5**2, 0.5**2, 0.5**2]), + ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2])} @staticmethod def generate_code(generated_dir): @@ -103,35 +105,6 @@ class PoseKalman: self.filter = EKF_sym_pyx(generated_dir, self.name, PoseKalman.Q, PoseKalman.initial_x, PoseKalman.initial_P, dim_state, dim_state_err, max_rewind_age=max_rewind_age) - @property - def x(self): - return self.filter.state() - - @property - def P(self): - return self.filter.covs() - - @property - def t(self): - return self.filter.get_filter_time() - - def predict_and_observe(self, t, kind, data, obs_noise=None): - data = np.atleast_2d(data) - if obs_noise is None: - obs_noise = self.obs_noise[kind] - R = self._get_R(len(data), obs_noise) - return self.filter.predict_and_update_batch(t, kind, data, R) - - def reset(self, t, x_init, P_init): - self.filter.init_state(x_init, P_init, t) - - def _get_R(self, n, obs_noise): - dim = obs_noise.shape[0] - R = np.zeros((n, dim, dim)) - for i in range(n): - R[i, :, :] = np.diag(obs_noise) - return R - if __name__ == "__main__": generated_dir = sys.argv[2]