feat(long): proactive closing jerk

sunnypilot/selfdrive/controls/lib/accel_personality/accel_controller.py

@@ -8,7 +8,10 @@ Acceleration Personality (ECO / NORMAL / SPORT). Tunes only MPC INPUTS, never th
   * positive-accel ceiling + speed-dependent per-cycle open-rate -> tier-scaled take-off from a stop
     (the open-rate is fast near v=0 so launch is never delayed, tapering to a steady-state rate at speed);
   * jerk-cost relaxation (scales the core MPC's jerk_factor) -> smooth accel/decel onset: near a stop, on
-    any fresh accel<->decel direction change, and when the tracked lead is itself braking hard;
+    any fresh accel<->decel direction change, when the tracked lead is itself braking hard, or when the gap
+    is closing fast for any other reason (cut-in, ego overtaking a slower lead) -- the last one is the only
+    proactive trigger keyed on an MPC INPUT (vRel) rather than a_ego's own realized sign flip, so it can
+    soften the very first brake jab instead of only the recovery after it;
   * add-only, speed-dependent follow-gap widen on the MPC t_follow -> earlier/gentler braking, roomier gap;
   * sticky should_stop hysteresis -> no stop-and-go gas-brake-gas-brake.
 Add-only gap => desired distance >= stock => braking >= stock. Disabled => stock everywhere (byte-stock).
@@ -24,8 +27,8 @@ from openpilot.sunnypilot import get_sanitize_int_param
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
   NORMAL, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, A_CRUISE_MAX_V, STOCK_A_CRUISE_MAX_V, \
   RISE_RATE_BP, RISE_RATE_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \
-  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, \
-  TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A
+  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, CLOSING_VREL_BP, CLOSING_FACTOR_V, TF_WIDEN_V_BP, \
+  TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A
 
 
 class _OnsetRelax:
@@ -69,6 +72,7 @@ class AccelController:
     self._onset_relax = _OnsetRelax()
     self._onset_factor = 1.0
     self._lead_brake_factor = 1.0
+    self._closing_factor = 1.0
     self._read_params()
 
   def _read_params(self) -> None:
@@ -85,12 +89,15 @@ class AccelController:
     self._a_ego = float(sm['carState'].aEgo)
 
     if self._enabled:
+      lead = sm['radarState'].leadOne
       self._onset_factor = self._onset_relax.update(self._a_ego, ONSET_FLOOR[self._personality])
-      self._lead_brake_factor = self._get_lead_brake_factor(sm['radarState'].leadOne)
+      self._lead_brake_factor = self._get_lead_brake_factor(lead)
+      self._closing_factor = self._get_closing_factor(lead)
     else:
       self._onset_relax.reset()
       self._onset_factor = 1.0
       self._lead_brake_factor = 1.0
+      self._closing_factor = 1.0
 
     self._frame += 1
 
@@ -99,12 +106,18 @@ class AccelController:
       return 1.0
     return float(np.interp(lead.aLeadK, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V[self._personality]))
 
+  def _get_closing_factor(self, lead) -> float:
+    if not lead.status:
+      return 1.0
+    return float(np.interp(lead.vRel, CLOSING_VREL_BP, CLOSING_FACTOR_V[self._personality]))
+
   def reset(self) -> None:
     # Drop the accumulated widen (e.g. on disengage / standstill re-init) so it re-ramps cleanly.
     self._widen = 0.0
     self._onset_relax.reset()
     self._onset_factor = 1.0
     self._lead_brake_factor = 1.0
+    self._closing_factor = 1.0
 
   def get_max_accel(self, v_ego: float) -> float:
     # Disabled -> stock ceiling (off == stock, independent of the NORMAL profile so NORMAL is free to differ).
@@ -119,13 +132,14 @@ class AccelController:
     return float(np.interp(v_ego, RISE_RATE_BP, RISE_RATE_V[self._personality]))
 
   def get_jerk_scale(self, v_ego: float) -> float:
-    # Disabled -> 1.0 -> byte-stock jerk cost. Enabled: takes the most-relaxed of three tier-scaled factors
-    # -- near a stop (v_ego), a fresh accel<->decel onset (any speed), and a hard-braking lead -- each never
-    # exceeding 1.0 (stock), so this only ever relaxes jerk cost, never tightens it beyond stock.
+    # Disabled -> 1.0 -> byte-stock jerk cost. Enabled: takes the most-relaxed of four tier-scaled factors --
+    # near a stop (v_ego), a fresh accel<->decel onset (any speed), a hard-braking lead, and a fast-closing
+    # gap (any cause) -- each never exceeding 1.0 (stock), so this only ever relaxes jerk cost, never tightens
+    # it beyond stock.
     if not self._enabled:
       return 1.0
     near_stop = float(np.interp(v_ego, JERK_SCALE_BP, JERK_SCALE_V[self._personality]))
-    return min(near_stop, self._onset_factor, self._lead_brake_factor)
+    return min(near_stop, self._onset_factor, self._lead_brake_factor, self._closing_factor)
 
   def get_t_follow(self, t_follow: float, v_ego: float) -> float:
     # MPC t_follow hook. Adds a slewed, decel-held, speed-dependent comfort widen on top of the stock

sunnypilot/selfdrive/controls/lib/accel_personality/constants.py

@@ -84,6 +84,20 @@ LEAD_BRAKE_FACTOR_V = {
   SPORT:  [0.45, 1.0],
 }
 
+# --- Closing-rate jerk-cost relaxation (MPC INPUT: react faster to a fast-closing gap, any cause) ----------
+# Complements LEAD_BRAKE_FACTOR_V, which keys off the LEAD's own deceleration: a gap can close quickly for
+# reasons aLeadK never reflects (a cut-in, or ego simply catching up faster than the lead is slowing). Onset
+# relax (above) only reacts the cycle AFTER a_ego has already crossed its deadband -- reactive on a realized
+# signal, so it structurally can't soften the very first jab into a fresh, fast-closing gap. vRel is an MPC
+# INPUT (causal, known before any brake is commanded), so keying off it directly closes that gap. No lead, or
+# not closing past the gate -> 1.0. Disabled -> 1.0.
+CLOSING_VREL_BP = [-6.0, -1.5]          # m/s, closing rate (negative = closing), ascending for np.interp
+CLOSING_FACTOR_V = {
+  ECO:    [0.75, 1.0],
+  NORMAL: [0.60, 1.0],
+  SPORT:  [0.45, 1.0],
+}
+
 # --- Follow-gap widen (add-only, fed to the MPC t_follow) ------------------------------------------------
 # Add a small speed-dependent widen to the stock t_follow (the driver's gap-button value). Wider gap ->
 # MPC brakes earlier + gentler onto a slowing lead and settles a roomier cruise gap. Invariants:

sunnypilot/selfdrive/controls/lib/accel_personality/tests/test_accel_controller.py

@@ -20,8 +20,8 @@ from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.accel_control
 from openpilot.sunnypilot.selfdrive.controls.lib.accel_personality.constants import \
   ECO, NORMAL, SPORT, PERSONALITY_MIN, PERSONALITY_MAX, A_CRUISE_MAX_BP, RISE_RATE_V, \
   STOCK_A_CRUISE_MAX_V, STOCK_RISE_RATE, JERK_SCALE_BP, JERK_SCALE_V, ONSET_DEADBAND, ONSET_RAMP_S, \
-  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, TF_WIDEN_V_BP, TF_WIDEN_BASE_V, TF_WIDEN_TIER, \
-  TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality
+  ONSET_FLOOR, LEAD_BRAKE_ALEAD_BP, LEAD_BRAKE_FACTOR_V, CLOSING_VREL_BP, CLOSING_FACTOR_V, TF_WIDEN_V_BP, \
+  TF_WIDEN_BASE_V, TF_WIDEN_TIER, TF_WIDEN_MAX, TF_SLEW_PER_S, TF_DECEL_HOLD_A, AccelerationPersonality
 
 _EPS = 1e-6
 _TF_STOCK = 1.45          # a representative stock t_follow (standard personality); the widen is add-only on top
@@ -42,8 +42,8 @@ class FakeParams:
     self.store[key] = val
 
 
-def make_lead(status=False, aLeadK=0.0):
-  return SimpleNamespace(status=status, aLeadK=aLeadK)
+def make_lead(status=False, aLeadK=0.0, vRel=0.0):
+  return SimpleNamespace(status=status, aLeadK=aLeadK, vRel=vRel)
 
 
 def make_sm(v_ego=20.0, a_ego=0.0, lead=None):
@@ -287,6 +287,56 @@ def test_lead_brake_matches_constants_table():
       assert ctrl.get_jerk_scale(20.0) == pytest.approx(expected)
 
 
+# --- closing-rate relax: fast-closing gap relaxes jerk cost proactively, any cause -------------------------
+
+def test_closing_no_lead_is_stock():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=False, vRel=-8.0)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
+def test_closing_relaxes_with_fast_closing_lead():
+  for personality, floor in ((ECO, 0.75), (NORMAL, 0.60), (SPORT, 0.45)):
+    ctrl = make_controller(personality=personality)
+    ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0)))
+    assert ctrl.get_jerk_scale(20.0) == pytest.approx(floor)
+
+
+def test_closing_slow_closing_is_stock():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-0.5)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)   # above the -1.5 gate -> no relax
+
+
+def test_closing_opening_gap_is_stock():
+  ctrl = make_controller(personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=3.0)))   # lead pulling away
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
+def test_closing_matches_constants_table():
+  for personality in (ECO, NORMAL, SPORT):
+    ctrl = make_controller(personality=personality)
+    for v_rel in (-1.5, -3.0, -6.0):
+      ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=v_rel)))
+      expected = np.interp(v_rel, CLOSING_VREL_BP, CLOSING_FACTOR_V[personality])
+      assert ctrl.get_jerk_scale(20.0) == pytest.approx(expected)
+
+
+def test_closing_disabled_is_stock():
+  ctrl = make_controller(enabled=False, personality=SPORT)
+  ctrl.update(make_sm(v_ego=20.0, lead=make_lead(status=True, vRel=-6.0)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)
+
+
+def test_closing_fires_before_a_ego_moves():
+  # The whole point: on the VERY FIRST cycle a fast-closing lead appears, before a_ego has had any chance to
+  # react (still 0.0, so onset-relax is untouched) -- the closing factor alone must already be relaxed.
+  ctrl = make_controller(personality=NORMAL)
+  ctrl.update(make_sm(v_ego=20.0, a_ego=0.0, lead=make_lead(status=True, vRel=-6.0)))
+  assert ctrl.get_jerk_scale(20.0) == pytest.approx(CLOSING_FACTOR_V[NORMAL][0])
+
+
 # --- combined: get_jerk_scale takes the most-relaxed of all three factors ----------------------------------
 
 def test_combined_takes_most_relaxed_factor():
@@ -300,7 +350,7 @@ def test_combined_takes_most_relaxed_factor():
 def test_reset_clears_onset_and_lead_brake_state():
   ctrl = make_controller(personality=SPORT)
   ctrl.update(make_sm(v_ego=20.0, a_ego=1.0))
-  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0)))
+  ctrl.update(make_sm(v_ego=20.0, a_ego=-1.0, lead=make_lead(status=True, aLeadK=-3.0, vRel=-6.0)))
   assert ctrl.get_jerk_scale(20.0) < 1.0 - _EPS
   ctrl.reset()
   assert ctrl.get_jerk_scale(20.0) == pytest.approx(1.0)

sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py

@@ -12,10 +12,15 @@ reports a farther-or-faster lead than reality, so braking is always >= stock. Fo
     out. A closer jump of any size always passes immediately -- this only ever delays relief, never a brake;
   * flicker-hold: keep a just-dropped, recently-sustained lead alive (dead-reckoned) through a brief radar
     dropout so the MPC does not lose and re-grab it (which reads as a phantom release then a catch-up brake);
-  * churn smoother: a short EMA on a trackId-churning lead's dRel/vLead/vRel so the MPC stops hunting the gap
-    (removes the follow-jitter that reads as rubber-banding). dRel is asymmetric -- closer accepted
-    immediately, only farther is EMA-lagged -- so it can't hold a steadily-closing lead farther-than-true;
-    vLead/vRel stay symmetric (secondary terms, no demonstrated need to bias them);
+  * churn/noise smoother: a short EMA on a lead's dRel/vLead/vRel so the MPC stops hunting the gap (removes
+    the follow-jitter that reads as rubber-banding and, on the sensor side, as a lead-detection "lurch").
+    Covers two DISTINCT same-physical-object noise signatures: trackId churn (id flips frame-to-frame but the
+    kinematics stay coherent -- one real lead getting re-labeled) and same-track noise (id stays constant but
+    vLead itself is bimodal/bouncing -- one real lead with a noisy fusion/Doppler velocity read). Both are
+    safe to EMA because the id evidence pins them to a SINGLE physical object; a bimodal vLead WITH the id
+    also changing is left alone (ambiguous -- could be two really-different real objects) so this can never
+    average two real tracks together. dRel is asymmetric -- closer accepted immediately, only farther is
+    EMA-lagged -- so it can't hold a steadily-closing lead farther-than-true; vLead/vRel stay symmetric;
   * stop-gap: near a (near-)stopped lead at low speed report dRel a touch closer so the MPC's own smooth stop
     settles farther back (the Prius TSS2 stock crawl creeps in to ~1.5 m). Monotone (closer => brake >= stock).
     Overridden off by sustained lead motion (even slow creep) so it can't suppress a real, growing gap during
@@ -134,12 +139,13 @@ class _RadarStateProxy:
 
 
 class _LeadSmoother:
-  # 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. dRel is ASYMMETRIC: a closer raw
-  # reading is accepted immediately (never delay awareness of closer -- the file's own invariant), only a
-  # FARTHER raw reading is EMA-lagged (reject noise in that direction). Without this, a lead that's genuinely
-  # closing steadily while churn is (even briefly) active gets held farther-than-true for the full
-  # LEAD_SMOOTH_HOLD window, then snaps -- a false-relief-then-correction that itself becomes a hard brake.
+  # EMA on a noisy same-physical-object lead's dRel/vLead/vRel (jitter removal; see _LeadStability for what
+  # qualifies as "same object"). A hold keeps it active through brief noise gaps (the trigger toggles on/off);
+  # passthrough + reset only after the hold lapses. dRel is ASYMMETRIC: a closer raw reading is accepted
+  # immediately (never delay awareness of closer -- the file's own invariant), only a FARTHER raw reading is
+  # EMA-lagged (reject noise in that direction). Without this, a lead that's genuinely closing steadily while
+  # noisy (even briefly) gets held farther-than-true for the full LEAD_SMOOTH_HOLD window, then snaps -- a
+  # false-relief-then-correction that itself becomes a hard brake.
   def __init__(self):
     self._d = None
     self._vl = None
@@ -152,8 +158,8 @@ class _LeadSmoother:
     self._vr = None
     self._hold = 0
 
-  def update(self, lead, churn: bool):
-    self._hold = LEAD_SMOOTH_HOLD if churn else self._hold - 1
+  def update(self, lead, noisy: bool):
+    self._hold = LEAD_SMOOTH_HOLD if noisy else self._hold - 1
     if self._hold <= 0 or not lead.status:
       self.reset()
       return lead
@@ -234,12 +240,21 @@ class _LeadHold:
 class _LeadStability:
   # 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.
+  # Also derives same_track_noise: a bimodal/bouncing vLead while radarTrackId sat CONSTANT the whole window
+  # -- i.e. the id evidence pins the noise to one physical object (a Doppler/fusion-noisy velocity read on one
+  # real lead), so it is safe to feed the smoother (see _LeadSmoother). A bimodal vLead WITH the id also
+  # changing stays outside same_track_noise (could be two really-different real objects at different speeds)
+  # and is left unmitigated, same as before. dRel track-jumps are deliberately excluded here: while status
+  # stays True (this class's own precondition), a repeated FARTHER dRel jump is already absorbed by
+  # _JumpGuard upstream (same SWITCH_DREL threshold), so adding it here would just double up on the same
+  # signal rather than covering a real gap.
   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
+    self.same_track_noise = False
 
   def reset(self):
     self._v.clear()
@@ -247,6 +262,7 @@ class _LeadStability:
     self._id.clear()
     self.unstable = False
     self.churn = False
+    self.same_track_noise = False
 
   def update(self, lead, v_ego: float) -> None:
     if not lead.status or v_ego < CREEP_PASSTHROUGH_V:
@@ -262,7 +278,10 @@ class _LeadStability:
     d_jumps = sum(abs(b - a) > SWITCH_DREL for a, b in zip(self._d, list(self._d)[1:], strict=False))
     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)
+    recent_ids = ids[-STABILITY_WINDOW:]
+    same_track = recent_ids[0] > 0 and len(set(recent_ids)) == 1
     self.churn = id_churn >= ID_CHURN and v_spread <= VLEAD_SPREAD   # steady lead, flipping ids (not bimodal)
+    self.same_track_noise = same_track and v_spread > VLEAD_SPREAD
     self.unstable = v_spread > VLEAD_SPREAD or d_jumps >= 2 or self.churn
 
 
@@ -333,14 +352,15 @@ class RadarDistanceController:
     if not self._enabled:
       return radarstate                                       # off: byte-stock passthrough
     two = radarstate.leadTwo
+    noisy = self._stability.churn or self._stability.same_track_noise
     if self._v_ego >= LOW_SPEED_PASSTHROUGH_V:
       one = self._jump_guard.step(radarstate.leadOne)         # reject a same-cycle farther-jump transient ...
       one = self._one.step(one)                               # ... + flicker-hold ...
       two = self._two.step(radarstate.leadTwo)
-      one = self._smoother.update(one, self._stability.churn) # ... + churn de-jitter (anti follow-hunt)
+      one = self._smoother.update(one, noisy)                 # ... + same-object de-jitter (anti follow-hunt)
     elif self._v_ego >= CREEP_PASSTHROUGH_V:
-      # creep band: churn de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch)
-      one = self._smoother.update(radarstate.leadOne, self._stability.churn)
+      # creep band: de-jitter ONLY (symmetric EMA), no flicker-hold (a stale held lead would delay launch)
+      one = self._smoother.update(radarstate.leadOne, noisy)
     else:
       one = radarstate.leadOne                                # full standstill: no hold/smoothing
     one = self._stop_gap_bias(one)                            # low-speed near-stopped: settle farther back

sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py

@@ -464,3 +464,39 @@ def test_stability_runs_even_when_disabled():
   for i in range(10):
     c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0)))
   assert c.lead_unstable()                           # telemetry not gated by the RadarDistance param
+
+
+# --- same-track noise smoother (bimodal vLead / repeated dRel jump on a CONSTANT radarTrackId) -------------
+
+def test_smoother_dejitters_bimodal_vlead_on_same_track():
+  # Same physical object (radarTrackId constant) but a bouncing velocity read (Doppler/fusion noise) -- the
+  # id evidence pins this to ONE real lead, so it's safe to EMA (unlike a bimodal read with a changing id).
+  c = ctrl()
+  out = None
+  for i in range(30):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0, vRel=-1.0, radarTrackId=9)))
+  assert c.lead_unstable()
+  assert 10.0 < out.leadOne.vLead < 18.0             # EMA settled between the two bouncing readings
+  assert out.leadOne.vLead not in (10.0, 18.0)
+
+
+def test_smoother_inactive_on_bimodal_vlead_with_changing_track():
+  # Same bimodal vLead signature, but radarTrackId ALSO changes -- ambiguous (could be two really-different
+  # real objects at different speeds), so this must NOT be smoothed, unlike the same-track case above.
+  c = ctrl()
+  one = lead(dRel=40.0, vLead=18.0, radarTrackId=1)
+  for i in range(10):
+    c.smooth_radarstate(rs(lead(dRel=40.0, vLead=18.0 if i % 2 else 10.0, radarTrackId=1 if i % 2 else 2)))
+  out = c.smooth_radarstate(rs(one))
+  assert out.leadOne is one                          # exact passthrough -- not averaged across tracks
+
+
+def test_smoother_same_track_noise_ignores_drel_jump():
+  # dRel track-jumps are excluded from same_track_noise on purpose: while status stays True, a repeated
+  # farther jump this large is already absorbed by _JumpGuard upstream, so the smoother never even sees the
+  # raw alternation here -- confirms the two mechanisms don't double up on the same signal.
+  c = ctrl()
+  out = None
+  for i in range(30):
+    out = c.smooth_radarstate(rs(lead(dRel=40.0 if i % 2 == 0 else 55.0, vLead=18.0, vRel=-1.0, radarTrackId=4)))
+  assert out.leadOne.dRel < 45.0                     # held near the trusted value by the jump-guard, not 55