stuff

common/filter_simple.py

@@ -17,6 +17,7 @@ class FirstOrderFilter:
     return self.x
 
 
+# low pass the raw jerk
 class JerkEstimator1:
   def __init__(self, dt):
     self.dt = dt
@@ -38,6 +39,7 @@ class JerkEstimator1:
     return self.filter.x
 
 
+# use a kalman filter to estimate jerk
 class JerkEstimator2:
   def __init__(self, dt):
     self.dt = dt
@@ -69,34 +71,30 @@ class JerkEstimator2:
 
 class JerkEstimator3:
   def __init__(self, dt):
+    from collections import deque
     self.dt = dt
-    self.prev_x = 0.0
-    self.initialized = False
-    self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False)
-    self._x = 0.0
+    self.xs = deque(maxlen=int(0.2 / dt))
+    self._x = 0
 
   @property
   def x(self):
     return self._x
 
   def update(self, x):
-    filtered_x = self.filter.update(x)
-
-    if not self.initialized:
-      self.prev_x = filtered_x
-      self.initialized = True
-
-    self._x = (filtered_x - self.prev_x) / self.dt
+    self.xs.append(x)
+    if len(self.xs) < 2:
+      return 0.0
 
-    self.prev_x = filtered_x
+    self._x = (self.xs[-1] - self.xs[0]) / ((len(self.xs) - 1) * self.dt)
     return self._x
 
 
 class JerkEstimator4:
   def __init__(self, dt):
-    from collections import deque
     self.dt = dt
-    self.xs = deque(maxlen=int(0.25 / dt))
+    self.prev_x = 0.0
+    self.initialized = False
+    self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False)
     self._x = 0
 
   @property
@@ -104,9 +102,6 @@ class JerkEstimator4:
     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)
+    self.filter.update(x)
+    self._x = (x - self.filter.x) / 0.2
     return self._x

estimate_lat_jerk.py

@@ -16,10 +16,11 @@ j1 = JerkEstimator1(1/20)
 j2 = JerkEstimator2(1/100)
 j3 = JerkEstimator3(1/20)
 j4 = JerkEstimator4(1/20)
+j5 = JerkEstimator1(1/100)
 
 accels = []
 kf_accels = []
-jerks1, jerks2, jerks3, jerks4 = [], [], [], []
+jerks1, jerks2, jerks3, jerks4, jerks5 = [], [], [], [], []
 lp_updated = False
 
 for msg in lr:
@@ -40,22 +41,25 @@ for msg in lr:
     roll = sm['liveParameters'].roll
     roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
 
-    _j3 = j2.update(roll_compensated_lateral_accel)
+    _j2 = j2.update(roll_compensated_lateral_accel)
+    _j5 = j5.update(roll_compensated_lateral_accel)
     if lp_updated:
       _j1 = j1.update(roll_compensated_lateral_accel)
-      _j2 = j2.update(roll_compensated_lateral_accel)
-      _j4 = j3.update(roll_compensated_lateral_accel)
+      _j3 = j3.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
+      _j5 = j5.x
 
     jerks1.append(_j1)
     jerks2.append(_j2)
     jerks3.append(_j3)
     jerks4.append(_j4)
+    jerks5.append(_j5)
     accels.append(roll_compensated_lateral_accel)
 
     print(roll_compensated_lateral_accel)
@@ -69,8 +73,9 @@ 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(jerks3, label='Jerk Estimator 3')
 axs[1].plot(jerks4, label='Jerk Estimator 4')
+# axs[1].plot(jerks5, label='Jerk Estimator 5')
 axs[1].set_ylabel('Lateral Jerk (m/s³)')
 axs[1].legend()