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>pull/21838/head
parent
060ba14eda
commit
c900bce1b0
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
||||
|
|
Loading…
Reference in New Issue