From 32fc4073ec47ec45d2ad065e28bd5bbf40f09700 Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Mon, 28 Jul 2025 19:40:26 -0700
Subject: [PATCH] rolling window

---
 common/filter_simple.py | 62 +++++++++++++++++++++++++++++++++++++++--
 estimate_lat_jerk.py    | 32 +++++++++++++++++----
 2 files changed, 87 insertions(+), 7 deletions(-)

diff --git a/common/filter_simple.py b/common/filter_simple.py
index fd63dd613c..79a75e1f07 100644
--- a/common/filter_simple.py
+++ b/common/filter_simple.py
@@ -23,6 +23,11 @@ class JerkEstimator1:
     self.prev_x = 0.0
     self.initialized = False
     self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False)
+    self._x = 0.0
+
+  @property
+  def x(self):
+    return self._x
 
   def update(self, x):
     x_filtered = self.filter.update(x)
@@ -30,9 +35,9 @@ class JerkEstimator1:
       self.prev_x = x_filtered
       self.initialized = True
 
-    jerk = (x_filtered - self.prev_x) / self.dt
+    self._x = (x_filtered - self.prev_x) / self.dt
     self.prev_x = x_filtered
-    return jerk
+    return self._x
 
 
 class JerkEstimator2:
@@ -42,6 +47,10 @@ class JerkEstimator2:
     self.initialized = False
     self.filter = FirstOrderFilter(0.0, 0.2, dt, initialized=False)
 
+  @property
+  def x(self):
+    return self.filter.x
+
   def update(self, x):
     if not self.initialized:
       self.prev_x = x
@@ -69,9 +78,58 @@ class JerkEstimator3:
     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 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.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) / .01
+
+    self.prev_x = filtered_x
+    return self._x
+
+
+class JerkEstimator5:
+  def __init__(self, dt):
+    from collections import deque
+    self.dt = dt
+    self.xs = deque(maxlen=int(0.25 / 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
diff --git a/estimate_lat_jerk.py b/estimate_lat_jerk.py
index 4715a53af1..7edd5c987c 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
+from openpilot.common.filter_simple import JerkEstimator1, JerkEstimator2, JerkEstimator3, JerkEstimator4, JerkEstimator5
 from tools.lib.logreader import LogReader
 from openpilot.selfdrive.locationd.helpers import PoseCalibrator, Pose
 from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
@@ -15,13 +15,18 @@ sm = {}
 j1 = JerkEstimator1(0.01)
 j2 = JerkEstimator2(0.01)
 j3 = JerkEstimator3(0.01)
+j4 = JerkEstimator4(0.01)
+j5 = JerkEstimator5(0.01)
 
 accels = []
-jerks1, jerks2, jerks3 = [], [], []
+kf_accels = []
+jerks1, jerks2, jerks3, jerks4, jerks5 = [], [], [], [], []
+lp_updated = False
 
 for msg in lr:
   if msg.which() == 'livePose':
     sm['livePose'] = msg.livePose
+    lp_updated = True
   elif msg.which() == 'liveParameters':
     sm['liveParameters'] = msg.liveParameters
   elif msg.which() == 'carState':
@@ -36,10 +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 = j3.update(roll_compensated_lateral_accel)
+    if lp_updated:
+      _j1 = j1.update(roll_compensated_lateral_accel)
+      _j2 = j2.update(roll_compensated_lateral_accel)
+      _j4 = j4.update(roll_compensated_lateral_accel)
+      _j5 = j5.update(roll_compensated_lateral_accel)
+      lp_updated = False
+    else:
+      _j1 = j1.x
+      _j2 = j2.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)
-    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)
 
@@ -53,6 +73,8 @@ 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].set_ylabel('Lateral Jerk (m/s³)')
 axs[1].legend()