Longitudinal planner: make v_desired a FirstOrderFilter (#23341)

* make v_desired a FirstOrderFilter

* forgot one

* one more ref

* Add a new object for the filter

* fix

* fix tests

* update ref

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/controls/lib/longitudinal_planner.py

@@ -4,6 +4,7 @@ import numpy as np
 from common.numpy_fast import interp
 
 import cereal.messaging as messaging
+from common.filter_simple import FirstOrderFilter
 from common.realtime import DT_MDL
 from selfdrive.modeld.constants import T_IDXS
 from selfdrive.config import Conversions as CV
@@ -48,9 +49,8 @@ class Planner:
 
     self.fcw = False
 
-    self.v_desired = init_v
     self.a_desired = init_a
-    self.alpha = np.exp(-DT_MDL / 2.0)
+    self.v_desired_filter = FirstOrderFilter(init_v, 2.0, DT_MDL)
 
     self.v_desired_trajectory = np.zeros(CONTROL_N)
     self.a_desired_trajectory = np.zeros(CONTROL_N)
@@ -69,14 +69,13 @@ class Planner:
 
     prev_accel_constraint = True
     if long_control_state == LongCtrlState.off or sm['carState'].gasPressed:
-      self.v_desired = v_ego
+      self.v_desired_filter.x = v_ego
       self.a_desired = a_ego
       # Smoothly changing between accel trajectory is only relevant when OP is driving
       prev_accel_constraint = False
 
     # Prevent divergence, smooth in current v_ego
-    self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
-    self.v_desired = max(0.0, self.v_desired)
+    self.v_desired_filter.x = max(0.0, self.v_desired_filter.update(v_ego))
 
     accel_limits = [A_CRUISE_MIN, get_max_accel(v_ego)]
     accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
@@ -88,7 +87,7 @@ class Planner:
     accel_limits_turns[0] = min(accel_limits_turns[0], self.a_desired + 0.05)
     accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
     self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
-    self.mpc.set_cur_state(self.v_desired, self.a_desired)
+    self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired)
     self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
     self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
     self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
@@ -102,7 +101,7 @@ class Planner:
     # Interpolate 0.05 seconds and save as starting point for next iteration
     a_prev = self.a_desired
     self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
-    self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
+    self.v_desired_filter.x = self.v_desired_filter.x + DT_MDL * (self.a_desired + a_prev) / 2.0
 
   def publish(self, sm, pm):
     plan_send = messaging.new_message('longitudinalPlan')

selfdrive/test/longitudinal_maneuvers/plant.py

@@ -97,7 +97,7 @@ class Plant():
           'carState': car_state.carState,
           'controlsState': control.controlsState}
     self.planner.update(sm)
-    self.speed = self.planner.v_desired
+    self.speed = self.planner.v_desired_filter.x
     self.acceleration = self.planner.a_desired
     fcw = self.planner.fcw
     self.distance_lead = self.distance_lead + v_lead * self.ts

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-80430e515ea7ca44f2c73f047692d357856e74c1
+67534ae58a87b6993a0a9653d255e629785a07c3