From c0a4010bd192a223d5655e06afa475d4b2e076d1 Mon Sep 17 00:00:00 2001 From: DevTekVE Date: Sat, 29 Mar 2025 10:59:51 +0100 Subject: [PATCH] Optimize curvature filtering by adding speed-dependent logic. Introduced speed-based dynamic alpha adjustment using interpolation for smoother curvature filtering. This improves steering angle calculations by adapting filter sensitivity to vehicle speed, enhancing control performance. --- selfdrive/controls/lib/latcontrol_angle.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/selfdrive/controls/lib/latcontrol_angle.py b/selfdrive/controls/lib/latcontrol_angle.py index 4fb5ff855b..e312b18cfa 100644 --- a/selfdrive/controls/lib/latcontrol_angle.py +++ b/selfdrive/controls/lib/latcontrol_angle.py @@ -1,4 +1,5 @@ import math +import numpy as np from cereal import log from openpilot.selfdrive.controls.lib.latcontrol import LatControl @@ -12,7 +13,8 @@ class LatControlAngle(LatControl): # Initialize the filtered curvature to zero (or an appropriate initial value) self.filtered_curvature = 0.0 # Filter coefficient: adjust between 0 (very smooth) and 1 (no filtering) - self.filter_alpha = 0.1 + self.filter_speed_matrox = [0, 2.5, 8.3, 13.8, 22.22] + self.filter_alpha_matrix = [0.05, 0.1, 0.3, 0.6, 1] def update(self, active, CS, VM, params, steer_limited_by_controls, desired_curvature, calibrated_pose, curvature_limited): angle_log = log.ControlsState.LateralAngleState.new_message() @@ -23,7 +25,8 @@ class LatControlAngle(LatControl): else: angle_log.active = True # Apply exponential smoothing to the curvature - self.filtered_curvature = (self.filter_alpha * desired_curvature + (1 - self.filter_alpha) * self.filtered_curvature) + adjusted_alpha = float(np.interp(CS.vEgo, self.filter_speed_matrox, self.filter_alpha_matrix)) + self.filtered_curvature = (adjusted_alpha * desired_curvature + (1 - adjusted_alpha) * self.filtered_curvature) # Convert the smoothed curvature to a steering angle angle_steers_des = math.degrees(VM.get_steer_from_curvature(-self.filtered_curvature, CS.vEgo, params.roll)) angle_steers_des += params.angleOffsetDeg