Use first order filter (#21411)

* Use first order filter in INDI

* use first order filter in LP as well

* Update selfdrive/controls/lib/lane_planner.py

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

* RC->rc

* division safe

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

common/filter_simple.py

@@ -1,9 +1,13 @@
-class FirstOrderFilter():
+class FirstOrderFilter:
   # first order filter
-  def __init__(self, x0, ts, dt):
-    self.k = (dt / ts) / (1. + dt / ts)
+  def __init__(self, x0, rc, dt):
     self.x = x0
+    self.dt = dt
+    self.update_alpha(rc)
+
+  def update_alpha(self, rc):
+    self.alpha = self.dt / (rc + self.dt)
 
   def update(self, x):
-    self.x = (1. - self.k) * self.x + self.k * x
+    self.x = (1. - self.alpha) * self.x + self.alpha * x
     return self.x

selfdrive/controls/lib/lane_planner.py

@@ -1,8 +1,10 @@
-from common.numpy_fast import interp
 import numpy as np
+from cereal import log
+from common.filter_simple import FirstOrderFilter
+from common.numpy_fast import interp
+from common.realtime import DT_MDL
 from selfdrive.hardware import EON, TICI
 from selfdrive.swaglog import cloudlog
-from cereal import log
 
 
 TRAJECTORY_SIZE = 33
@@ -24,8 +26,8 @@ class LanePlanner:
     self.ll_x = np.zeros((TRAJECTORY_SIZE,))
     self.lll_y = np.zeros((TRAJECTORY_SIZE,))
     self.rll_y = np.zeros((TRAJECTORY_SIZE,))
-    self.lane_width_estimate = 3.7
-    self.lane_width_certainty = 1.0
+    self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
+    self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL)
     self.lane_width = 3.7
 
     self.lll_prob = 0.
@@ -79,12 +81,12 @@ class LanePlanner:
     r_prob *= r_std_mod
 
     # Find current lanewidth
-    self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
+    self.lane_width_certainty.update(l_prob * r_prob)
     current_lane_width = abs(self.rll_y[0] - self.lll_y[0])
-    self.lane_width_estimate += 0.005 * (current_lane_width - self.lane_width_estimate)
+    self.lane_width_estimate.update(current_lane_width)
     speed_lane_width = interp(v_ego, [0., 31.], [2.8, 3.5])
-    self.lane_width = self.lane_width_certainty * self.lane_width_estimate + \
-                      (1 - self.lane_width_certainty) * speed_lane_width
+    self.lane_width = self.lane_width_certainty.x * self.lane_width_estimate.x + \
+                      (1 - self.lane_width_certainty.x) * speed_lane_width
 
     clipped_lane_width = min(4.0, self.lane_width)
     path_from_left_lane = self.lll_y + clipped_lane_width / 2.0

selfdrive/controls/lib/latcontrol_indi.py

@@ -2,10 +2,11 @@ import math
 import numpy as np
 
 from cereal import log
-from common.realtime import DT_CTRL
+from common.filter_simple import FirstOrderFilter
 from common.numpy_fast import clip, interp
-from selfdrive.car.toyota.values import CarControllerParams
+from common.realtime import DT_CTRL
 from selfdrive.car import apply_toyota_steer_torque_limits
+from selfdrive.car.toyota.values import CarControllerParams
 from selfdrive.controls.lib.drive_helpers import get_steer_max
 
 
@@ -43,6 +44,7 @@ class LatControlINDI():
 
     self.sat_count_rate = 1.0 * DT_CTRL
     self.sat_limit = CP.steerLimitTimer
+    self.steer_filter = FirstOrderFilter(0., self.RC, DT_CTRL)
 
     self.reset()
 
@@ -63,9 +65,9 @@ class LatControlINDI():
     return interp(self.speed, self._inner_loop_gain[0], self._inner_loop_gain[1])
 
   def reset(self):
-    self.delayed_output = 0.
+    self.steer_filter.x = 0.
     self.output_steer = 0.
-    self.sat_count = 0.0
+    self.sat_count = 0.
     self.speed = 0.
 
   def _check_saturation(self, control, check_saturation, limit):
@@ -96,14 +98,14 @@ class LatControlINDI():
     if CS.vEgo < 0.3 or not active:
       indi_log.active = False
       self.output_steer = 0.0
-      self.delayed_output = 0.0
+      self.steer_filter.x = 0.0
     else:
 
       rate_des = VM.get_steer_from_curvature(-curvature_rate, CS.vEgo)
 
       # Expected actuator value
-      alpha = 1. - DT_CTRL / (self.RC + DT_CTRL)
-      self.delayed_output = self.delayed_output * alpha + self.output_steer * (1. - alpha)
+      self.steer_filter.update_alpha(self.RC)
+      self.steer_filter.update(self.output_steer)
 
       # Compute acceleration error
       rate_sp = self.outer_loop_gain * (steers_des - self.x[0]) + rate_des
@@ -121,12 +123,12 @@ class LatControlINDI():
       # Enforce rate limit
       if self.enforce_rate_limit:
         steer_max = float(CarControllerParams.STEER_MAX)
-        new_output_steer_cmd = steer_max * (self.delayed_output + delta_u)
+        new_output_steer_cmd = steer_max * (self.steer_filter.x + delta_u)
         prev_output_steer_cmd = steer_max * self.output_steer
         new_output_steer_cmd = apply_toyota_steer_torque_limits(new_output_steer_cmd, prev_output_steer_cmd, prev_output_steer_cmd, CarControllerParams)
         self.output_steer = new_output_steer_cmd / steer_max
       else:
-        self.output_steer = self.delayed_output + delta_u
+        self.output_steer = self.steer_filter.x + delta_u
 
       steers_max = get_steer_max(CP, CS.vEgo)
       self.output_steer = clip(self.output_steer, -steers_max, steers_max)
@@ -135,7 +137,7 @@ class LatControlINDI():
       indi_log.rateSetPoint = float(rate_sp)
       indi_log.accelSetPoint = float(accel_sp)
       indi_log.accelError = float(accel_error)
-      indi_log.delayedOutput = float(self.delayed_output)
+      indi_log.delayedOutput = float(self.steer_filter.x)
       indi_log.delta = float(delta_u)
       indi_log.output = float(self.output_steer)