      l_prob *= l_std_mod
      r_prob *= r_std_mod
  
      # Find current lanewidth
      self.lane_width_certainty += 0.05 * (l_prob * r_prob - self.lane_width_certainty)
      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)
      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
 
</s> remove     self.lane_width_estimate = 3.7
    self.lane_width_certainty = 1.0
 </s> add     self.lane_width_estimate = FirstOrderFilter(3.7, 9.95, DT_MDL)
    self.lane_width_certainty = FirstOrderFilter(1.0, 0.95, DT_MDL) </s> remove     self.x = (1. - self.k) * self.x + self.k * x
 </s> add     self.x = (1. - self.alpha) * self.x + self.alpha * x </s> remove   def __init__(self, x0, ts, dt):
    self.k = (dt / ts) / (1. + dt / ts)
 </s> add   def __init__(self, x0, rc, dt): </s> remove class FirstOrderFilter():
 </s> add class FirstOrderFilter:
    self.lane_width_certainty.update(l_prob * r_prob)
      l_prob *= l_std_mod
      r_prob *= r_std_mod
  
      # Find current lanewidth
    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)
      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
keep keep keep keep replace keep keep keep keep keep
 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> 
selfdrive/controls/lib/lane_planner.py