From 38ffb324f8c4903aaafafe1daea1192cbb886771 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 11 May 2026 00:25:15 -0700 Subject: [PATCH] radard: filter lead prob (#37879) * filter lead prob * rename * try correcting model bias * Revert "try correcting model bias" This reverts commit b5e9b7147e58f200ca2e02ccea8adf88be99e206. * fast gain slow lose * cmt * deb * rename * rename * end --- selfdrive/controls/radard.py | 27 ++++++++++++++++++--------- 1 file changed, 18 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 98fce1cb2..6450ff416 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -138,7 +138,7 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, tracks return None -def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): +def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float, lead_prob: float): lead_v_rel_pred = lead_msg.v[0] - model_v_ego return { "dRel": float(lead_msg.x[0] - RADAR_TO_CAMERA), @@ -149,7 +149,7 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa "aLeadK": float(lead_msg.a[0]), "aLeadTau": 0.3, "fcw": False, - "modelProb": float(lead_msg.prob), + "modelProb": float(lead_prob), "status": True, "radar": False, "radarTrackId": -1, @@ -157,18 +157,18 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, - model_v_ego: float, low_speed_override: bool = True) -> dict[str, Any]: + model_v_ego: float, lead_prob: float, low_speed_override: bool = True) -> dict[str, Any]: # 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_prob > .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) - elif (track is None) and ready and (lead_msg.prob > .5): - lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) + lead_dict = track.get_RadarState(lead_prob) + elif (track is None) and ready and (lead_prob > .5): + lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego, lead_prob) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] @@ -188,6 +188,7 @@ class RadarD: self.tracks: dict[int, Track] = {} self.kalman_params = KalmanParams(DT_MDL) + self.lead_prob_filters = [FirstOrderFilter(0.0, 0.2, DT_MDL) for _ in range(2)] self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) @@ -239,8 +240,16 @@ class RadarD: model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, low_speed_override=False) + for i in range(2): + # Asymmetric filter on lead prob to keep lead when uncertain + lead_prob = leads_v3[i].prob + if lead_prob > self.lead_prob_filters[i].x: + self.lead_prob_filters[i].x = lead_prob + else: + self.lead_prob_filters[i].update(lead_prob) + + self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, self.lead_prob_filters[0].x, low_speed_override=True) + self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, self.lead_prob_filters[1].x, low_speed_override=False) def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None