kalman: cap process noise dt to prevent t^7 explosion on tracking gaps#352
Open
rocketmark wants to merge 1 commit intocollabora:masterfrom
Open
kalman: cap process noise dt to prevent t^7 explosion on tracking gaps#352rocketmark wants to merge 1 commit intocollabora:masterfrom
rocketmark wants to merge 1 commit intocollabora:masterfrom
Conversation
The process noise Q matrix contains terms up to t^7 (jerk model). When
tracking is interrupted for more than ~200ms (lighthouse occlusion, USB
dropout, cold start) the dt passed to survive_kalman_tracker_process_noise
can be large enough that t^7 overflows into Inf/NaN, causing the filter
to reject all subsequent observations and requiring a process restart to
recover.
Cap dt at 0.05s (5x the normal ~8ms IMU interval) before computing the
power series. State prediction still uses the real dt; only the uncertainty
growth (Q matrix) is bounded. This matches standard practice for
discretised continuous-time process noise models where large gaps should
widen uncertainty to a finite maximum rather than to infinity.
Fixes: tracker freezes after lighthouse occlusion / blackout recovery
(collabora#346)
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.
Summary
survive_kalman_tracker_process_noisecomputes a Q matrix with terms up to t⁷ (jerk model). When tracking is interrupted — lighthouse occlusion, USB dropout, cold start — thedtpassed to this function can be large enough that t⁷ overflows into Inf/NaN. The filter then rejects all subsequent valid observations and the tracker freezes until the process is restarted.Closes #346.
Demonstration
Normal IMU interval is ~8ms (t = 0.008). Consider a 1-second blackout:
t = 1.0s (blackout or cold start gap)
BEFORE fix:
t^7 = 1.0^7 = 1.0
Q_jerk[0] = t^7 / 252 = 0.00397 ← finite but large
... longer gap (t = 2s):
t^7 = 128.0
Q entries → Inf/NaN
Next filter update: quatrotateabout assertion fires or observations rejected
Result: tracker frozen, restart required
AFTER fix:
t capped at 0.05s before power series
t^7 = 0.05^7 = 7.8e-10 ← uncertainty growth bounded
State prediction unaffected (uses real dt)
Result: filter widens uncertainty to a finite maximum, resumes tracking
on next valid observation
The cap does not affect normal tracking. At the 8ms IMU rate, t = 0.008 << 0.05, so the cap is never active during steady-state operation.
Why 50ms
50ms is 5–6× the normal IMU interval and well below any plausible legitimate tracking gap. It bounds t⁷ at
0.05^7 ≈ 7.8e-10, keeping all Q entries finite and reasonable. This matches standard practice for discrete continuous-time process noise models: large gaps should widen uncertainty to a finite maximum, not to infinity.Impact
Any scenario with a tracking gap longer than ~200–300ms is affected: lighthouse occlusion (the specific case in #346), USB cable disturbance, cold start with delayed first IMU packet. The symptom is always the same: filter enters a degenerate state and stops accepting observations.
Change
One file, one line in
src/survive_kalman_tracker.c. State prediction is not touched; only the Q matrix computation is bounded.