diff --git a/common/filter_simple.py b/common/filter_simple.py index 30bfbfb240..e45934c475 100644 --- a/common/filter_simple.py +++ b/common/filter_simple.py @@ -23,7 +23,7 @@ class JerkEstimator1: self.dt = dt self.prev_x = 0.0 self.initialized = False - self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False) + self.filter = FirstOrderFilter(0.0, 0.08, dt, initialized=False) @property def x(self): @@ -87,21 +87,3 @@ class JerkEstimator3: self._x = (self.xs[-1] - self.xs[0]) / ((len(self.xs) - 1) * self.dt) return self._x - - -class JerkEstimator4: - def __init__(self, dt): - self.dt = dt - self.prev_x = 0.0 - self.initialized = False - self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False) - self._x = 0 - - @property - def x(self): - return self._x - - def update(self, x): - self.filter.update(x) - self._x = (x - self.filter.x) / 0.2 - return self._x diff --git a/estimate_lat_jerk.py b/estimate_lat_jerk.py index 265571f926..0391efe066 100644 --- a/estimate_lat_jerk.py +++ b/estimate_lat_jerk.py @@ -1,6 +1,6 @@ import math import matplotlib.pyplot as plt -from openpilot.common.filter_simple import JerkEstimator1, JerkEstimator2, JerkEstimator3, JerkEstimator4 +from openpilot.common.filter_simple import JerkEstimator1, JerkEstimator2, JerkEstimator3 from tools.lib.logreader import LogReader from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose from opendbc.car import ACCELERATION_DUE_TO_GRAVITY @@ -15,7 +15,7 @@ sm = {} j1 = JerkEstimator1(1/20) j2 = JerkEstimator2(1/100) j3 = JerkEstimator3(1/20) -j4 = JerkEstimator4(1/20) +# j4 = JerkEstimator4(1/20) j5 = JerkEstimator1(1/100) accels = [] @@ -46,19 +46,19 @@ for msg in lr: if lp_updated: _j1 = j1.update(roll_compensated_lateral_accel) _j3 = j3.update(roll_compensated_lateral_accel) - _j4 = j4.update(roll_compensated_lateral_accel) + # _j4 = j4.update(roll_compensated_lateral_accel) lp_updated = False else: _j1 = j1.x _j2 = j2.x _j3 = j3.x - _j4 = j4.x + # _j4 = j4.x _j5 = j5.x jerks1.append(_j1) jerks2.append(_j2) jerks3.append(_j3) - jerks4.append(_j4) + # jerks4.append(_j4) jerks5.append(_j5) accels.append(roll_compensated_lateral_accel) @@ -71,11 +71,11 @@ axs[0].plot(accels, label='Lateral Accel') axs[0].set_ylabel('Lateral Acceleration (m/s²)') axs[0].legend() -axs[1].plot(jerks1, label='Jerk Estimator 1') -# axs[1].plot(jerks2, label='Jerk Estimator 2') -# axs[1].plot(jerks3, label='Jerk Estimator 3') -axs[1].plot(jerks4, label='Jerk Estimator 4') -# axs[1].plot(jerks5, label='Jerk Estimator 5') +axs[1].plot(jerks1, label='Low pass filter at 20 Hz (1)') +axs[1].plot(jerks2, label='Kalman filter (2)') +axs[1].plot(jerks3, label='Windowed (3)') +# axs[1].plot(jerks4, label='Jerk Estimator 4') +# axs[1].plot(jerks5, label='Low pass filter at 100 Hz (5)') axs[1].set_ylabel('Lateral Jerk (m/s³)') axs[1].legend()