Check for excessive lateral actuation

cereal/log.capnp

@@ -818,6 +818,8 @@ struct SelfdriveState {
   active @2 :Bool;
   engageable @9 :Bool;  # can OP be engaged?
 
+  rollCompensatedLateralAccel @13 :Float32;
+
   # UI alerts
   alertText1 @3 :Text;
   alertText2 @4 :Text;

common/filter_simple.py

@@ -15,3 +15,75 @@ def update(self, x):
       self.initialized = True
       self.x = x
     return self.x
+
+
+# low pass the raw jerk
+class JerkEstimator1:
+  def __init__(self, dt):
+    self.dt = dt
+    self.prev_x = 0.0
+    self.initialized = False
+    self.filter = FirstOrderFilter(0.0, 0.08, dt, initialized=False)
+
+  @property
+  def x(self):
+    return self.filter.x
+
+  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
+
+
+# use a kalman filter to estimate jerk
+class JerkEstimator2:
+  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)
+
+  @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 JerkEstimator3:
+  def __init__(self, dt):
+    from collections import deque
+    self.dt = dt
+    self.xs = deque(maxlen=int(0.2 / 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

estimate_lat_jerk.py

@@ -0,0 +1,83 @@
+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(1/20)
+j2 = JerkEstimator2(1/100)
+j3 = JerkEstimator3(1/100)
+# j4 = JerkEstimator4(1/20)
+j5 = JerkEstimator1(1/100)
+
+accels = []
+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':
+    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)
+
+    _j2 = j2.update(roll_compensated_lateral_accel)
+    _j3 = j3.update(roll_compensated_lateral_accel)
+    _j5 = j5.update(roll_compensated_lateral_accel)
+    if lp_updated:
+      _j1 = j1.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)
+    kf_accels.append(j2.kf.x[0][0])
+    accels.append(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].plot(kf_accels, label='Kalman filter accel')
+axs[0].set_ylabel('Lateral Acceleration (m/s²)')
+axs[0].legend()
+
+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()
+

selfdrive/selfdrived/selfdrived.py

@@ -1,16 +1,19 @@
 #!/usr/bin/env python3
 import os
+import math
 import time
 import threading
 
 import cereal.messaging as messaging
 
 from cereal import car, log
 from msgq.visionipc import VisionIpcClient, VisionStreamType
-from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX
+from opendbc.car import ACCELERATION_DUE_TO_GRAVITY
+from opendbc.car.interfaces import ACCEL_MIN, ACCEL_MAX, ISO_LATERAL_ACCEL, ISO_LATERAL_JERK
 
 
 from openpilot.common.params import Params
+from openpilot.common.filter_simple import JerkEstimator3
 from openpilot.common.realtime import config_realtime_process, Priority, Ratekeeper, DT_CTRL
 from openpilot.common.swaglog import cloudlog
 from openpilot.common.gps import get_gps_location_service
@@ -29,7 +32,7 @@
 TESTING_CLOSET = "TESTING_CLOSET" in os.environ
 
 LONGITUDINAL_PERSONALITY_MAP = {v: k for k, v in log.LongitudinalPersonality.schema.enumerants.items()}
-MIN_EXCESSIVE_ACTUATION_COUNT = int(0.25 / DT_CTRL)
+MIN_EXCESSIVE_ACTUATION_COUNT = int(0.15 / DT_CTRL)  # TODO: needs to be 25 for longitudinal
 
 ThermalStatus = log.DeviceState.ThermalStatus
 State = log.SelfdriveState.OpenpilotState
@@ -43,17 +46,40 @@
 IGNORED_SAFETY_MODES = (SafetyModel.silent, SafetyModel.noOutput)
 
 
-def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose, counter: int) -> tuple[int, bool]:
+def check_excessive_actuation(sm: messaging.SubMaster, CS: car.CarState, calibrated_pose: Pose,
+                              jerk_estimator: JerkEstimator3, counter: int) -> tuple[int, bool]:
   # CS.aEgo can be noisy to bumps in the road, transitioning from standstill, losing traction, etc.
+  # longitudinal
   accel_calibrated = calibrated_pose.acceleration.x
+  excessive_long_actuation = sm['carControl'].longActive and (accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2)
+
+  # lateral
+  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)
+
+  excessive_lat_actuation = False
+  estimated_jerk = jerk_estimator.update(roll_compensated_lateral_accel)
+  print('vEgo', CS.vEgo, yaw_rate, roll)
+  print('roll_compensated_lateral_accel', roll_compensated_lateral_accel, estimated_jerk)
+  if sm['carControl'].latActive:
+    if not CS.steeringPressed:
+      if abs(roll_compensated_lateral_accel) > ISO_LATERAL_ACCEL * 2:
+        excessive_lat_actuation = True
+
+    # Note that we do not check steeringPressed as it is unreliable under high jerk conditions
+    if abs(estimated_jerk) > ISO_LATERAL_JERK * 4:
+      excessive_lat_actuation = True
 
   # livePose acceleration can be noisy due to bad mounting or aliased livePose measurements
-  accel_valid = abs(CS.aEgo - accel_calibrated) < 2
+  livepose_valid = abs(CS.aEgo - accel_calibrated) < 2
+  print('excessive_long_actuation', excessive_long_actuation, 'excessive_lat_actuation', excessive_lat_actuation, 'livepose_valid', livepose_valid)
+  counter = counter + 1 if livepose_valid and (excessive_long_actuation or excessive_lat_actuation) else 0
 
-  excessive_actuation = accel_calibrated > ACCEL_MAX * 2 or accel_calibrated < ACCEL_MIN * 2
-  counter = counter + 1 if sm['carControl'].longActive and excessive_actuation and accel_valid else 0
+  if counter > 0:
+    print('counter', counter, excessive_long_actuation, excessive_lat_actuation, livepose_valid)
 
-  return counter, counter > MIN_EXCESSIVE_ACTUATION_COUNT
+  return counter, counter > MIN_EXCESSIVE_ACTUATION_COUNT, roll_compensated_lateral_accel
 
 
 class SelfdriveD:
@@ -74,6 +100,7 @@ def __init__(self, CP=None):
 
     self.pose_calibrator = PoseCalibrator()
     self.calibrated_pose: Pose | None = None
+    self.jerk_estimator = JerkEstimator3(DT_CTRL)
 
     # Setup sockets
     self.pm = messaging.PubMaster(['selfdriveState', 'onroadEvents'])
@@ -133,6 +160,7 @@ def __init__(self, CP=None):
     self.recalibrating_seen = False
     self.excessive_actuation = self.params.get("Offroad_ExcessiveActuation") is not None
     self.excessive_actuation_counter = 0
+    self.roll_compensated_lateral_accel = 0.0
     self.state_machine = StateMachine()
     self.rk = Ratekeeper(100, print_delay_threshold=None)
 
@@ -249,20 +277,23 @@ def update_events(self, CS):
       if self.sm['driverAssistance'].leftLaneDeparture or self.sm['driverAssistance'].rightLaneDeparture:
         self.events.add(EventName.ldw)
 
-    # Check for excessive (longitudinal) actuation
+    # Check for excessive actuation
     if self.sm.updated['liveCalibration']:
       self.pose_calibrator.feed_live_calib(self.sm['liveCalibration'])
     if self.sm.updated['livePose']:
       device_pose = Pose.from_live_pose(self.sm['livePose'])
       self.calibrated_pose = self.pose_calibrator.build_calibrated_pose(device_pose)
 
     if self.calibrated_pose is not None:
-      self.excessive_actuation_counter, excessive_actuation = check_excessive_actuation(self.sm, CS, self.calibrated_pose, self.excessive_actuation_counter)
+      self.excessive_actuation_counter, excessive_actuation, roll_compensated_lateral_accel = check_excessive_actuation(self.sm, CS, self.calibrated_pose,
+                                                                                                                        self.jerk_estimator, self.excessive_actuation_counter)
+      self.roll_compensated_lateral_accel = roll_compensated_lateral_accel
       if not self.excessive_actuation and excessive_actuation:
         set_offroad_alert("Offroad_ExcessiveActuation", True, extra_text="longitudinal")
         self.excessive_actuation = True
 
     if self.excessive_actuation:
+      print('EXCESSIVE ACTUATION')
       self.events.add(EventName.excessiveActuation)
 
     # Handle lane change
@@ -482,6 +513,7 @@ def publish_selfdriveState(self, CS):
     ss.active = self.active
     ss.state = self.state_machine.state
     ss.engageable = not self.events.contains(ET.NO_ENTRY)
+    ss.rollCompensatedLateralAccel = float(self.roll_compensated_lateral_accel)
     ss.experimentalMode = self.experimental_mode
     ss.personality = self.personality