Skip to content

Commit a16132c

Browse files
Mark Stalzerclaude
andcommitted
kalman: cap process noise dt to prevent t^7 explosion on tracking gaps
Cap dt at 0.05s (5x the normal ~8ms IMU interval) before computing the power series. State prediction still uses the real dt; only uncertainty growth (Q matrix) is bounded. Without this cap, a gap of ~1s yields t^7 = 1.0 which inflates Q enough to cause NaN/Inf in the filter on the next update (observed as a quatrotateabout assertion failure on cold start or blackout recovery). 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 (#346) Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
1 parent eb409fc commit a16132c

1 file changed

Lines changed: 3 additions & 0 deletions

File tree

src/survive_kalman_tracker.c

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -944,6 +944,9 @@ void survive_kalman_tracker_process_noise(const struct SurviveKalmanTracker_Para
944944
* positional model with a second order rotational model with tuning parameters
945945
*/
946946

947+
/* dt is capped to prevent NaN/inf values */
948+
if (t > 0.05) t = 0.05;
949+
947950
FLT t2 = t * t;
948951
FLT t3 = t2 * t;
949952
FLT t4 = t3 * t;

0 commit comments

Comments
 (0)