plot

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

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()