Ford: anti-weave smoothing of the angle-mode command path (opt-in strength dial)

[ghbarker]

Stacked on #161 — the first 14 commits are that PR (auto-calibration); review the last two commits here. Merging #161 first shrinks this to just them.

The evidence that motivated this

• Community reports of a slow, rhythmic left-right "centering" motion on straight roads in angle mode — the car works the wheel on a steady few-second cycle with nothing to correct.
• Spectral analysis of straight engaged segments on the reference drive (00000006--31********) put numbers on it: a 0.23 Hz dither at ±0.033 m, a slow ±0.46 m lane sway, and the near-zero command crossing a wire code boundary 962 times/min (LatCtlPath_An_Actl's LSB is 0.0005 rad) — a square wave the PSCM integrates into felt motion.
• The mechanism is a self-sustaining loop, not noise to be averaged: the curvature-gain interp's 0.0003-wide transition band sits exactly in straight-road noise range, so unfiltered |kappa| lets the weave modulate its own loop gain 1.0↔~1.2 every cycle. Filter the schedule input and the limit cycle loses its gain source.

Why the fix is shaped this way

• Input-side only, on principle and on data. A curvature-scheduled output low-pass was in the original package and was removed by measurement: a closed-loop rig (real strategy math + wire quantization + PSCM lag + delayed lane policy) showed it degrading station-keeping 2.5x — in-loop lag fights the model's own correction. Open-loop replay structurally cannot show this in either direction, which is why the rig exists.
• Five elements, one pure module (angle_smoothing.AngleSmoother, no I/O — unit-testable anywhere, same pattern as the auto-cal estimator): an asymmetric filter on the gain-schedule input (fast fixed attack, slow strength-scaled release — the primary fix), a low-pass on the model prediction (inside the variable-lookup slack, so free), a slew on the exit-blend ratio (boolean chatter becomes bounded ramps, not 4x steps), hysteresis on the curve-entering decision, and a 1-LSB hold on the outgoing wire value (a held frame is zero-ROC; the release step is far inside the tightest soft ROC — panda cannot be tripped).
• The dial has factor semantics, and 1.0 is provably stock. Menu 1.0 = bit-identical to the feature not existing (fuzz-tested for equality, not closeness); 2.0 = the log-tuned setting; 2.5 = max. The toggle ships ON at strength 1.0 — stock behavior until the driver steps up. Damping is always an explicit choice.
• Curve entry is strength-independent by design: the attack RC is fixed, tested to reach 80% of a gain step in 0.2 s at every strength.
• No stale state, ever: every element resets across disengage/human-turn/stall-blip discontinuities, and re-seeds from the live value when inactive — toggling on or stepping strength mid-drive picks up from the current steering state (both prior transient paths are regression-tested).

The evidence afterwards

• On-road A/B (same car, straight segments; smoothing at menu 1.8 vs none): slow lane sway ±0.46 → ±0.26 m (−43%), dither sway ±0.033 → ±0.022 m (−33%), command-side dither band power −29%. Curve exits measured faster and cleaner with smoothing on (median unwind 0.70 s vs 1.15 s, lower residual curvature). Different days/roads — directional, not controlled — but the command-side numbers replay apples-to-apples and move the same way.
• The cost, quantified by the closed-loop rig: station-keeping std 0.039 m stock → 0.042 m at menu 2.0 (+3 mm) → 0.047 m at 2.5. Three millimeters for half the weave at the tuned setting.
• 13 unit tests on the pure module (stock bit-identity fuzz, entry-speed guarantee, release scaling, transition regressions, hysteresis, bounded slew, wire-hold no-chatter) plus the closed-loop strategy tests exercising the real ext; 81 Ford tests green on the branch.
• Auto-cal evidence integrity was audited against every smoothing element: the calibrator's MIN_KAPPA floor sits at the gain-interp knee (coupling now documented at both sites), the wire hold's worst bias is two orders under the calibrator's stderr threshold, and the same hour-long drive that ran smoothing at 1.8 also locked the auto-calibration — the two features coexist measurably.

What each change is

• opendbc/sunnypilot/car/ford/angle_smoothing.py: the pure AngleSmoother — five elements, constants, strength semantics, transition-safe seeding.
• opendbc/sunnypilot/car/ford/lateral_angle_ext.py: one method call per element at the exact command-path point where it lives; smoothing_enabled/smoothing_strength compat properties for offline tooling.
• common/params_keys.h: register FordAngleSmoothing (BOOL, default on) and FordAngleSmoothStrength (FLOAT, default 1.0 = stock).
• selfdrive/ui/bp/layouts/settings/bluepilot.py + mici/.../lateral_mici.py: toggle + strength stepper on both surfaces, angle-mode gated.
• sunnypilot/sunnylink/settings_ui.json: the two entries, same gating.
• opendbc/sunnypilot/car/ford/tests/test_angle_smoothing.py: the 13 pure unit tests.
• opendbc/sunnypilot/car/ford/tests/test_lateral_angle_ext.py: closed-loop strategy tests (OFF path equals unsmoothed math; filters reset on override paths; ROC properties hold).
• docs/ford-angle-smoothing.md: driver-facing guide — the numbers above, how to find a setting, hard guarantees, troubleshooting.

Test it yourself

• Driver docs: docs/ford-angle-smoothing.md.
• Leave strength at 1.0: nothing changes, anywhere — that's the contract, and test_menu_one_is_bit_identical_passthrough holds it.
• Step to 2.0 on a road where you feel the weave; the slow cycle should drop to roughly half amplitude. Any logged drive can be measured: python bp/angle_weave_analyze.py <dir> <route> (bp-tools) prints the band powers and sway amplitudes above.
• Step strength mid-drive: takes effect within a second, no transient (regression-tested).

Risks / limits

• Angle mode only; the controls are greyed elsewhere. Defaults are stock — zero behavior change until opted in.
• The tuned setting costs ~3 mm of station-keeping tightness; max strength ~8 mm for diminishing extra damping. The guide says try 2.0 first.
• The on-road A/B spans different days and roads; the same-day comparison will tighten when the remaining routes upload.
• A car with badly-off speed factors can weave for that reason alone — calibrate first (Ford: continuous closed-loop auto-calibration of the angle-mode speed factors #161), then judge residual weave. The docs make the same point to drivers.
• Never add output-side lag to this path: measured 2.5x station-keeping harm, noted in the module docstring for the next person tempted.

🤖 Generated with Claude Code