feat(long): lead jitter smoother for trackId

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
 

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

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))

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()