From d5e432a05940fe9d1d2e658c8a48addff5f72d02 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 01:34:56 -0800 Subject: [PATCH 01/13] some stuff --- selfdrive/controls/radard.py | 90 ++++++++++++++++++++++++++---------- 1 file changed, 65 insertions(+), 25 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 98fce1cb26e7b5..b3ed394438920b 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -138,37 +138,76 @@ def prob(c): return None -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 - 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]), - "aLeadTau": 0.3, - "fcw": False, - "modelProb": float(lead_msg.prob), - "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, low_speed_override: bool = True) -> dict[str, Any]: +class VisionTrack: + def __init__(self): + self.distances = deque(maxlen=int(1 / DT_MDL)) + + def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): + lead_v_rel_pred = lead_msg.v[0] - model_v_ego + + d_rel = float(lead_msg.x[0] - RADAR_TO_CAMERA) + + dt = len(self.distances) * DT_MDL + if dt > 0.0: + # v_rel = (d2 - d1) / (t2 - t1) + v_rel = (d_rel - self.distances[0]) / dt + # TODO: what if ego is accelerating? + new_vlead = v_ego + v_rel + else: + new_vlead = v_ego + lead_v_rel_pred + + self.distances.append(d_rel) + + return { + "dRel": d_rel, + "yRel": float(-lead_msg.y[0]), + "vRel": float(lead_v_rel_pred), + "vLead": new_vlead, # new vlead + "vLeadK": float(v_ego + lead_v_rel_pred), # old vlead logic for comparison + "aLeadK": float(lead_msg.a[0]), + "aLeadTau": 0.3, + "fcw": False, + "modelProb": float(lead_msg.prob), + "status": True, + "radar": False, + "radarTrackId": -1, + } + + +# 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 +# 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]), +# "aLeadTau": 0.3, +# "fcw": False, +# "modelProb": float(lead_msg.prob), +# "status": True, +# "radar": False, +# "radarTrackId": -1, +# } + + +def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], vision_track, lead_msg: capnp._DynamicStructReader, + model_v_ego: float, low_speed_override: bool = True) -> tuple[dict[str, Any], Any | None]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) else: - track = None + track, vision_track = None, 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) + if vision_track is None: + vision_track = VisionTrack(lead_msg, v_ego, model_v_ego) + lead_dict = vision_track.get_RadarState(lead_msg, v_ego, model_v_ego) + # lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] @@ -179,7 +218,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn if (not lead_dict['status']) or (closest_track.dRel < lead_dict['dRel']): lead_dict = closest_track.get_RadarState() - return lead_dict + return lead_dict, vision_track class RadarD: @@ -187,6 +226,7 @@ def __init__(self, delay: float = 0.0): self.current_time = 0.0 self.tracks: dict[int, Track] = {} + self.vision_track = None self.kalman_params = KalmanParams(DT_MDL) self.v_ego = 0.0 @@ -239,8 +279,8 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): 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) + self.radar_state.leadOne, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[0], model_v_ego, low_speed_override=True) + self.radar_state.leadTwo, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[1], model_v_ego, low_speed_override=False) def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None From 04d6ed5054c95e0d141ec1db82098e1117d94ae3 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 01:44:59 -0800 Subject: [PATCH 02/13] fixes --- selfdrive/controls/radard.py | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index b3ed394438920b..12cafce383d149 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -147,14 +147,16 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod d_rel = float(lead_msg.x[0] - RADAR_TO_CAMERA) + old_v_lead = v_ego + lead_v_rel_pred + dt = len(self.distances) * DT_MDL if dt > 0.0: # v_rel = (d2 - d1) / (t2 - t1) v_rel = (d_rel - self.distances[0]) / dt - # TODO: what if ego is accelerating? - new_vlead = v_ego + v_rel + # TODO: what if ego is accelerating? use average? + v_lead = v_ego + v_rel else: - new_vlead = v_ego + lead_v_rel_pred + v_lead = old_v_lead self.distances.append(d_rel) @@ -162,9 +164,9 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod "dRel": d_rel, "yRel": float(-lead_msg.y[0]), "vRel": float(lead_v_rel_pred), - "vLead": new_vlead, # new vlead + "vLead": v_lead, # new vlead "vLeadK": float(v_ego + lead_v_rel_pred), # old vlead logic for comparison - "aLeadK": float(lead_msg.a[0]), + "aLeadK": float(lead_msg.a[0]), # this seems stable? "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), @@ -198,16 +200,18 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], vision_track, if len(tracks) > 0 and ready and lead_msg.prob > .5: track = match_vision_to_track(v_ego, lead_msg, tracks) else: - track, vision_track = None, None + track = None + + if low_speed_override and lead_msg.prob <= 0.5: + vision_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): - if vision_track is None: - vision_track = VisionTrack(lead_msg, v_ego, model_v_ego) + if vision_track is None and low_speed_override: + vision_track = VisionTrack() lead_dict = vision_track.get_RadarState(lead_msg, v_ego, model_v_ego) - # lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] From 86ba2d51451839727a9e9357b13a11734378a2e6 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 02:02:32 -0800 Subject: [PATCH 03/13] use kf --- selfdrive/controls/radard.py | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 12cafce383d149..dc82c7c03fccfe 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -10,7 +10,7 @@ from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL, Priority, config_realtime_process from openpilot.common.swaglog import cloudlog -from openpilot.common.simple_kalman import KF1D +from openpilot.common.simple_kalman import KF1D, get_kalman_gain # Default lead acceleration decay set to 50% at 1s @@ -139,8 +139,16 @@ def prob(c): class VisionTrack: - def __init__(self): + def __init__(self, lead_msg: capnp._DynamicStructReader): self.distances = deque(maxlen=int(1 / DT_MDL)) + A = [[1.0, DT_MDL], [0.0, 1.0]] + C = [[1.0, 0.0]] + Q = [[0.02, 0.0], [0.0, 0.15**2]] # Distance: 0.15m, velocity: 0.15 m/s per step (~3 m/s²) + R = 1.0**2 # Measurement noise: ~1m std dev + self.K = get_kalman_gain(DT_MDL, np.array(A), np.array(C), np.array(Q), R) + self.A, self.C = A, C[0] + d_rel = float(lead_msg.x[0] - RADAR_TO_CAMERA) + self.kf = KF1D([[d_rel], [0.0]], self.A, self.C, self.K.tolist()) def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): lead_v_rel_pred = lead_msg.v[0] - model_v_ego @@ -151,9 +159,8 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod dt = len(self.distances) * DT_MDL if dt > 0.0: - # v_rel = (d2 - d1) / (t2 - t1) - v_rel = (d_rel - self.distances[0]) / dt - # TODO: what if ego is accelerating? use average? + self.kf.update(d_rel) + v_rel = self.kf.x[1][0] v_lead = v_ego + v_rel else: v_lead = old_v_lead @@ -210,7 +217,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], vision_track, lead_dict = track.get_RadarState(lead_msg.prob) elif (track is None) and ready and (lead_msg.prob > .5): if vision_track is None and low_speed_override: - vision_track = VisionTrack() + vision_track = VisionTrack(lead_msg) lead_dict = vision_track.get_RadarState(lead_msg, v_ego, model_v_ego) if low_speed_override: From 51d3fad9a6ab0dc5b3b8edb6e987b7dc01eed74c Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 02:05:08 -0800 Subject: [PATCH 04/13] no alead for now --- selfdrive/controls/radard.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index dc82c7c03fccfe..17f54cdd602806 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -173,7 +173,7 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod "vRel": float(lead_v_rel_pred), "vLead": v_lead, # new vlead "vLeadK": float(v_ego + lead_v_rel_pred), # old vlead logic for comparison - "aLeadK": float(lead_msg.a[0]), # this seems stable? + "aLeadK": 0.0, # float(lead_msg.a[0]), # this seems stable? "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob), From b0a1905de08b4bfdc0411dc321cd093f007060ce Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 16:04:41 -0800 Subject: [PATCH 05/13] fixes --- selfdrive/controls/radard.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 17f54cdd602806..6cbcd289809127 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -140,7 +140,7 @@ def prob(c): class VisionTrack: def __init__(self, lead_msg: capnp._DynamicStructReader): - self.distances = deque(maxlen=int(1 / DT_MDL)) + self.distances = deque(maxlen=round(0.5 / DT_MDL)) A = [[1.0, DT_MDL], [0.0, 1.0]] C = [[1.0, 0.0]] Q = [[0.02, 0.0], [0.0, 0.15**2]] # Distance: 0.15m, velocity: 0.15 m/s per step (~3 m/s²) @@ -159,8 +159,9 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod dt = len(self.distances) * DT_MDL if dt > 0.0: - self.kf.update(d_rel) - v_rel = self.kf.x[1][0] + #self.kf.update(d_rel) + #v_rel = self.kf.x[1][0] + v_rel = (d_rel - self.distances[0]) / dt v_lead = v_ego + v_rel else: v_lead = old_v_lead @@ -291,7 +292,7 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: self.radar_state.leadOne, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[0], model_v_ego, low_speed_override=True) - self.radar_state.leadTwo, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[1], model_v_ego, low_speed_override=False) + # self.radar_state.leadTwo, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[1], model_v_ego, low_speed_override=False) def publish(self, pm: messaging.PubMaster): assert self.radar_state is not None From c2375ba829140345e807c93945eb80f40d649b63 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 22:36:23 -0800 Subject: [PATCH 06/13] explore --- voacc_debugging.py | 65 ++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 65 insertions(+) create mode 100644 voacc_debugging.py diff --git a/voacc_debugging.py b/voacc_debugging.py new file mode 100644 index 00000000000000..4bac7ba2aa93fb --- /dev/null +++ b/voacc_debugging.py @@ -0,0 +1,65 @@ +from tools.lib.logreader import LogReader +import matplotlib.pyplot as plt +from selfdrive.controls.radard import RADAR_TO_CAMERA + +plt.ion() + +# Camry w/ accurate lead from radar +# lr = LogReader('https://connect.comma.ai/3f447b402cbe27b6/0000008d--b6a2350b41/351/448', sort_by_time=True) +lr = LogReader('https://connect.comma.ai/3f447b402cbe27b6/0000008d--b6a2350b41/572/950', sort_by_time=True) +lr = sorted(lr, key=lambda m: m.logMonoTime) + +radar_data = [] +model_data = [] + +for msg in lr: + if msg.which() == 'radarState': + RS = msg.radarState + + if RS.leadOne.status: + radar_data.append((msg.logMonoTime, RS.leadOne.dRel, RS.leadOne.vLead, RS.leadOne.aLeadK)) + + elif msg.which() == 'modelV2': + MD = msg.modelV2 + + if len(MD.leadsV3): + lead = MD.leadsV3[0] + if lead.prob > 0.5: + model_data.append((msg.logMonoTime, lead.x[0] - RADAR_TO_CAMERA, lead.v[0], lead.a[0])) + +fig, ax = plt.subplots(3, 1, sharex=True) +ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel', + # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' + ) +ax[0].plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel', + # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' + ) +ax[0].set_ylabel('dRel (m)') +ax[0].legend() + +ax[1].plot([d[0] for d in radar_data], [d[2] for d in radar_data], label='radar vLead', + # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' + ) +ax[1].plot([d[0] for d in model_data], [d[2] for d in model_data], label='model vLead', + # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' + ) +ax[1].set_ylabel('vLead (m/s)') +ax[1].legend() + +ax[2].plot([d[0] for d in radar_data], [d[3] for d in radar_data], label='radar aLeadK', + # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' + ) +ax[2].plot([d[0] for d in model_data], [d[3] for d in model_data], label='model aLead', + # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' + ) +ax[2].set_ylabel('aLead (m/s²)') +ax[2].legend() + +# plt.plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel', +# # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' +# ) +# plt.plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel', +# # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' +# ) +# plt.legend() +# plt.show() From 1234a09869ea407efbb78185bb178144a9eddd4a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 22:48:37 -0800 Subject: [PATCH 07/13] simple kf (not really) --- voacc_debugging.py | 47 ++++++++++++++++++++++++++++++++++++++-------- 1 file changed, 39 insertions(+), 8 deletions(-) diff --git a/voacc_debugging.py b/voacc_debugging.py index 4bac7ba2aa93fb..cf743b44789e1c 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -1,6 +1,8 @@ -from tools.lib.logreader import LogReader +from collections import deque import matplotlib.pyplot as plt +from tools.lib.logreader import LogReader from selfdrive.controls.radard import RADAR_TO_CAMERA +from openpilot.common.realtime import DT_MDL plt.ion() @@ -9,11 +11,19 @@ lr = LogReader('https://connect.comma.ai/3f447b402cbe27b6/0000008d--b6a2350b41/572/950', sort_by_time=True) lr = sorted(lr, key=lambda m: m.logMonoTime) -radar_data = [] -model_data = [] +CS = None + +radar_data = [] # from radard/radar +model_data = [] # straight from model +pred_data = [] # kf predictions derived from model + +dRel_deque = deque(maxlen=round(1.0 / DT_MDL)) for msg in lr: - if msg.which() == 'radarState': + if msg.which() == 'carState': + CS = msg.carState + + elif msg.which() == 'radarState': RS = msg.radarState if RS.leadOne.status: @@ -22,10 +32,25 @@ elif msg.which() == 'modelV2': MD = msg.modelV2 - if len(MD.leadsV3): - lead = MD.leadsV3[0] - if lead.prob > 0.5: - model_data.append((msg.logMonoTime, lead.x[0] - RADAR_TO_CAMERA, lead.v[0], lead.a[0])) + if CS is None: + continue + + if not len(MD.leadsV3): + continue + + lead = MD.leadsV3[0] + if lead.prob > 0.5: + dRel = lead.x[0] - RADAR_TO_CAMERA + model_data.append((msg.logMonoTime, dRel, lead.v[0], lead.a[0])) + + # simple kf prediction for vlead + if len(dRel_deque) == dRel_deque.maxlen: + vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) + pred_data.append((msg.logMonoTime, dRel, vLead, lead.a[0])) + + dRel_deque.append(dRel) + else: + dRel_deque.clear() fig, ax = plt.subplots(3, 1, sharex=True) ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel', @@ -43,6 +68,9 @@ ax[1].plot([d[0] for d in model_data], [d[2] for d in model_data], label='model vLead', # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' ) +ax[1].plot([d[0] for d in pred_data], [d[2] for d in pred_data], label='predicted vLead', + # marker='o', markersize=2, markeredgecolor='green', markerfacecolor='green' + ) ax[1].set_ylabel('vLead (m/s)') ax[1].legend() @@ -52,6 +80,9 @@ ax[2].plot([d[0] for d in model_data], [d[3] for d in model_data], label='model aLead', # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' ) +# ax[2].plot([d[0] for d in pred_data], [d[3] for d in pred_data], label='predicted aLead', +# # marker='o', markersize=2, markeredgecolor='green', markerfacecolor='green' +# ) ax[2].set_ylabel('aLead (m/s²)') ax[2].legend() From 370ae8283f2609acc907ab5edd5cf839ae0aa644 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 23:24:13 -0800 Subject: [PATCH 08/13] stash --- voacc_debugging.py | 75 ++++++++++++++++++++++++---------------------- 1 file changed, 40 insertions(+), 35 deletions(-) diff --git a/voacc_debugging.py b/voacc_debugging.py index cf743b44789e1c..a411f474a4b344 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -1,8 +1,10 @@ +import numpy as np from collections import deque import matplotlib.pyplot as plt from tools.lib.logreader import LogReader from selfdrive.controls.radard import RADAR_TO_CAMERA from openpilot.common.realtime import DT_MDL +from openpilot.common.simple_kalman import KF1D, get_kalman_gain plt.ion() @@ -17,8 +19,18 @@ model_data = [] # straight from model pred_data = [] # kf predictions derived from model +# deque (noisy) dRel_deque = deque(maxlen=round(1.0 / DT_MDL)) +# kf (inaccurate?) +A = [[1.0, DT_MDL], [0.0, 1.0]] +C = [[1.0, 0.0]] +Q = [[0.02, 0.0], [0.0, 0.15 ** 2]] # Distance: 0.15m, velocity: 0.15 m/s per step (~3 m/s²) +R = 1.0 ** 2 # Measurement noise: ~1m std dev +K = get_kalman_gain(DT_MDL, np.array(A), np.array(C), np.array(Q), R) +A, C = A, C[0] +kf = KF1D([[0.0], [0.0]], A, C, K.tolist()) + for msg in lr: if msg.which() == 'carState': CS = msg.carState @@ -41,56 +53,49 @@ lead = MD.leadsV3[0] if lead.prob > 0.5: dRel = lead.x[0] - RADAR_TO_CAMERA - model_data.append((msg.logMonoTime, dRel, lead.v[0], lead.a[0])) + model_data.append((msg.logMonoTime, dRel, lead.v[0], lead.a[0], lead.xStd[0])) # simple kf prediction for vlead if len(dRel_deque) == dRel_deque.maxlen: - vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) - pred_data.append((msg.logMonoTime, dRel, vLead, lead.a[0])) + # vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) + + kf.update(dRel) + kf_dRel = kf.x[0][0] + kf_vLead = CS.vEgo + kf.x[1][0] + + pred_data.append((msg.logMonoTime, kf_dRel, kf_vLead, lead.a[0])) dRel_deque.append(dRel) else: dRel_deque.clear() fig, ax = plt.subplots(3, 1, sharex=True) -ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel', - # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' - ) -ax[0].plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel', - # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' - ) +ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel') +ax[0].plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel') +ax[0].plot([d[0] for d in pred_data], [d[1] for d in pred_data], label='predicted dRel') ax[0].set_ylabel('dRel (m)') ax[0].legend() -ax[1].plot([d[0] for d in radar_data], [d[2] for d in radar_data], label='radar vLead', - # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' - ) -ax[1].plot([d[0] for d in model_data], [d[2] for d in model_data], label='model vLead', - # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' - ) -ax[1].plot([d[0] for d in pred_data], [d[2] for d in pred_data], label='predicted vLead', - # marker='o', markersize=2, markeredgecolor='green', markerfacecolor='green' - ) +ax[1].plot([d[0] for d in radar_data], [d[2] for d in radar_data], label='radar vLead') +ax[1].plot([d[0] for d in model_data], [d[2] for d in model_data], label='model vLead') +ax[1].plot([d[0] for d in pred_data], [d[2] for d in pred_data], label='predicted vLead') ax[1].set_ylabel('vLead (m/s)') ax[1].legend() -ax[2].plot([d[0] for d in radar_data], [d[3] for d in radar_data], label='radar aLeadK', - # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' - ) -ax[2].plot([d[0] for d in model_data], [d[3] for d in model_data], label='model aLead', - # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' - ) -# ax[2].plot([d[0] for d in pred_data], [d[3] for d in pred_data], label='predicted aLead', -# # marker='o', markersize=2, markeredgecolor='green', markerfacecolor='green' -# ) +ax[2].plot([d[0] for d in radar_data], [d[3] for d in radar_data], label='radar aLeadK') +ax[2].plot([d[0] for d in model_data], [d[3] for d in model_data], label='model aLead') +# ax[2].plot([d[0] for d in pred_data], [d[3] for d in pred_data], label='predicted aLead') ax[2].set_ylabel('aLead (m/s²)') ax[2].legend() -# plt.plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel', -# # marker='o', markersize=2, markeredgecolor='orange', markerfacecolor='orange' -# ) -# plt.plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel', -# # marker='o', markersize=2, markeredgecolor='blue', markerfacecolor='blue' -# ) -# plt.legend() -# plt.show() +# prob +# ax[3].plot([d[0] for d in model_data], [d[4] for d in model_data], label='model lead prob') +# ax[3].set_ylabel('lead prob') +# ax[3].legend() + +# xStd +# ax[3].plot([d[0] for d in model_data], [d[4] for d in model_data], label='model lead xStd') +# ax[3].set_ylabel('lead xStd (m)') +# ax[3].legend() + +# TODO: print some stats about how close model and kf are from radar ground truths From 25bbbc2aea48e5d7f3a0033599b637f4a9b7891a Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Sun, 28 Dec 2025 23:49:02 -0800 Subject: [PATCH 09/13] actually not bad --- voacc_debugging.py | 47 +++++++++++++++++++++++++++++++--------------- 1 file changed, 32 insertions(+), 15 deletions(-) diff --git a/voacc_debugging.py b/voacc_debugging.py index a411f474a4b344..4ee8b56b9050a0 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -22,14 +22,30 @@ # deque (noisy) dRel_deque = deque(maxlen=round(1.0 / DT_MDL)) + # kf (inaccurate?) -A = [[1.0, DT_MDL], [0.0, 1.0]] -C = [[1.0, 0.0]] -Q = [[0.02, 0.0], [0.0, 0.15 ** 2]] # Distance: 0.15m, velocity: 0.15 m/s per step (~3 m/s²) -R = 1.0 ** 2 # Measurement noise: ~1m std dev -K = get_kalman_gain(DT_MDL, np.array(A), np.array(C), np.array(Q), R) -A, C = A, C[0] -kf = KF1D([[0.0], [0.0]], A, C, K.tolist()) +class KF: + MIN_STD = 0.5 + MAX_STD = 15 + + def __init__(self): + self.x = 0.0 # state estimate # TODO: initialize properly + self.P = 0.0 # variance of the state estimate + self.Q = 0.1 # process variance per step + + def update(self, x, x_std): + P_pred = self.P + self.Q + + x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) + R = x_std ** 2 + K = P_pred / (P_pred + R) + + self.x = self.x + K * (x - self.x) + self.P = (1.0 - K) * P_pred + return self.x + + +kf = KF() for msg in lr: if msg.which() == 'carState': @@ -59,17 +75,18 @@ if len(dRel_deque) == dRel_deque.maxlen: # vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) - kf.update(dRel) - kf_dRel = kf.x[0][0] - kf_vLead = CS.vEgo + kf.x[1][0] + kf_dRel = kf.update(dRel, lead.xStd[0]) + print(dRel, kf_dRel) + # kf_dRel = kf.x[0][0] + # kf_vLead = CS.vEgo + kf.x[1][0] - pred_data.append((msg.logMonoTime, kf_dRel, kf_vLead, lead.a[0])) + pred_data.append((msg.logMonoTime, kf_dRel, lead.v[0], lead.a[0])) dRel_deque.append(dRel) else: dRel_deque.clear() -fig, ax = plt.subplots(3, 1, sharex=True) +fig, ax = plt.subplots(4, 1, sharex=True) ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel') ax[0].plot([d[0] for d in model_data], [d[1] for d in model_data], label='model dRel') ax[0].plot([d[0] for d in pred_data], [d[1] for d in pred_data], label='predicted dRel') @@ -94,8 +111,8 @@ # ax[3].legend() # xStd -# ax[3].plot([d[0] for d in model_data], [d[4] for d in model_data], label='model lead xStd') -# ax[3].set_ylabel('lead xStd (m)') -# ax[3].legend() +ax[3].plot([d[0] for d in model_data], [d[4] for d in model_data], label='model lead xStd') +ax[3].set_ylabel('lead xStd (m)') +ax[3].legend() # TODO: print some stats about how close model and kf are from radar ground truths From 34bb6336ac0c15fd7d6543288e831dbc9480c66f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 29 Dec 2025 00:43:57 -0800 Subject: [PATCH 10/13] predict vRel --- voacc_debugging.py | 69 +++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 62 insertions(+), 7 deletions(-) diff --git a/voacc_debugging.py b/voacc_debugging.py index 4ee8b56b9050a0..adb92e771e5dab 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -18,6 +18,7 @@ radar_data = [] # from radard/radar model_data = [] # straight from model pred_data = [] # kf predictions derived from model +kf_data = [] # internal kf state to visualize # deque (noisy) dRel_deque = deque(maxlen=round(1.0 / DT_MDL)) @@ -27,23 +28,69 @@ class KF: MIN_STD = 0.5 MAX_STD = 15 + SIGMA_A = 2.5 # m/s^2, how much can lead relative velocity realistically change (not per step) + DT = DT_MDL def __init__(self): - self.x = 0.0 # state estimate # TODO: initialize properly - self.P = 0.0 # variance of the state estimate - self.Q = 0.1 # process variance per step + self.x = 0.0 # distance state estimate (m) # TODO: initialize properly + self.vRel = 0.0 # relative velocity state estimate (m/s) + self.P = np.diag([10.0 ** 2, 5.0 ** 2]) # variance of the state estimate. x uncertainty: 10m, vRel uncertainty: 5 m/s + self.K = 0.0 # Kalman gain + + # TODO: understand this fully + var_a = self.SIGMA_A ** 2 # acceleration variance + self.Q = var_a * np.array([ + [self.DT ** 4 / 4.0, self.DT ** 3 / 2.0], # affects x (m^2) and coupling + [self.DT ** 3 / 2.0, self.DT ** 2], # affects vRel ((m/s)^2) + ],) def update(self, x, x_std): - P_pred = self.P + self.Q + # predict state + self.x = self.x + self.vRel * self.DT + + # predict covariance + A = np.array([[1.0, self.DT], + [0.0, 1.0]]) + + self.P = A @ self.P @ A.T + self.Q + + # update + H = np.array([[1.0, 0.0]]) x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) R = x_std ** 2 - K = P_pred / (P_pred + R) - self.x = self.x + K * (x - self.x) - self.P = (1.0 - K) * P_pred + x_pred = (H @ np.array([[self.x], [self.vRel]]))[0, 0] + + S = (H @ self.P @ H.T)[0, 0] + R + + K = (self.P @ H.T) / S + + y = x - x_pred # how far meas is form prediction + self.x = self.x + K[0, 0] * y + self.vRel = self.vRel + K[1, 0] * y + + # update covariance + I = np.eye(2) + self.P = (I - K @ H) @ self.P + return self.x + # def update(self, x, x_std): + # P_pred = self.P + self.Q + # + # x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) + # R = x_std ** 2 + # self.K = P_pred / (P_pred + R) + # + # self.x = self.x + self.K * (x - self.x) + # self.P = (1.0 - self.K) * P_pred + # + # # velocity estimate + # x_pred = self.x + self.vRel * self.DT + # vRel_pred = self.vRel + # return self.x + kf = KF() @@ -76,6 +123,8 @@ def update(self, x, x_std): # vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) kf_dRel = kf.update(dRel, lead.xStd[0]) + + kf_data.append((msg.logMonoTime, kf.P, kf.K)) print(dRel, kf_dRel) # kf_dRel = kf.x[0][0] # kf_vLead = CS.vEgo + kf.x[1][0] @@ -115,4 +164,10 @@ def update(self, x, x_std): ax[3].set_ylabel('lead xStd (m)') ax[3].legend() +# # kf internal stats +# ax[4].plot([d[0] for d in kf_data], [d[1] for d in kf_data], label='kf P') +# ax[4].plot([d[0] for d in kf_data], [d[2] for d in kf_data], label='kf K') +# ax[4].set_ylabel('kf stats') +# ax[4].legend() + # TODO: print some stats about how close model and kf are from radar ground truths From 92c36a44df84eccc55deaf7fa58341cd8598d9a0 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 29 Dec 2025 01:09:41 -0800 Subject: [PATCH 11/13] kinda works --- voacc_debugging.py | 29 ++++++++++++++++++++--------- 1 file changed, 20 insertions(+), 9 deletions(-) diff --git a/voacc_debugging.py b/voacc_debugging.py index adb92e771e5dab..c975e659b820d8 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -1,6 +1,7 @@ import numpy as np from collections import deque import matplotlib.pyplot as plt + from tools.lib.logreader import LogReader from selfdrive.controls.radard import RADAR_TO_CAMERA from openpilot.common.realtime import DT_MDL @@ -11,6 +12,9 @@ # Camry w/ accurate lead from radar # lr = LogReader('https://connect.comma.ai/3f447b402cbe27b6/0000008d--b6a2350b41/351/448', sort_by_time=True) lr = LogReader('https://connect.comma.ai/3f447b402cbe27b6/0000008d--b6a2350b41/572/950', sort_by_time=True) + +# tesla with no radar +# lr = LogReader('https://connect.comma.ai/0f79c454f812791a/000000a3--e1e54de187/1037/1067', sort_by_time=True) lr = sorted(lr, key=lambda m: m.logMonoTime) CS = None @@ -26,13 +30,13 @@ # kf (inaccurate?) class KF: - MIN_STD = 0.5 + MIN_STD = 1 MAX_STD = 15 SIGMA_A = 2.5 # m/s^2, how much can lead relative velocity realistically change (not per step) DT = DT_MDL - def __init__(self): - self.x = 0.0 # distance state estimate (m) # TODO: initialize properly + def __init__(self, x): + self.x = x # distance state estimate (m) self.vRel = 0.0 # relative velocity state estimate (m/s) self.P = np.diag([10.0 ** 2, 5.0 ** 2]) # variance of the state estimate. x uncertainty: 10m, vRel uncertainty: 5 m/s self.K = 0.0 # Kalman gain @@ -44,9 +48,11 @@ def __init__(self): [self.DT ** 3 / 2.0, self.DT ** 2], # affects vRel ((m/s)^2) ],) - def update(self, x, x_std): + def update(self, x, x_std, a_ego): # predict state - self.x = self.x + self.vRel * self.DT + # self.x = self.x + self.vRel * self.DT + self.x = self.x + self.vRel * self.DT - 0.5 * a_ego * self.DT * self.DT + self.vRel = self.vRel - a_ego * self.DT # predict covariance A = np.array([[1.0, self.DT], @@ -74,7 +80,7 @@ def update(self, x, x_std): I = np.eye(2) self.P = (I - K @ H) @ self.P - return self.x + return self.x, self.vRel # def update(self, x, x_std): # P_pred = self.P + self.Q @@ -92,7 +98,7 @@ def update(self, x, x_std): # return self.x -kf = KF() +kf = None for msg in lr: if msg.which() == 'carState': @@ -116,24 +122,29 @@ def update(self, x, x_std): lead = MD.leadsV3[0] if lead.prob > 0.5: dRel = lead.x[0] - RADAR_TO_CAMERA + if kf is None: + kf = KF(dRel) + model_data.append((msg.logMonoTime, dRel, lead.v[0], lead.a[0], lead.xStd[0])) # simple kf prediction for vlead if len(dRel_deque) == dRel_deque.maxlen: # vLead = CS.vEgo + (dRel - dRel_deque[0]) / (DT_MDL * len(dRel_deque)) - kf_dRel = kf.update(dRel, lead.xStd[0]) + kf_dRel, kf_vRel = kf.update(dRel, lead.xStd[0], CS.aEgo) kf_data.append((msg.logMonoTime, kf.P, kf.K)) print(dRel, kf_dRel) # kf_dRel = kf.x[0][0] # kf_vLead = CS.vEgo + kf.x[1][0] + kf_vLead = CS.vEgo + kf_vRel - pred_data.append((msg.logMonoTime, kf_dRel, lead.v[0], lead.a[0])) + pred_data.append((msg.logMonoTime, kf_dRel, kf_vLead, lead.a[0])) dRel_deque.append(dRel) else: dRel_deque.clear() + kf = None fig, ax = plt.subplots(4, 1, sharex=True) ax[0].plot([d[0] for d in radar_data], [d[1] for d in radar_data], label='radar dRel') From f767e03c81ffa9f07c49b612c346575391b0b1f2 Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 29 Dec 2025 01:19:03 -0800 Subject: [PATCH 12/13] passes run process on route --- selfdrive/controls/radard.py | 84 ++++++++++++++++++++++++++++-------- voacc_debugging.py | 75 ++------------------------------ 2 files changed, 70 insertions(+), 89 deletions(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 6cbcd289809127..7d7d2d971d876a 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -26,6 +26,60 @@ RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +class VisionLeadKF: + MIN_STD = 1 + MAX_STD = 15 + SIGMA_A = 2.5 # m/s^2, how much can lead relative velocity realistically change (not per step) + DT = DT_MDL + + def __init__(self, x): + self.x = x # distance state estimate (m) + self.vRel = 0.0 # relative velocity state estimate (m/s) + self.P = np.diag([10.0 ** 2, 5.0 ** 2]) # variance of the state estimate. x uncertainty: 10m, vRel uncertainty: 5 m/s + self.K = 0.0 # Kalman gain + + # TODO: understand this fully + var_a = self.SIGMA_A ** 2 # acceleration variance + self.Q = var_a * np.array([ + [self.DT ** 4 / 4.0, self.DT ** 3 / 2.0], # affects x (m^2) and coupling + [self.DT ** 3 / 2.0, self.DT ** 2], # affects vRel ((m/s)^2) + ],) + + def update(self, x, x_std, a_ego): + # predict state + # self.x = self.x + self.vRel * self.DT + self.x = self.x + self.vRel * self.DT - 0.5 * a_ego * self.DT * self.DT + self.vRel = self.vRel - a_ego * self.DT + + # predict covariance + A = np.array([[1.0, self.DT], + [0.0, 1.0]]) + + self.P = A @ self.P @ A.T + self.Q + + # update + H = np.array([[1.0, 0.0]]) + + x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) + R = x_std ** 2 + + x_pred = (H @ np.array([[self.x], [self.vRel]]))[0, 0] + + S = (H @ self.P @ H.T)[0, 0] + R + + K = (self.P @ H.T) / S + + y = x - x_pred # how far meas is form prediction + self.x = self.x + K[0, 0] * y + self.vRel = self.vRel + K[1, 0] * y + + # update covariance + I = np.eye(2) + self.P = (I - K @ H) @ self.P + + return self.x, self.vRel + + class KalmanParams: def __init__(self, dt: float): # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. @@ -141,16 +195,10 @@ def prob(c): class VisionTrack: def __init__(self, lead_msg: capnp._DynamicStructReader): self.distances = deque(maxlen=round(0.5 / DT_MDL)) - A = [[1.0, DT_MDL], [0.0, 1.0]] - C = [[1.0, 0.0]] - Q = [[0.02, 0.0], [0.0, 0.15**2]] # Distance: 0.15m, velocity: 0.15 m/s per step (~3 m/s²) - R = 1.0**2 # Measurement noise: ~1m std dev - self.K = get_kalman_gain(DT_MDL, np.array(A), np.array(C), np.array(Q), R) - self.A, self.C = A, C[0] d_rel = float(lead_msg.x[0] - RADAR_TO_CAMERA) - self.kf = KF1D([[d_rel], [0.0]], self.A, self.C, self.K.tolist()) + self.kf = VisionLeadKF(d_rel) - def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, model_v_ego: float): + def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, a_ego: float, model_v_ego: float): lead_v_rel_pred = lead_msg.v[0] - model_v_ego d_rel = float(lead_msg.x[0] - RADAR_TO_CAMERA) @@ -159,20 +207,20 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod dt = len(self.distances) * DT_MDL if dt > 0.0: - #self.kf.update(d_rel) - #v_rel = self.kf.x[1][0] - v_rel = (d_rel - self.distances[0]) / dt - v_lead = v_ego + v_rel + kf_dRel, kf_vRel = self.kf.update(d_rel, lead_msg.xStd[0], a_ego) + # v_rel = (d_rel - self.distances[0]) / dt + v_lead = v_ego + kf_vRel else: + kf_dRel = d_rel v_lead = old_v_lead self.distances.append(d_rel) return { - "dRel": d_rel, + "dRel": float(kf_dRel), "yRel": float(-lead_msg.y[0]), "vRel": float(lead_v_rel_pred), - "vLead": v_lead, # new vlead + "vLead": float(v_lead), # new vlead "vLeadK": float(v_ego + lead_v_rel_pred), # old vlead logic for comparison "aLeadK": 0.0, # float(lead_msg.a[0]), # this seems stable? "aLeadTau": 0.3, @@ -202,7 +250,7 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, mod # } -def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], vision_track, lead_msg: capnp._DynamicStructReader, +def get_lead(v_ego: float, a_ego: float, ready: bool, tracks: dict[int, Track], vision_track, lead_msg: capnp._DynamicStructReader, model_v_ego: float, low_speed_override: bool = True) -> tuple[dict[str, Any], Any | None]: # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > .5: @@ -219,7 +267,7 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], vision_track, elif (track is None) and ready and (lead_msg.prob > .5): if vision_track is None and low_speed_override: vision_track = VisionTrack(lead_msg) - lead_dict = vision_track.get_RadarState(lead_msg, v_ego, model_v_ego) + lead_dict = vision_track.get_RadarState(lead_msg, v_ego, a_ego, model_v_ego) if low_speed_override: low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] @@ -242,6 +290,7 @@ def __init__(self, delay: float = 0.0): self.kalman_params = KalmanParams(DT_MDL) self.v_ego = 0.0 + self.a_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL))+1) self.last_v_ego_frame = -1 @@ -256,6 +305,7 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): if sm.recv_frame['carState'] != self.last_v_ego_frame: self.v_ego = sm['carState'].vEgo + self.a_ego = sm['carState'].aEgo self.v_ego_hist.append(self.v_ego) self.last_v_ego_frame = sm.recv_frame['carState'] @@ -291,7 +341,7 @@ def update(self, sm: messaging.SubMaster, rr: car.RadarData): model_v_ego = self.v_ego leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: - self.radar_state.leadOne, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[0], model_v_ego, low_speed_override=True) + self.radar_state.leadOne, self.vision_track = get_lead(self.v_ego, self.a_ego, self.ready, self.tracks, self.vision_track, leads_v3[0], model_v_ego, low_speed_override=True) # self.radar_state.leadTwo, self.vision_track = get_lead(self.v_ego, self.ready, self.tracks, self.vision_track, leads_v3[1], model_v_ego, low_speed_override=False) def publish(self, pm: messaging.PubMaster): diff --git a/voacc_debugging.py b/voacc_debugging.py index c975e659b820d8..287cb83ca9e5cd 100644 --- a/voacc_debugging.py +++ b/voacc_debugging.py @@ -3,7 +3,7 @@ import matplotlib.pyplot as plt from tools.lib.logreader import LogReader -from selfdrive.controls.radard import RADAR_TO_CAMERA +from selfdrive.controls.radard import RADAR_TO_CAMERA, VisionLeadKF from openpilot.common.realtime import DT_MDL from openpilot.common.simple_kalman import KF1D, get_kalman_gain @@ -28,76 +28,7 @@ dRel_deque = deque(maxlen=round(1.0 / DT_MDL)) -# kf (inaccurate?) -class KF: - MIN_STD = 1 - MAX_STD = 15 - SIGMA_A = 2.5 # m/s^2, how much can lead relative velocity realistically change (not per step) - DT = DT_MDL - - def __init__(self, x): - self.x = x # distance state estimate (m) - self.vRel = 0.0 # relative velocity state estimate (m/s) - self.P = np.diag([10.0 ** 2, 5.0 ** 2]) # variance of the state estimate. x uncertainty: 10m, vRel uncertainty: 5 m/s - self.K = 0.0 # Kalman gain - - # TODO: understand this fully - var_a = self.SIGMA_A ** 2 # acceleration variance - self.Q = var_a * np.array([ - [self.DT ** 4 / 4.0, self.DT ** 3 / 2.0], # affects x (m^2) and coupling - [self.DT ** 3 / 2.0, self.DT ** 2], # affects vRel ((m/s)^2) - ],) - - def update(self, x, x_std, a_ego): - # predict state - # self.x = self.x + self.vRel * self.DT - self.x = self.x + self.vRel * self.DT - 0.5 * a_ego * self.DT * self.DT - self.vRel = self.vRel - a_ego * self.DT - - # predict covariance - A = np.array([[1.0, self.DT], - [0.0, 1.0]]) - - self.P = A @ self.P @ A.T + self.Q - - # update - H = np.array([[1.0, 0.0]]) - - x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) - R = x_std ** 2 - - x_pred = (H @ np.array([[self.x], [self.vRel]]))[0, 0] - - S = (H @ self.P @ H.T)[0, 0] + R - - K = (self.P @ H.T) / S - - y = x - x_pred # how far meas is form prediction - self.x = self.x + K[0, 0] * y - self.vRel = self.vRel + K[1, 0] * y - - # update covariance - I = np.eye(2) - self.P = (I - K @ H) @ self.P - - return self.x, self.vRel - - # def update(self, x, x_std): - # P_pred = self.P + self.Q - # - # x_std = np.clip(x_std, self.MIN_STD, self.MAX_STD) - # R = x_std ** 2 - # self.K = P_pred / (P_pred + R) - # - # self.x = self.x + self.K * (x - self.x) - # self.P = (1.0 - self.K) * P_pred - # - # # velocity estimate - # x_pred = self.x + self.vRel * self.DT - # vRel_pred = self.vRel - # return self.x - - +# kf kf = None for msg in lr: @@ -123,7 +54,7 @@ def update(self, x, x_std, a_ego): if lead.prob > 0.5: dRel = lead.x[0] - RADAR_TO_CAMERA if kf is None: - kf = KF(dRel) + kf = VisionLeadKF(dRel) model_data.append((msg.logMonoTime, dRel, lead.v[0], lead.a[0], lead.xStd[0])) From 62b9238043bd60213b852975edfe685c5683d06f Mon Sep 17 00:00:00 2001 From: Shane Smiskol Date: Mon, 29 Dec 2025 01:19:15 -0800 Subject: [PATCH 13/13] this is prob better than 0 --- selfdrive/controls/radard.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 7d7d2d971d876a..4b9279ee94cd6c 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -222,7 +222,7 @@ def get_RadarState(self, lead_msg: capnp._DynamicStructReader, v_ego: float, a_e "vRel": float(lead_v_rel_pred), "vLead": float(v_lead), # new vlead "vLeadK": float(v_ego + lead_v_rel_pred), # old vlead logic for comparison - "aLeadK": 0.0, # float(lead_msg.a[0]), # this seems stable? + "aLeadK": float(lead_msg.a[0]), # this seems stable? "aLeadTau": 0.3, "fcw": False, "modelProb": float(lead_msg.prob),