diff --git a/selfdrive/controls/lib/disturbance_controller.py b/selfdrive/controls/lib/disturbance_controller.py index e8490ee72..7ee0ce0b8 100644 --- a/selfdrive/controls/lib/disturbance_controller.py +++ b/selfdrive/controls/lib/disturbance_controller.py @@ -6,16 +6,40 @@ 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 -ALPHA_MAX = 0.4 +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.05 # small I to cancel steady wind offset +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 + """ + def __init__(self, CP): self.lowpass_filtered = 0.0 self.alpha_prev = ALPHA_MIN self.desired_curvature_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) + 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 def reset(self): self.lowpass_filtered = 0.0 @@ -24,44 +48,83 @@ 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 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) + 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)) self.alpha_prev = alpha - self.desired_curvature_prev = desired_curvature return alpha - def lowpass_filter(self, current_value, alpha): - alpha = min(alpha, ALPHA_MAX) + def _lowpass_filter(self, current_value, alpha): if alpha >= ALPHA_MAX * 0.9: reset_factor = (alpha - ALPHA_MIN) / (ALPHA_MAX - ALPHA_MIN) - self.lowpass_filtered = (1 - reset_factor) * self.lowpass_filtered + reset_factor * current_value + self.lowpass_filtered = (1.0 - reset_factor) * self.lowpass_filtered + reset_factor * current_value else: - self.lowpass_filtered = (1 - alpha) * self.lowpass_filtered + alpha * current_value + self.lowpass_filtered = (1.0 - alpha) * self.lowpass_filtered + alpha * current_value return self.lowpass_filtered - def highpass_filter(self, current_value, lowpass_value): + @staticmethod + def _highpass_filter(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: 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.) - - alpha = self.compute_dynamic_alpha(desired_curvature) - reaction = self.lowpass_filter(actual_curvature, alpha) + v_ego = CS.vEgo + if v_ego < 0.1: + return desired_curvature + + # Build actual curvature from 3‑DoF 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_wind = ay_meas - ay_cmd + + # Fy_hat dynamics: F̂̇ = -F̂/τ + 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) + 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) 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_curvature - (reaction_delayed + disturbance) - output_curvature = self.pid.update(error, feedforward=desired_curvature, speed=CS.vEgo) - + # 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) + return float(output_curvature)