compare some methods

common/filter_simple.py

@@ -15,3 +15,63 @@ class FirstOrderFilter:
       self.initialized = True
       self.x = x
     return self.x
+
+
+class JerkEstimator1:
+  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)
+
+  def update(self, x):
+    x_filtered = self.filter.update(x)
+    if not self.initialized:
+      self.prev_x = x_filtered
+      self.initialized = True
+
+    jerk = (x_filtered - self.prev_x) / self.dt
+    self.prev_x = x_filtered
+    return jerk
+
+
+class JerkEstimator2:
+  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)
+
+  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
+
+
+class JerkEstimator3:
+  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)
+
+  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]

estimate_lat_jerk.py

@@ -0,0 +1,58 @@
+import math
+import matplotlib.pyplot as plt
+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
+
+plt.ion()
+
+lr = LogReader("https://connect.comma.ai/8011d605be1cbb77/000001c6--130f915e07/130/138")
+calibrator = PoseCalibrator()
+
+sm = {}
+
+j1 = JerkEstimator1(0.01)
+j2 = JerkEstimator2(0.01)
+j3 = JerkEstimator3(0.01)
+
+accels = []
+jerks1, jerks2, jerks3 = [], [], []
+
+for msg in lr:
+  if msg.which() == 'livePose':
+    sm['livePose'] = msg.livePose
+  elif msg.which() == 'liveParameters':
+    sm['liveParameters'] = msg.liveParameters
+  elif msg.which() == 'carState':
+    if len(sm) < 2:
+      continue
+
+    CS = msg.carState
+    device_pose = Pose.from_live_pose(sm['livePose'])
+    calibrated_pose = calibrator.build_calibrated_pose(device_pose)
+
+    yaw_rate = calibrated_pose.angular_velocity.yaw
+    roll = sm['liveParameters'].roll
+    roll_compensated_lateral_accel = (CS.vEgo * yaw_rate) - (math.sin(roll) * ACCELERATION_DUE_TO_GRAVITY)
+
+    accels.append(roll_compensated_lateral_accel)
+    jerks1.append(j1.update(roll_compensated_lateral_accel))
+    jerks2.append(j2.update(roll_compensated_lateral_accel))
+    jerks3.append(j3.update(roll_compensated_lateral_accel))
+
+    print(roll_compensated_lateral_accel)
+
+
+fig, axs = plt.subplots(2, sharex=True)
+
+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].set_ylabel('Lateral Jerk (m/s³)')
+axs[1].legend()
+

selfdrive/selfdrived/selfdrived.py

@@ -69,6 +69,9 @@ def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibra
   return counter, counter > MIN_EXCESSIVE_ACTUATION_COUNT
 
 
+def estimate_jerk(signal)
+
+
 class SelfdriveD:
   def __init__(self, CP=None):
     self.params = Params()