|
|
@ -381,6 +381,10 @@ class CarStateBase(ABC): |
|
|
|
K = get_kalman_gain(DT_CTRL, np.array(A), np.array(C), np.array(Q), R) |
|
|
|
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) |
|
|
|
self.v_ego_kf = KF1D(x0=x0, A=A, C=C[0], K=K) |
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
@abstractmethod |
|
|
|
|
|
|
|
def update(self, *args) -> structs.CarState: |
|
|
|
|
|
|
|
raise NotImplementedError |
|
|
|
|
|
|
|
|
|
|
|
def update_speed_kf(self, v_ego_raw): |
|
|
|
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 |
|
|
|
if abs(v_ego_raw - self.v_ego_kf.x[0][0]) > 2.0: # Prevent large accelerations when car starts at non zero speed |
|
|
|
self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) |
|
|
|
self.v_ego_kf.set_x([[v_ego_raw], [0.0]]) |
|
|
|