Significant improvement on the jerkiness

selfdrive/controls/lib/latcontrol_angle.py

@@ -1,10 +1,14 @@
 import math
+import numpy as np
 
 from cereal import log
 from openpilot.selfdrive.controls.lib.latcontrol import LatControl
 
 STEER_ANGLE_SATURATION_THRESHOLD = 2.5  # Degrees
 
+# Define a speed-based scaling factor similar to low_speed_factor
+LOW_SPEED_X = [0, 10, 20, 30]   # Speed breakpoints
+LOW_SPEED_Y = [0.67, 0.77, 0.91, 1.0]  # Factor reducing influence at low speeds
 
 class LatControlAngle(LatControl):
   def __init__(self, CP, CP_SP, CI):
@@ -19,11 +23,18 @@ class LatControlAngle(LatControl):
       angle_steers_des = float(CS.steeringAngleDeg)
     else:
       angle_log.active = True
-      angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
-      angle_steers_des += params.angleOffsetDeg
+
+      # Compute the base desired steering angle
+      base_angle_steers_des = math.degrees(VM.get_steer_from_curvature(-desired_curvature, CS.vEgo, params.roll))
+      base_angle_steers_des += params.angleOffsetDeg
+
+      # Apply a low-speed factor to reduce aggressive changes at low speeds
+      low_speed_factor = np.interp(CS.vEgo, LOW_SPEED_X, LOW_SPEED_Y)
+      angle_steers_des = CS.steeringAngleDeg + low_speed_factor * (base_angle_steers_des - CS.steeringAngleDeg)
 
     angle_control_saturated = abs(angle_steers_des - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
     angle_log.saturated = bool(self._check_saturation(angle_control_saturated, CS, False, curvature_limited))
     angle_log.steeringAngleDeg = float(CS.steeringAngleDeg)
-    angle_log.steeringAngleDesiredDeg = angle_steers_des
+    angle_log.steeringAngleDesiredDeg = float(angle_steers_des)
+
     return 0, float(angle_steers_des), angle_log