radard: fuse vision's lead speed into the matched radar lead - #39021
Closed
ds-sebastian wants to merge 1 commit into
Closed
ds-sebastian wants to merge 1 commit into
ds-sebastian wants to merge 1 commit into
Conversation
Rewrite Track's fixed-gain KF1D as the same [vLead, aLead] filter in covariance form (one-step predictor, R = 1, Q = diag(0.2, 2.0) * dt, which reproduces the KalmanParams gain table exactly), so radar-only tracks are unchanged. For the track matched to leadOne, fuse vision's lead speed as a second measurement with sigma = 2 * vStd, so radar speed excursions that vision does not see are damped instead of passed to the planner. Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
Author
|
Opened by mistake, sorry for the noise. Closing. |
Contributor
Process replay diff reportReplays driving segments through this PR and compares the behavior to master. Show changes |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Purpose
Some radars report short, smooth excursions in a lead's relative speed (1–10 s) that the lead's own range does not follow. radard's per-track filter only sees
vLead, so the excursion goes straight intovLeadK/aLeadKand the planner: unnecessary slowdowns and a rougher ride. This PR lets the vision-matched lead track take vision's lead speed as a second, lower-weight measurement, so excursions that vision does not see are damped instead of passed on.Change (one file, +59 −6)
Track's fixed-gainKF1D[vLead, aLead] is rewritten as the same filter in covariance form: a one-step predictor with R = 1 (m/s)² and Q = diag(0.2, 2.0)·dt, whose steady-state predictor gain equals the existingKalmanParamstable exactly. Radar-only behaviour is unchanged: it matchesKF1Dto 3e-8 on a synthetic sequence, and replay output is identical with fusion switched off. The update is plain floats and costs 0.42 µs per update vs 0.60 µs forKF1D. (Side note: the comment's Q = diag(10, 100), R = 1e3 does not reproduce the table; Q = diag(0.2, 2.0)·dt, R = 1 does.)Track.fuse_vision(): for the track matched to leadOne only (vision prob > 0.5, matching unchanged), fusev_ego + lead.v[0] − model_v_egowith σ =VISION_V_STD_SCALE· max(vStd, 0.1), whereVISION_V_STD_SCALE = 2. Where radar and vision agree, nothing changes.Verification
process_replayof card → radard → plannerd on 0.11.2 (radard.py identical to current master) with a Toyota RAV4 2023, which has a Continental ARS510 radar (decoder: ds-sebastian/ars510-radar).Radar still reacts earlier than vision-only to real slowdowns.
On the development drives, two other radard ideas did not help:
aLeadKon radar/vision disagreement: it cost anticipation.Limits and open questions
radarStatefor every radar car, so process-replay refs will change and it needs checking on other radar platforms.vStdtracks that only loosely. In the sustained false-closing episodes, though, range sided with vision in 22 of 33 cases. A largerVISION_V_STD_SCALE(3–4) may be the better trade for radars with stronger velocity.I understand this may be out of scope for stock openpilot (a behaviour change validated on one platform). It is a small, self-contained change that forks using radar leads may find useful, so I'm opening it as a draft for visibility and feedback.