From ba981930904360577eb0b1a7ce2044f7ea31f664 Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Sat, 19 Apr 2025 13:53:29 +0200 Subject: [PATCH] Update disturbance_controller.py reimplement old working state and a little bit of ff style disturbance correction by lat accel --- .../controls/lib/disturbance_controller.py | 119 +++++------------- 1 file changed, 31 insertions(+), 88 deletions(-) diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py index acfce5be1..b090a15ce 100644 --- a/selfdrive/controls/lib/disturbance_controller.py +++ b/selfdrive/controls/lib/disturbance_controller.py @@ -6,42 +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 - -# 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 - -# 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 -KE_ENERGY = 0.25 # scaling from |ay_bp| to alpha boost - -# PID gains -PID_KP = 1.0 -PID_KI = 0.0 #0.05 # small I to cancel steady wind offset -PID_KF = 0.0 - -FF_GAIN = 0.2 +ALPHA_MIN = 0.004 +ALPHA_MAX = 0.4 +FF_GAIN = 0.05 class DisturbanceController: - """Wind disturbance compensator using - * 3DoF curvature estimate - * 1st order Disturbance Observer (Fy_hat) - * adaptive LP/HP separation with band pass energy - """ - def __init__(self, CP): self.lowpass_filtered = 0.0 self.alpha_prev = ALPHA_MIN self.desired_curvature_prev = 0.0 - 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_prev = 0.0 + self.pid = PIDController(1, 0, k_f=0, pos_limit=MAX_CURVATURE, neg_limit=-MAX_CURVATURE) + self.reaction_hist = deque([0.0], maxlen=int(round(CP.steerActuatorDelay / DT_CTRL))+1) def reset(self): self.lowpass_filtered = 0.0 @@ -50,83 +25,51 @@ class DisturbanceController: self.pid.reset() self.reaction_hist.clear() self.reaction_hist.append(0.0) - self.Fy_hat = 0.0 - self.ay_hp = 0.0 - self.ay_prev = 0.0 - 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]) - self.ay_hp = BP_ALPHA_HP * (self.ay_hp + ay_meas - self.ay_prev) - self.ay_prev = ay_meas - return abs(self.ay_hp) - - def _compute_dynamic_alpha(self, energy, dt=DT_CTRL): - # baseline exponential decay - alpha = self.alpha_prev * math.exp(-3.0 * dt) - # energy based boost - alpha += KE_ENERGY * energy - alpha = float(np.clip(alpha, ALPHA_MIN, ALPHA_MAX)) + def compute_dynamic_alpha(self, desired_curvature, dt=DT_CTRL, A=0.02, n=2.0, beta=3.0, k=2.0): + d_desired = abs(desired_curvature - self.desired_curvature_prev) / dt + alpha_reactive = d_desired**n / (k * A) if A > 0 else 0.0 + alpha = np.clip(self.alpha_prev * np.exp(-beta * dt) + alpha_reactive, ALPHA_MIN, ALPHA_MAX) self.alpha_prev = alpha + self.desired_curvature_prev = desired_curvature return alpha - def _lowpass_filter(self, current_value, alpha): + def lowpass_filter(self, current_value, alpha): + alpha = min(alpha, ALPHA_MAX) if alpha >= ALPHA_MAX * 0.9: reset_factor = (alpha - ALPHA_MIN) / (ALPHA_MAX - ALPHA_MIN) - self.lowpass_filtered = (1.0 - reset_factor) * self.lowpass_filtered + reset_factor * current_value + self.lowpass_filtered = (1 - reset_factor) * self.lowpass_filtered + reset_factor * current_value else: - self.lowpass_filtered = (1.0 - alpha) * self.lowpass_filtered + alpha * current_value + self.lowpass_filtered = (1 - alpha) * self.lowpass_filtered + alpha * current_value return self.lowpass_filtered - @staticmethod - def _highpass_filter(current_value, lowpass_value): + def highpass_filter(self, current_value, lowpass_value): return current_value - lowpass_value def compensate(self, CS, VM, params, calibrated_pose, desired_curvature): - """Return curvature command with wind compensation.""" - - if calibrated_pose is None: + if calibrated_pose is None or CS.vEgo < 0.1: return desired_curvature - v_ego = CS.vEgo - if v_ego < 0.1: - return desired_curvature + steering_angle_without_offset = math.radians(CS.steeringAngleDeg - params.angleOffsetDeg) + actual_curvature = -VM.calc_curvature_3dof(calibrated_pose.acceleration.y, calibrated_pose.acceleration.x, + calibrated_pose.angular_velocity.yaw, CS.vEgo, steering_angle_without_offset, + 0.) - # 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) - - # Disturbance observer (1st order) -> ay_wind_est - ay_cmd = desired_curvature * v_ego * v_ego + ay_cmd = desired_curvature * CS.vEgo * CS.vEgo ay_wind = ay_meas - ay_cmd - - # 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 - curv_ff = -ay_wind_est / (v_ego * v_ego + 1e-3) * FF_GAIN - desired_curvature_ff = desired_curvature + curv_ff - - # LP/HP separation with adaptive alpha (band pass energy) - energy = self._update_bandpass_energy(ay_meas) - alpha = self._compute_dynamic_alpha(energy) - - reaction = self._lowpass_filter(actual_curvature, alpha) + raw_ff = -ay_wind / (CS.vEgo * CS.vEgo + 1e-3) + curv_ff = raw_ff * FF_GAIN + desired_ff = desired_curvature + curv_ff + + alpha = self.compute_dynamic_alpha(desired_curvature) + reaction = self.lowpass_filter(actual_curvature, alpha) self.reaction_hist.append(reaction) - disturbance = self._highpass_filter(actual_curvature, reaction) + disturbance = self.highpass_filter(actual_curvature, reaction) - # compensate actuator delay (use earliest lp value in deque) reaction_delayed = self.reaction_hist[0] + error = desired_ff - (reaction_delayed + disturbance) - # PID – track curvature with disturbance rejection - error = desired_curvature_ff - (reaction_delayed + disturbance) - output_curvature = self.pid.update(error, feedforward=desired_curvature_ff, speed=v_ego) - + output_curvature = self.pid.update(error, feedforward=desired_ff, speed=CS.vEgo) + return float(output_curvature)