From 40f18c5d4a7060f768199658109698ee54fd1187 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 4 Aug 2025 16:54:10 -0700 Subject: [PATCH] revert filter to master --- common/filter_simple.py | 72 ----------------------------------------- 1 file changed, 72 deletions(-) diff --git a/common/filter_simple.py b/common/filter_simple.py index e45934c475..9ea6fe3070 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -15,75 +15,3 @@ class FirstOrderFilter: self.initialized = True self.x = x return self.x - - -# low pass the raw jerk -class JerkEstimator1: - def __init__(self, dt): - self.dt = dt - self.prev_x = 0.0 - self.initialized = False - self.filter = FirstOrderFilter(0.0, 0.08, dt, initialized=False) - - @property - def x(self): - return self.filter.x - - def update(self, x): - if not self.initialized: - self.prev_x = x - self.initialized = True - - self.filter.update((x - self.prev_x) / self.dt) - self.prev_x = x - return self.filter.x - - -# use a kalman filter to estimate jerk -class JerkEstimator2: - def __init__(self, dt): - self.dt = dt - self.initialized = False - from opendbc.car.common.simple_kalman import KF1D, get_kalman_gain - import numpy as np - - DT_CTRL = 0.01 - - 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.kf = KF1D(x0=x0, A=A, C=C[0], K=K) - - @property - def x(self): - return self.kf.x[1][0] if self.initialized else 0.0 - - def update(self, x): - if not self.initialized: - self.kf.set_x([[x], [0.0]]) - self.initialized = True - self.kf.update(x) - return self.kf.x[1][0] - - -class JerkEstimator3: - def __init__(self, dt): - from collections import deque - self.dt = dt - self.xs = deque(maxlen=int(0.2 / dt)) - self._x = 0 - - @property - def x(self): - return self._x - - def update(self, x): - self.xs.append(x) - if len(self.xs) < 2: - return 0.0 - - self._x = (self.xs[-1] - self.xs[0]) / ((len(self.xs) - 1) * self.dt) - return self._x