diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 53b3e381f..1b68b1dbc 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -1,13 +1,14 @@ import yaml import os import time +import numpy as np from abc import abstractmethod, ABC from typing import Any, Dict, Optional, Tuple, List, Callable from cereal import car from openpilot.common.basedir import BASEDIR from openpilot.common.conversions import Conversions as CV -from openpilot.common.kalman.simple_kalman import KF1D +from openpilot.common.kalman.simple_kalman import KF1D, get_kalman_gain from openpilot.common.numpy_fast import clip from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car import apply_hysteresis, gen_empty_fingerprint, scale_rot_inertia, scale_tire_stiffness, STD_CARGO_KG @@ -335,12 +336,13 @@ class CarStateBase(ABC): self.cluster_speed_hyst_gap = 0.0 self.cluster_min_speed = 0.0 # min speed before dropping to 0 - # Q = np.matrix([[0.0, 0.0], [0.0, 100.0]]) - # R = 0.3 - self.v_ego_kf = KF1D(x0=[[0.0], [0.0]], - A=[[1.0, DT_CTRL], [0.0, 1.0]], - C=[1.0, 0.0], - K=[[0.17406039], [1.65925647]]) + Q = [[0.0, 0.0], [0.0, 100.0]] + R = 0.3 + A = [[1.0, DT_CTRL], [0.0, 1.0]] + C = [[1.0, 0.0]] + x0=[[0.0], [0.0]] + K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) + self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) def update_speed_kf(self, v_ego_raw): if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index d5ff4b606..955c5c7e9 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -9d6369c5aa88c37499c330a06aa7a81a5f4f3283 \ No newline at end of file +3fd85c41e61044a86bf364b9a7a0470e5758f231