diff --git a/common/filter_simple.py b/common/filter_simple.py index 9ea6fe3070..fd63dd613c 100644 --- a/common/filter_simple.py +++ b/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] diff --git a/estimate_lat_jerk.py b/estimate_lat_jerk.py new file mode 100644 index 0000000000..4715a53af1 --- /dev/null +++ b/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() + diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index e301bcd9f9..4c3561d23d 100755 --- a/selfdrive/selfdrived/selfdrived.py +++ b/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()