From 14c3da26736f65bdc9cc8727b4db5d979970efbd Mon Sep 17 00:00:00 2001 From: infiniteCable <75014343+infiniteCable@users.noreply.github.com> Date: Fri, 18 Apr 2025 11:26:25 +0200 Subject: [PATCH] Update radard.py optimization --- selfdrive/controls/radard.py | 96 ++++++++++++++++++++++++++---------- 1 file changed, 71 insertions(+), 25 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index bee424405..222a0b799 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -import math +import math, time import numpy as np from collections import deque from typing import Any @@ -18,7 +18,16 @@ from opendbc.sunnypilot.car.hyundai.values import HyundaiFlagsSP # Default lead acceleration decay set to 50% at 1s -_LEAD_ACCEL_TAU = 1.5 +_LEAD_ACCEL_TAU = 1.0 + +# Exponential decay / growth factors +TAU_GROW = 1.05 # pro Update inkrementell grösser +TAU_SHRINK= 0.90 # wenn |a| klein ⇒ schneller kleiner +TAU_MIN = 0.4 + +# Vision‑blending +BLEND_KF = 0.2 # Anteil des vorgefilterten aLeadK +BLEND_VREL_DERIV = 0.3 # Anteil Δd/Δt in vRel # radar tracks SPEED, ACCEL = 0, 1 # Kalman filter states enum @@ -29,6 +38,9 @@ V_EGO_STATIONARY = 4. # no stationary object flag below this speed RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +_prev_aLeadK = 0.0 +_prev_ts = time.monotonic() + class KalmanParams: def __init__(self, dt: float): @@ -56,11 +68,13 @@ class Track: def __init__(self, identifier: int, v_lead: float, kalman_params: KalmanParams): self.identifier = identifier self.cnt = 0 - self.aLeadTau = FirstOrderFilter(_LEAD_ACCEL_TAU, 0.45, DT_MDL) + self.aLeadTau = _LEAD_ACCEL_TAU self.K_A = kalman_params.A self.K_C = kalman_params.C self.K_K = kalman_params.K self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) + self.last_dRel = None # für derivative vRel aus Vision + self.last_t = None def update(self, d_rel: float, y_rel: float, v_rel: float, v_lead: float, measured: float): # relative values, copy @@ -78,13 +92,15 @@ class Track: self.aLeadK = float(self.kf.x[ACCEL][0]) # Learn if constant acceleration + # adaptive aLeadTau if abs(self.aLeadK) < 0.5: - self.aLeadTau.x = _LEAD_ACCEL_TAU + self.aLeadTau = min(max(self.aLeadTau, 0.05)*TAU_GROW, _LEAD_ACCEL_TAU) else: - self.aLeadTau.update(0.0) + self.aLeadTau = max(self.aLeadTau*TAU_SHRINK, TAU_MIN) self.cnt += 1 + def get_RadarState(self, model_prob: float = 0.0): return { "dRel": float(self.dRel), @@ -93,7 +109,7 @@ class Track: "vLead": float(self.vLead), "vLeadK": float(self.vLeadK), "aLeadK": float(self.aLeadK), - "aLeadTau": float(self.aLeadTau.x), + "aLeadTau": float(self.aLeadTau), "status": True, "fcw": self.is_potential_fcw(model_prob), "modelProb": model_prob, @@ -143,45 +159,75 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): - lead_v_rel_pred = lead_msg.v[0] - model_v_ego + global _prev_aLeadK, _prev_ts + now = time.monotonic() + dt = now - _prev_ts + _prev_ts = now + + # Baseline model values + d_rel = lead_msg.x[0] - RADAR_TO_CAMERA + v_rel_mod = lead_msg.v[0] - model_v_ego + a_mod = (lead_msg.a[0] if len(lead_msg.a) else 0.0) + + # Derivative‑based vRel + v_rel_der = None + if dt > 1e-3: + v_rel_der = (d_rel - getattr(get_RadarState_from_vision, 'last_d', d_rel)) / dt + get_RadarState_from_vision.last_d = d_rel + + # Blend derivative with model (saturate if None) + if v_rel_der is not None: + v_rel_pred = (1.0 - BLEND_VREL_DERIV)*v_rel_mod + BLEND_VREL_DERIV*v_rel_der + else: + v_rel_pred = v_rel_mod + + # aLeadK blending / smoothing + a_blend = (1.0 - BLEND_KF)*a_mod + BLEND_KF*_prev_aLeadK + _prev_aLeadK = a_blend + return { - "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), - "yRel": float(-lead_msg.y[0]), - "vRel": float(lead_v_rel_pred), - "vLead": float(v_ego + lead_v_rel_pred), - "vLeadK": float(v_ego + lead_v_rel_pred), - "aLeadK": float(lead_msg.a[0]), + "dRel" : float(d_rel), + "yRel" : float(-lead_msg.y[0]), + "vRel" : float(v_rel_pred), + "vLead" : float(v_ego + v_rel_pred), + "vLeadK" : float(v_ego + v_rel_pred), + "aLeadK" : float(a_blend), "aLeadTau": 0.3, - "fcw": False, + "fcw" : False, "modelProb": float(lead_msg.prob), - "status": True, - "radar": False, + "status" : True, + "radar" : False, "radarTrackId": -1, } def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, CP: structs.CarParams, CP_SP: structs.CarParamsSP, low_speed_override: bool = True) -> dict[str, Any]: + track: Track | None = None + # Determine leads, this is where the essential logic happens - if len(tracks) > 0 and ready and lead_msg.prob > .5: + if len(tracks) > 0 and ready and lead_msg.prob > 0.5: track = match_vision_to_track(v_ego, lead_msg, tracks) - else: - track = None - lead_dict = {'status': False} if track is not None: lead_dict = track.get_RadarState(lead_msg.prob) lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg) - elif (track is None) and ready and (lead_msg.prob > .5): + elif ready and lead_msg.prob > 0.5: lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + else: + lead_dict = {"status": False} + + if track is not None and lead_msg.prob > 0.5 and lead_dict["status"]: + d_vision = lead_msg.x[0] - RADAR_TO_CAMERA + if abs(track.dRel - d_vision) > 3.0: # Threshold 3 m + lead_dict = track.get_RadarState(lead_msg.prob) + lead_dict = get_custom_yrel(CP, CP_SP, lead_dict, lead_msg) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] - if len(low_speed_tracks) > 0: + if low_speed_tracks: closest_track = min(low_speed_tracks, key=lambda c: c.dRel) - - # Only choose new track if it is actually closer than the previous one - if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): + if (not lead_dict["status"]) or (closest_track.dRel < lead_dict["dRel"]): lead_dict = closest_track.get_RadarState() return lead_dict