mirror of
https://github.com/infiniteCable2/openpilot.git
synced 2026-08-05 16:26:10 +08:00
Update disturbance_controller.py reimplement old working state and a little bit of ff style disturbance correction by lat accel
This commit is contained in:
infiniteCable
@@ -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)
|
||||
|
||||
Reference in New Issue
Block a user