mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-07-07 05:52:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix(long): reject radar fusion

Browse Source
This commit is contained in:
rav4kumar
2026-07-04 12:53:55 -07:00
parent e65a39e749
commit d044951bdc
2 changed files with 178 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sunnypilot/selfdrive/controls/lib/radar_distance/radar_distance.py
+57 -8
View File
@@ -5,11 +5,17 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
RadarDistance conditions the lead the longitudinal MPC follows on a noisy (TSS2-class) radar. It NEVER
reports a farther-or-faster lead than reality, so braking is always >= stock. Three mechanisms:
reports a farther-or-faster lead than reality, so braking is always >= stock. Four mechanisms:
* jump-guard: reject a same-cycle FARTHER dRel jump on a lead that never dropped status (a vision/radar
fusion transient during lead acquisition -- e.g. a cut-in whose vision distance estimate briefly
disagrees with a solid radar track) by holding the last-trusted, closer reading instead of snapping back
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 SYMMETRIC 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);
* 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);
* 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).
Also publishes a read-only lead-instability flag (telemetry). Disabled => byte-stock passthrough always.
@@ -31,7 +37,12 @@ LOW_SPEED_PASSTHROUGH_V = 5.0 # m/s: below this, no flicker-hold (holding a st
# delay the launch); the churn smoother still runs down to CREEP_PASSTHROUGH_V
CREEP_PASSTHROUGH_V = 1.0 # m/s: below this, full byte-stock passthrough (protect the stock stop distance)
SWITCH_DREL = 8.0 # m, dRel jump = a track switch (used by the instability detector)
SWITCH_DREL = 8.0 # m, dRel jump = a track switch (used by the instability detector + jump-guard)
# Jump-guard: a same-cycle dRel jump this far FARTHER, on a lead that never dropped status, is treated as a
# fusion transient (not a real sudden separation) and held at the last-trusted value for a bounded number of
# frames. Self-heals fast so a genuinely-departing lead is never held stale for long.
JUMP_GUARD_MAX_HOLD = 10 # frames (~0.5s)
# Lead-instability detector (telemetry only): flags a bimodal/bouncing radar lead.
STABILITY_WINDOW = 5 # frames (~0.25s @ 20Hz)
@@ -111,8 +122,12 @@ class _RadarStateProxy:
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.
# 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.
def __init__(self):
self._d = None
self._vl = None
@@ -134,12 +149,43 @@ class _LeadSmoother:
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._d = lead.dRel if lead.dRel < self._d else 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 _JumpGuard:
# Rejects a same-cycle FARTHER dRel jump on a lead that never dropped status (a vision/radar fusion
# transient, e.g. a cut-in whose vision distance estimate briefly disagrees with a solid radar track before
# the match locks on) by holding the last-trusted reading, extrapolated by its own vRel, for a bounded
# number of frames. A CLOSER jump of any size always passes through immediately -- this can only ever delay
# relief, never a brake -- and it self-heals after JUMP_GUARD_MAX_HOLD frames if the jump was real.
def __init__(self):
self._last = None
self._hold = 0
def reset(self):
self._last = None
self._hold = 0
def step(self, raw):
if not raw.status:
self.reset()
return raw
if self._last is not None and (raw.dRel - self._last[0]) > SWITCH_DREL and self._hold < JUMP_GUARD_MAX_HOLD:
dRel0, vRel0, vLead0, aLeadK0, aLeadTau0, prob0 = self._last
self._hold += 1
held_dRel = max(MIN_HELD_DREL, dRel0 - max(-vRel0, 0.0) * DT_MDL)
self._last = (held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, prob0)
return _HeldLead(held_dRel, vRel0, vLead0, aLeadK0, aLeadTau0, prob0)
self._hold = 0
self._last = (raw.dRel, raw.vRel, raw.vLead, raw.aLeadK, raw.aLeadTau, raw.modelProb)
return raw
class _LeadHold:
def __init__(self):
self._last = None
@@ -215,6 +261,7 @@ class RadarDistanceController:
self._frame = 0
self._v_ego = 0.0
self._enabled = self._params.get_bool("RadarDistance")
self._jump_guard = _JumpGuard()
self._one = _LeadHold()
self._two = _LeadHold()
self._stability = _LeadStability()
@@ -223,6 +270,7 @@ class RadarDistanceController:
def _read_params(self) -> None:
enabled = self._params.get_bool("RadarDistance")
if not enabled and self._enabled:
self._jump_guard.reset()
self._one.reset()
self._two.reset()
self._smoother.reset()
@@ -258,7 +306,8 @@ class RadarDistanceController:
return radarstate # off: byte-stock passthrough
two = radarstate.leadTwo
if self._v_ego >= LOW_SPEED_PASSTHROUGH_V:
one = self._one.step(radarstate.leadOne) # flicker-hold ...
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)
elif self._v_ego >= CREEP_PASSTHROUGH_V:
sunnypilot/selfdrive/controls/lib/radar_distance/tests/test_radar_distance.py
+121 -1
View File
@@ -16,7 +16,8 @@ import pytest
from openpilot.sunnypilot.selfdrive.controls.lib.radar_distance.radar_distance import \
RadarDistanceController, HOLD_MAX_FRAMES, FCW_PROB_CAP, LOW_SPEED_PASSTHROUGH_V, CREEP_PASSTHROUGH_V, \
DROPOUT_DREL, STOP_GAP_MIN_DREL, STOP_GAP_VEGO, STOP_GAP_VLEAD, STOP_GAP_REGIME_DREL
DROPOUT_DREL, STOP_GAP_MIN_DREL, STOP_GAP_VEGO, STOP_GAP_VLEAD, STOP_GAP_REGIME_DREL, SWITCH_DREL, \
JUMP_GUARD_MAX_HOLD
COMFORT_BRAKE = 2.5
@@ -157,6 +158,78 @@ def test_low_speed_override_lead_passthrough():
assert out.leadOne is one
# --- jump-guard (reject a same-cycle farther fusion transient) --------------------------------------------
def test_jump_guard_holds_farther_transient():
c = ctrl()
c.smooth_radarstate(rs(lead(dRel=27.92, vRel=-5.60, vLead=24.47, radarTrackId=1058)))
out = c.smooth_radarstate(rs(lead(dRel=38.88, vRel=-3.19, vLead=26.91, radarTrackId=-1)))
assert out.leadOne.dRel < 30.0 # farther jump rejected, held near the trusted value
assert out.leadOne.status is True
def test_jump_guard_passes_closer_jump_immediately():
c = ctrl()
c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-2.0, vLead=18.0)))
out = c.smooth_radarstate(rs(lead(dRel=27.0, vRel=-5.0, vLead=15.0))) # big CLOSER jump
assert out.leadOne.dRel == pytest.approx(27.0) # closer always passes through -- never delays a brake
def test_jump_guard_replays_real_route_whiplash():
# route 550a71ee4c7a7fbe/00000498--0704864d6a, t~402.2-402.8: a merging lead's vision distance estimate
# whiplashed 27.92 -> 38.88 -> 37.69 -> 37.20 -> 26.84 for ~0.3s while a solid radar track sat at ~27m the
# whole time. The guard should smooth the farther excursion into a monotone converge toward the real value.
c = ctrl()
raw = [
(74.18, -4.05, 25.77, -1), (53.21, -3.55, 26.33, -1), (47.42, -3.23, 26.67, -1),
(42.64, -3.50, 26.42, -1), (43.22, -3.49, 26.49, -1), (40.03, -3.04, 26.96, -1),
(39.50, -3.29, 26.74, -1), (27.92, -5.60, 24.47, 1058), (38.88, -3.19, 26.91, -1),
(37.69, -3.09, 27.04, -1), (37.20, -2.77, 27.39, -1), (26.84, -5.80, 24.37, 1058),
]
out = None
for dRel, vRel, vLead, tid in raw:
out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=vLead, radarTrackId=tid)))
assert out.leadOne.dRel == pytest.approx(26.84) # real value recovered exactly once raw resumes reporting it
# peak reported dRel during the excursion never revisits the raw 38.88 spike
seen = []
c = ctrl()
for dRel, vRel, vLead, tid in raw:
seen.append(c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=vLead, radarTrackId=tid))).leadOne.dRel)
assert max(seen[8:11]) < 30.0 # the 3 farther-jump frames are all held near ~27m, not ~37-39m
def test_jump_guard_self_heals_after_cap():
c = ctrl()
c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0)))
for _ in range(JUMP_GUARD_MAX_HOLD):
out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
assert out.leadOne.dRel < 40.0 # held while under the cap
out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
assert out.leadOne.dRel == pytest.approx(40.0) # cap exceeded -> accepts the real (departing) value
def test_jump_guard_resets_on_dropout():
c = ctrl()
c.smooth_radarstate(rs(lead(dRel=20.0, vRel=-1.0, vLead=19.0)))
c.smooth_radarstate(rs(lead(status=False, dRel=0.0, modelProb=0.0)))
out = c.smooth_radarstate(rs(lead(dRel=40.0, vRel=-1.0, vLead=19.0)))
assert out.leadOne.dRel == pytest.approx(40.0) # a real dropout in between is not a same-cycle jump
def test_jump_guard_off_when_disabled():
c = ctrl(enabled=False)
c.smooth_radarstate(rs(lead(dRel=27.92, vRel=-5.60, vLead=24.47)))
r = rs(lead(dRel=38.88, vRel=-3.19, vLead=26.91))
assert c.smooth_radarstate(r) is r # disabled -> raw passthrough, no guard
def test_jump_guard_boundary_not_triggered():
c = ctrl()
c.smooth_radarstate(rs(lead(dRel=30.0, vRel=-2.0, vLead=18.0)))
out = c.smooth_radarstate(rs(lead(dRel=30.0 + SWITCH_DREL - 0.1, vRel=-2.0, vLead=18.0)))
assert out.leadOne.dRel == pytest.approx(30.0 + SWITCH_DREL - 0.1) # under threshold -> passes through
# --- flicker-hold -----------------------------------------------------------------------------------------
def test_holds_after_sustained_dropout():
@@ -252,6 +325,53 @@ def test_smoother_inactive_without_churn():
assert out.leadOne is one # steady id -> no churn -> exact passthrough
def test_churn_smoother_closer_accepted_immediately():
# A steadily-closing lead that also briefly churns must never be held farther than the current raw value --
# otherwise the EMA lags a real closing lead for the whole LEAD_SMOOTH_HOLD window, then snaps (a false
# relief followed by a hard catch-up brake -- route 550a71ee4c7a7fbe/00000499, t~1387, real regression).
c = ctrl()
d = 82.0
for i in range(40):
tid = 1 if i % 3 else 2 # enough id-churn to keep the smoother engaged
out = c.smooth_radarstate(rs(lead(dRel=d, vRel=-6.0, vLead=24.0, radarTrackId=tid)))
assert out.leadOne.dRel <= d + 1e-6 # never farther than the latest raw reading
d -= 0.4 # steadily closing
def test_churn_smoother_replays_real_route_late_acquisition():
# route 550a71ee4c7a7fbe/00000499--7f57e1d000, t~1386.9-1388.4: radard toggles between two real candidate
# tracks (id 4611 ~110m, id 4609 ~82m closing) while acquiring, then a couple of vision-fallback frames
# (id -1) report ~104-109m mid-acquisition. The real dRel (track 4609) closes smoothly 82.0 -> 73.2m the
# whole time. Old symmetric EMA held the reported dRel near ~82m (farther than truth) for ~1s after the
# brief churn window, then snapped -- this is the false-relief-then-correction pattern being fixed here.
c = ctrl()
raw = [
(110.84, -1.75, 4611), (82.04, -3.78, 4609), (110.60, -1.85, 4611), (81.68, -3.80, 4609),
(82.28, -4.13, 4609), (110.40, -2.05, 4611), (110.16, -1.93, 4611), (110.08, -2.00, 4611),
(110.00, -2.00, 4611), (80.12, -4.83, 4609), (79.88, -4.95, 4609), (79.64, -5.08, 4609),
(79.32, -5.20, 4609), (79.48, -5.38, 4609), (79.08, -5.55, 4609), (78.64, -5.70, 4609),
(78.20, -5.88, 4609), (77.84, -6.00, 4609), (77.60, -6.18, 4609), (77.48, -6.30, 4609),
(76.96, -6.50, 4609), (76.48, -6.65, 4609), (103.52, -1.75, -1), (76.08, -6.90, 4609),
(75.52, -7.05, 4609), (108.97, -2.02, -1), (104.23, -2.15, -1), (103.64, -2.13, -1),
(74.16, -7.43, 4609), (73.72, -7.60, 4609), (73.24, -7.70, 4609),
]
out = None
for dRel, vRel, tid in raw:
out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=24.0 + vRel, radarTrackId=tid)))
assert out.leadOne.dRel == pytest.approx(73.24, abs=0.5) # tracks the true closing value, no lag
# at no point does the reported dRel sit meaningfully farther than the most recent real (id>0) reading
c = ctrl()
worst_overshoot = 0.0
last_real = None
for dRel, vRel, tid in raw:
out = c.smooth_radarstate(rs(lead(dRel=dRel, vRel=vRel, vLead=24.0 + vRel, radarTrackId=tid)))
if tid > 0:
last_real = dRel
if last_real is not None:
worst_overshoot = max(worst_overshoot, out.leadOne.dRel - last_real)
assert worst_overshoot < 1.0 # old code overshot by ~6-9m for up to ~1s
# --- instability telemetry --------------------------------------------------------------------------------
def test_stability_quiet_on_clean_lead():