From e868baf92fadbfc7bd04a51a7397179a6380a36c Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Fri, 18 Apr 2025 19:39:10 +0200 Subject: [PATCH] Update disturbance_controller.py cleanup --- .../controls/lib/disturbance_controller.py | 40 +++++++++---------- 1 file changed, 20 insertions(+), 20 deletions(-) diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py index 7ee0ce0b8..32d5c2aa1 100644 --- a/selfdrive/controls/lib/disturbance_controller.py +++ b/selfdrive/controls/lib/disturbance_controller.py @@ -6,17 +6,17 @@ from openpilot.common.pid import PIDController from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.controls.lib.drive_helpers import MAX_CURVATURE -ALPHA_MIN = 0.004 # baseline LP‑rate -ALPHA_MAX = 0.4 # fastest LP‑rate +ALPHA_MIN = 0.004 # baseline LP rate +ALPHA_MAX = 0.4 # fastest LP rate -# Disturbance‑Observer (wind lateral force) -OBS_TAU = 0.20 # [s] filter constant (1st‑order LP on Fy_hat) -OBS_K = 7.0 # observer gain (>> 1) – higher → faster → noisier +# Disturbance Observer (wind lateral force) +OBS_TAU = 0.20 # [s] filter constant (1st border LP on Fy_hat) +OBS_K = 7.0 # observer gain (>> 1) – higher -> faster -> noisier -# Band‑pass (≈ 0.5 … 15 Hz) for dynamic‑alpha -BP_FC_HP = 0.5 # high‑pass corner [Hz] +# Band pass (≈ 0.5 ... 15 Hz) for dynamic alpha +BP_FC_HP = 0.5 # high pass corner [Hz] BP_ALPHA_HP = (1.0 / (2.0 * math.pi * BP_FC_HP * DT_CTRL)) -BP_ALPHA_HP = BP_ALPHA_HP / (1.0 + BP_ALPHA_HP) # pre‑warp for 1st‑order +BP_ALPHA_HP = BP_ALPHA_HP / (1.0 + BP_ALPHA_HP) # pre warp for 1st order KE_ENERGY = 0.25 # scaling from |ay_bp| to alpha boost # PID gains @@ -26,9 +26,9 @@ PID_KF = 0.0 class DisturbanceController: """Wind‑disturbance compensator using - ▸ 3‑DoF curvature estimate - ▸ 1st‑order Disturbance‑Observer (Fy_hat) - ▸ adaptive LP/HP separation with band‑pass energy + * 3‑DoF curvature estimate + * 1st‑order Disturbance‑Observer (Fy_hat) + * adaptive LP/HP separation with band‑pass energy """ def __init__(self, CP): @@ -38,7 +38,7 @@ class DisturbanceController: self.pid = PIDController(PID_KP, PID_KI, k_f=PID_KF, pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE) self.reaction_hist = deque([0.0], maxlen=int(round(CP.steerActuatorDelay / DT_CTRL)) + 1) # Actuator delay compensation self.Fy_hat = 0.0 # Disturbance observer state: estimated lateral wind force [N] - self.ay_hp = 0.0 # Band‑pass filter states (simple 1st‑order HP + LP energy) + self.ay_hp = 0.0 # Band pass filter states (simple 1st‑order HP + LP energy) self.ay_prev = 0.0 def reset(self): @@ -54,7 +54,7 @@ class DisturbanceController: def _update_bandpass_energy(self, ay_meas): """High‑pass filter to isolate wind‑böe frequency content (≥ 0.5 Hz).""" - # 1st‑order HP: y[n] = alpha*(y[n‑1] + x[n] - x[n‑1]) + # 1st order HP: y[n] = alpha*(y[n_1] + x[n] - x[n_1]) self.ay_hp = BP_ALPHA_HP * (self.ay_hp + ay_meas - self.ay_prev) self.ay_prev = ay_meas return abs(self.ay_hp) @@ -62,7 +62,7 @@ class DisturbanceController: def _compute_dynamic_alpha(self, energy, dt=DT_CTRL): # baseline exponential decay alpha = self.alpha_prev * math.exp(-3.0 * dt) - # energy‑based boost + # energy based boost alpha += KE_ENERGY * energy alpha = float(np.clip(alpha, ALPHA_MIN, ALPHA_MAX)) self.alpha_prev = alpha @@ -90,29 +90,29 @@ class DisturbanceController: if v_ego < 0.1: return desired_curvature - # Build actual curvature from 3‑DoF inverse model + # Build actual curvature from 3DoF inverse model steering_angle_wo_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) ay_meas = calibrated_pose.acceleration.y ay_long = calibrated_pose.acceleration.x yaw_rate = calibrated_pose.angular_velocity.yaw actual_curvature = -VM.calc_curvature_3dof(ay_meas, ay_long, yaw_rate, - v_ego, steering_angle_wo_offset, 0.0) + v_ego, steering_angle_wo_offset, 0.0) - # Disturbance observer (1st order) → ay_wind_est + # Disturbance observer (1st order) -> ay_wind_est ay_cmd = desired_curvature * v_ego * v_ego ay_wind = ay_meas - ay_cmd - # Fy_hat dynamics: F̂̇ = -F̂/τ + k*(m*ay_wind) + # Fy_hat dynamics: F = -F/tau + k*(m*ay_wind) m = VM.m self.Fy_hat += DT_CTRL * (-self.Fy_hat / OBS_TAU + OBS_K * (m * ay_wind)) ay_wind_est = self.Fy_hat / m - # immediate feed‑forward curvature correction + # immediate feed forward curvature correction curv_ff = -ay_wind_est / (v_ego * v_ego + 1e-3) desired_curvature_ff = desired_curvature + curv_ff - # LP/HP separation with adaptive alpha (band‑pass energy) + # LP/HP separation with adaptive alpha (band pass energy) energy = self._update_bandpass_energy(ay_meas) alpha = self._compute_dynamic_alpha(energy)