mawei/openpilot-evo
1
0
Fork 0
mirror of https://github.com/infiniteCable2/openpilot.git synced 2026-07-26 11:52:04 +08:00
Code Issues Packages Projects Releases Wiki Activity

radard: filter lead prob (#37879)

Browse Source 
* 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
This commit is contained in:
Shane Smiskol
2026-05-11 00:25:15 -07:00
committed by GitHub
parent 534fb19714
commit 38ffb324f8
1 changed files with 18 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/controls/radard.py
+18 -9
View File
@@ -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