From 9f3fa8ceb7f9d11ab3b82bc59c1adde91f1ca011 Mon Sep 17 00:00:00 2001 From: rav4kumar <36933347+rav4kumar@users.noreply.github.com> Date: Sun, 28 Jun 2026 10:51:48 -0700 Subject: [PATCH] feat(long): lead jitter smoother for trackId --- .../test_longitudinal_smoothing_wiring.py | 6 +- .../lib/accel_personality/constants.py | 2 +- .../lib/radar_distance/radar_distance.py | 117 +++++++++++++++++- .../tests/test_radar_distance.py | 94 +++++++++++++- 4 files changed, 210 insertions(+), 9 deletions(-) diff --git a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py index 34d4f17c40..0cb47fe46e 100644 --- a/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py +++ b/selfdrive/controls/tests/test_longitudinal_smoothing_wiring.py @@ -42,9 +42,9 @@ def test_dec_model_stop_target_not_reintroduced(): assert token not in src, f"reverted DEC model-stop-target ({token}) re-introduced in {path}" -def test_comfort_stop_and_vlead_damp_gated_off(): - # Strategy invariants (tn @ 2026-06-26): final-approach stop passes through stock (goal 6 stock-met), and the - # input-side vLead speed-damp (B) stays off pending on-road proof. Flicker-hold (A) is unaffected by either. +def test_long_feature_gates(): + # comfort_stop OFF: keep the stock smooth taper (flat-hold firms the end); farther-stop comes from the MPC + # stop-target shift instead. vLead speed-damp (B) stays OFF pending on-road proof. from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import COMFORT_STOP_ENABLED from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import VLEAD_DAMP_ENABLED diff --git a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py index 02e320b6a7..6002014def 100644 --- a/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py +++ b/sunnypilot/selfdrive/controls/lib/accel_personality/constants.py @@ -79,7 +79,7 @@ ONSET_SPREAD_JERK = 2.5 # m/s^3: rate the spread output deepens back t # (cruising / gap opening as a creeping lead pulls away / lead moving / launch) the floor eases out at the # release rate. min(plan, floor) keeps it never weaker than the plan. Replaces the old kinematic v^2/(2*gap) # enforcer, which engaged late and demanded a firm ~-1.6 grab to hit a fixed gap. Off => no-op. -COMFORT_STOP_ENABLED = False # gated off: final-approach stops pass through stock +COMFORT_STOP_ENABLED = False # off: keeps the stock smooth taper (flat-hold firms the end). Farther-stop is via the MPC stop-target shift, not this. COMFORT_STOP_V = 4.0 # m/s: only engage at/below this ego speed COMFORT_STOP_LEAD_V = 1.0 # m/s: only behind a (near-)stopped lead COMFORT_STOP_GAP = 5.0 # m: reference standstill gap (radar dRel) for the final-approach window diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py index b6f73a875e..15e654cb2d 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py @@ -36,6 +36,27 @@ SWITCH_DREL = 8.0 # m, dRel jump that means the radar switched to a # Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead. STABILITY_WINDOW = 5 # frames (~0.25s @ 20Hz) VLEAD_SPREAD = 4.0 # m/s, vLead range over the window above which the lead is "unstable" +ID_CHURN_WINDOW = 10 # frames (~0.5s) for radarTrackId-churn detection (steady lead, flipping track ids) +ID_CHURN = 3 # trackId switches in the window above which the lead is "unstable" (follow-hunting) + +# Lead jitter smoother (B2): during trackId churn the per-track dRel/vRel jitter makes the MPC hunt the follow +# gap. A short SYMMETRIC EMA on the churning lead removes the jitter so the MPC sees a steady lead and stops +# hunting. Active ONLY during churn (NOT bimodal vLead -> never averages two real tracks). Bounded symmetric +# lag ~LEAD_SMOOTH_TAU. Gated OFF by default. +LEAD_SMOOTH_ENABLED = False +LEAD_SMOOTH_TAU = 0.5 # s, EMA time constant +LEAD_SMOOTH_HOLD = 20 # frames (~1s): keep smoothing through brief churn gaps (churn toggles on/off) + +# Stop-gap bias: near a (near-)stopped lead at low speed, report dRel up to STOP_GAP_BIAS_M closer so the MPC +# runs its own smooth stop but terminates that much farther back (stock crawl-creeps to ~2m). Monotone (closer +# => brake >= stock). Ramps in over the regime edge and out as the lead moves (no step, releases on launch). +STOP_GAP_BIAS_ENABLED = False +STOP_GAP_BIAS_M = 2.0 # m: max dRel reduction = added standstill gap +STOP_BIAS_VEGO = 8.0 # m/s: only below this ego speed +STOP_BIAS_VLEAD = 1.5 # m/s: only behind a (near-)stopped lead; ramps out as vLead rises to this +STOP_BIAS_REGIME_DREL = 12.0 # m: bias ramps in below this dRel +STOP_BIAS_RAMP_BAND = 2.0 # m: ramp-in band (full offset below REGIME_DREL - RAMP_BAND) +STOP_BIAS_MIN_DREL = 2.0 # m: never report a lead closer than this class _LeadView: @@ -53,6 +74,66 @@ class _LeadView: self.modelProb = src.modelProb +class _BiasedLead: + __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') + + def __init__(self, src, dRel): + self.status = src.status + self.dRel = dRel + self.yRel = src.yRel + self.vRel = src.vRel + self.vLead = src.vLead + self.vLeadK = src.vLeadK + self.aLeadK = src.aLeadK + self.aLeadTau = src.aLeadTau + self.modelProb = src.modelProb + + +class _SmoothedLead: + __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') + + def __init__(self, src, dRel, vLead, vRel): + self.status = src.status + self.dRel = dRel + self.yRel = src.yRel + self.vRel = vRel + self.vLead = vLead + self.vLeadK = vLead + self.aLeadK = src.aLeadK + self.aLeadTau = src.aLeadTau + self.modelProb = src.modelProb + + +class _LeadSmoother: + # Short symmetric EMA on a churning lead's dRel/vLead/vRel (jitter removal). A hold keeps it active through + # brief churn gaps (churn toggles); passthrough + reset only after the hold lapses. + def __init__(self): + self._d = None + self._vl = None + self._vr = None + self._hold = 0 + + def reset(self): + self._d = None + self._vl = None + self._vr = None + self._hold = 0 + + def update(self, lead, churn: bool): + self._hold = LEAD_SMOOTH_HOLD if churn else self._hold - 1 + if self._hold <= 0 or not lead.status: + self.reset() + return lead + if self._d is None: + self._d, self._vl, self._vr = lead.dRel, lead.vLead, lead.vRel + return lead + a = DT_MDL / LEAD_SMOOTH_TAU + self._d += (lead.dRel - self._d) * a + self._vl += (lead.vLead - self._vl) * a + self._vr += (lead.vRel - self._vr) * a + return _SmoothedLead(lead, self._d, self._vl, self._vr) + + class _HeldLead: __slots__ = ('status', 'dRel', 'yRel', 'vRel', 'vLead', 'vLeadK', 'aLeadK', 'aLeadTau', 'modelProb') @@ -127,16 +208,21 @@ class _LeadHold: class _LeadStability: - # Read-only monitor: flags a bimodal/bouncing leadOne (vLead range, or repeated dRel track-switches). Telemetry. + # Read-only monitor: flags an unstable leadOne -- bimodal/bouncing vLead, dRel track-switch jumps, or + # radarTrackId churn (a steady lead flipping track ids -> vRel jitter -> follow-hunting). Telemetry only. def __init__(self): self._v = deque(maxlen=STABILITY_WINDOW) self._d = deque(maxlen=STABILITY_WINDOW) + self._id = deque(maxlen=ID_CHURN_WINDOW) self.unstable = False + self.churn = False def reset(self): self._v.clear() self._d.clear() + self._id.clear() self.unstable = False + self.churn = False def update(self, lead, v_ego: float) -> None: if not lead.status or v_ego < LOW_SPEED_PASSTHROUGH_V: @@ -144,12 +230,16 @@ class _LeadStability: return self._v.append(float(lead.vLead)) self._d.append(float(lead.dRel)) + self._id.append(int(getattr(lead, 'radarTrackId', -1))) if len(self._v) < STABILITY_WINDOW: self.unstable = False return v_spread = max(self._v) - min(self._v) d_jumps = sum(abs(b - a) > SWITCH_DREL for a, b in zip(self._d, list(self._d)[1:], strict=False)) - self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2 + ids = list(self._id) + id_churn = sum(1 for a, b in zip(ids, ids[1:], strict=False) if a != b and a > 0 and b > 0) + self.churn = id_churn >= ID_CHURN and v_spread <= VLEAD_SPREAD # steady lead, flipping ids (not bimodal) + self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2 or self.churn class RadarDistanceController: @@ -160,9 +250,12 @@ class RadarDistanceController: self._v_ego = 0.0 self._enabled = self._params.get_bool("RadarDistance") self._vlead_damp_enabled = VLEAD_DAMP_ENABLED + self._stop_gap_bias_enabled = STOP_GAP_BIAS_ENABLED + self._lead_smooth_enabled = LEAD_SMOOTH_ENABLED self._one = _LeadHold() self._two = _LeadHold() self._stability = _LeadStability() + self._smoother = _LeadSmoother() def _read_params(self) -> None: enabled = self._params.get_bool("RadarDistance") @@ -183,6 +276,20 @@ class RadarDistanceController: def lead_unstable(self) -> bool: return self._stability.unstable + def _stop_gap_bias(self, lead): + # Report a (near-)stopped lead up to STOP_GAP_BIAS_M closer at low speed, so the MPC's own smooth stop ends + # that much farther back. Monotone (only ever reports closer). No-op outside the regime / when disabled. + if not self._stop_gap_bias_enabled or not lead.status: + return lead + if lead.vLead > STOP_BIAS_VLEAD or self._v_ego > STOP_BIAS_VEGO or lead.dRel <= STOP_BIAS_MIN_DREL: + return lead + d_ramp = min(max((STOP_BIAS_REGIME_DREL - lead.dRel) / STOP_BIAS_RAMP_BAND, 0.0), 1.0) + v_ramp = min(max((STOP_BIAS_VLEAD - lead.vLead) / STOP_BIAS_VLEAD, 0.0), 1.0) + offset = STOP_GAP_BIAS_M * d_ramp * v_ramp + if offset < 0.05: + return lead + return _BiasedLead(lead, max(lead.dRel - offset, STOP_BIAS_MIN_DREL)) + def smooth_radarstate(self, radarstate): self._stability.update(radarstate.leadOne, self._v_ego) # telemetry, runs every cycle if not self._enabled: @@ -190,7 +297,11 @@ class RadarDistanceController: one = self._one.step(radarstate.leadOne) two = self._two.step(radarstate.leadTwo) if self._v_ego < LOW_SPEED_PASSTHROUGH_V: - return radarstate + one_b = self._stop_gap_bias(radarstate.leadOne) # low speed = stock lead, only the stop-gap bias + return radarstate if one_b is radarstate.leadOne else _RadarStateProxy(one_b, radarstate.leadTwo) + one = self._stop_gap_bias(one) + if self._lead_smooth_enabled: + one = self._smoother.update(one, self._stability.churn) # de-jitter a churning lead (anti follow-hunt) if not self._vlead_damp_enabled: return _RadarStateProxy(one, two) # flicker-hold (A) only return _RadarStateProxy(self._one.smooth(one), self._two.smooth(two)) diff --git a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py index 6a8c6551d5..2b88329a56 100644 --- a/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py +++ b/sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py @@ -23,9 +23,9 @@ class FakeParams: return bool(self.store.get(key, False)) -def lead(status=True, dRel=40.0, vRel=-2.0, vLead=18.0, aLeadK=0.0, aLeadTau=1.5, modelProb=0.95): +def lead(status=True, dRel=40.0, vRel=-2.0, vLead=18.0, aLeadK=0.0, aLeadTau=1.5, modelProb=0.95, radarTrackId=-1): return SimpleNamespace(status=status, dRel=dRel, yRel=0.0, vRel=vRel, vLead=vLead, vLeadK=vLead, - aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb) + aLeadK=aLeadK, aLeadTau=aLeadTau, modelProb=modelProb, radarTrackId=radarTrackId) def rs(one, two=None): @@ -164,6 +164,96 @@ def test_stability_runs_even_when_disabled(): c.smooth_radarstate(rs(lead(dRel=60.0, vLead=v))) assert c.lead_unstable() +def test_stability_flags_trackid_churn(): + c = ctrl() + for tid in (10, 20, 10, 20, 10, 20, 10, 20, 10, 20): # steady lead, radarTrackId flipping (follow-hunt) + c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=tid))) + assert c.lead_unstable() + +def test_stability_steady_id_quiet(): + c = ctrl() + for _ in range(10): + c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=10))) + assert not c.lead_unstable() # steady lead + steady id -> stable + + +# --- lead jitter smoother (B2: anti follow-hunt) ----------------------------- + +def _churn_feed(c, n=20): + out = [] + for k in range(n): + dr = 42.0 if k % 2 == 0 else 46.0 # steady ~44m lead, dRel jitter + tid = 10 if k % 2 == 0 else 20 # radarTrackId churning + out.append(c.smooth_radarstate(rs(lead(dRel=dr, vLead=27.0, vRel=0.0, radarTrackId=tid))).leadOne.dRel) + return out + +def test_lead_smooth_removes_churn_jitter(): + c = ctrl() + c._lead_smooth_enabled = True + tail = _churn_feed(c)[12:] + assert max(tail) - min(tail) < 3.0 # raw range is 4.0 -> jitter reduced + assert all(42.5 < x < 45.5 for x in tail) # pulled toward the mean ~44 + +def test_lead_smooth_off_passthrough(): + c = ctrl() # smoother off (default) + tail = _churn_feed(c)[12:] + assert {round(x, 1) for x in tail} <= {42.0, 46.0} # raw dRel, no smoothing + +def test_lead_smooth_inactive_without_churn(): + c = ctrl() + c._lead_smooth_enabled = True + out = None + for _ in range(12): + out = c.smooth_radarstate(rs(lead(dRel=44.0, vLead=27.0, radarTrackId=10))) # steady id -> no churn + assert out.leadOne.dRel == pytest.approx(44.0, abs=1e-6) # smoother inactive -> exact dRel + + +# --- stop-gap bias (smooth farther stop) ------------------------------------- + +def _biased_ctrl(v_ego=2.0): + c = ctrl() + c._stop_gap_bias_enabled = True + c._v_ego = v_ego + return c + +def _bias(c, dRel, vLead): + return c._stop_gap_bias(lead(dRel=dRel, vLead=vLead)) + +def test_stop_bias_pulls_stopped_lead_closer(): + out = _bias(_biased_ctrl(), 8.0, 0.0) + assert 2.0 <= out.dRel < 8.0 # reported closer (farther stop), floored + assert out.vLead == 0.0 and out.status # other fields preserved + +def test_stop_bias_monotone_never_farther(): + c = _biased_ctrl() + for dr in (4.0, 6.0, 8.0, 10.0, 12.0, 20.0): + assert _bias(c, dr, 0.0).dRel <= dr + 1e-6 + +def test_stop_bias_min_floor(): + assert _bias(_biased_ctrl(), 2.5, 0.0).dRel == pytest.approx(2.0, abs=1e-6) + +def test_stop_bias_off_no_change(): + c = ctrl() + c._v_ego = 2.0 + ld = lead(dRel=8.0, vLead=0.0) + assert c._stop_gap_bias(ld) is ld # default off -> exact passthrough + +def test_stop_bias_moving_lead_no_change(): + ld = lead(dRel=8.0, vLead=5.0) + assert _biased_ctrl()._stop_gap_bias(ld) is ld + +def test_stop_bias_high_speed_no_change(): + ld = lead(dRel=8.0, vLead=0.0) + assert _biased_ctrl(v_ego=15.0)._stop_gap_bias(ld) is ld + +def test_stop_bias_far_lead_no_change(): + ld = lead(dRel=30.0, vLead=0.0) + assert _biased_ctrl()._stop_gap_bias(ld) is ld # beyond regime -> no bias + +def test_stop_bias_via_smooth_radarstate_low_speed(): + out = _biased_ctrl().smooth_radarstate(rs(lead(dRel=8.0, vLead=0.0, vRel=-2.0))) + assert out.leadOne.dRel < 8.0 # biased proxy returned at low speed + def test_obstacle_monotone_during_hold(): c = ctrl()