|
|
@ -19,8 +19,8 @@ def clamp(val, min_val, max_val): |
|
|
|
clamped_val = float(np.clip(val, min_val, max_val)) |
|
|
|
clamped_val = float(np.clip(val, min_val, max_val)) |
|
|
|
return clamped_val, clamped_val != val |
|
|
|
return clamped_val, clamped_val != val |
|
|
|
|
|
|
|
|
|
|
|
def smooth_value(val, prev_val, tau): |
|
|
|
def smooth_value(val, prev_val, tau, dt=DT_MDL): |
|
|
|
alpha = 1 - np.exp(-DT_MDL / tau) if tau > 0 else 1 |
|
|
|
alpha = 1 - np.exp(-dt/tau) if tau > 0 else 1 |
|
|
|
return alpha * val + (1 - alpha) * prev_val |
|
|
|
return alpha * val + (1 - alpha) * prev_val |
|
|
|
|
|
|
|
|
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll): |
|
|
|
def clip_curvature(v_ego, prev_curvature, new_curvature, roll): |
|
|
|