From 166b24d83ca826b25d2ec268fad14970a6e716d8 Mon Sep 17 00:00:00 2001 From: Morax Date: Sun, 9 Aug 2026 18:54:45 +0200 Subject: [PATCH] fix(estimation): let underflowed particles recover --- CHANGELOG.md | 5 ++ .../src/estimation/particle_filter.rs | 21 ++++-- .../tests/suite/estimation/particle_filter.rs | 65 ++++++++++++++++++- 3 files changed, 86 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 65223be..001864b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,11 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0 ## [Unreleased] +### Fixed + +- Particle filters floor an underflowed zero weight at the scalar's smallest positive value before + the next update, allowing that particle to recover when later measurements favor it. + ## [0.10.0] - 2026-08-09 A feature release adding signal processing, polynomials and minimum-snap trajectories, LQR and diff --git a/crates/multicalc/src/estimation/particle_filter.rs b/crates/multicalc/src/estimation/particle_filter.rs index 6216acd..14a1cd9 100644 --- a/crates/multicalc/src/estimation/particle_filter.rs +++ b/crates/multicalc/src/estimation/particle_filter.rs @@ -206,6 +206,15 @@ fn draw_from_remainders( weights.len() - 1 } +/// Logs a normalized weight without letting linear underflow make it permanently impossible. +fn recoverable_log_weight(weight: T) -> T { + if weight == T::ZERO { + T::MIN_POSITIVE.ln() + } else { + weight.ln() + } +} + /// Scores how well a particle's predicted measurement matches the real one, as a log-weight. /// /// Implement this for a custom sensor; [`GaussianLikelihood`] is the ready-made default. @@ -532,11 +541,13 @@ where // Score every particle in log space: ask the model what this particle would have measured, // let the likelihood rate how well that matches the real reading, and add it to the - // particle's current log-weight. Working in logs keeps tiny probabilities from underflowing. + // particle's current log-weight. A displayed zero only means the linear representation + // underflowed, so give it the smallest positive prior instead of making it permanently + // impossible. for i in 0..self.particles.len() { let predicted = measurement_model.eval(self.particles[i].as_array()); let score = likelihood.log_weight(&predicted, measurement.as_array()); - self.log_weight_scratch[i] = self.weights[i].ln() + score; + self.log_weight_scratch[i] = recoverable_log_weight(self.weights[i]) + score; } self.normalize_and_resample() @@ -598,9 +609,11 @@ where F: FnMut(&Vector) -> T, { // Combine each particle's score with its current weight in log space, matching update's - // scoring loop; the shared tail then normalizes and resamples. + // scoring loop. Floor a displayed zero so underflow does not make the particle permanently + // impossible; the shared tail then normalizes and resamples. for i in 0..self.particles.len() { - self.log_weight_scratch[i] = self.weights[i].ln() + score(&self.particles[i]); + self.log_weight_scratch[i] = + recoverable_log_weight(self.weights[i]) + score(&self.particles[i]); } self.normalize_and_resample() diff --git a/crates/multicalc/tests/suite/estimation/particle_filter.rs b/crates/multicalc/tests/suite/estimation/particle_filter.rs index 1cc63ae..a03b44f 100644 --- a/crates/multicalc/tests/suite/estimation/particle_filter.rs +++ b/crates/multicalc/tests/suite/estimation/particle_filter.rs @@ -1,6 +1,6 @@ use multicalc::error::EstimationError; use multicalc::estimation::{ - GaussianLikelihood, KalmanFilter, KalmanModel, ParticleFilter, ResamplingScheme, + GaussianLikelihood, KalmanFilter, KalmanModel, Likelihood, ParticleFilter, ResamplingScheme, }; use multicalc::linear_algebra::{Matrix, Matrix2D, Vector}; use multicalc::random::{Pcg32, RandomSource}; @@ -436,3 +436,66 @@ fn a_zero_score_closure_leaves_the_weights_uniform() { "a flat score should leave the full sample size: {effective_sample_size}" ); } + +#[test] +fn particle_recovers_after_its_exported_weight_underflows() { + let mut filter = ParticleFilter::<2, 2>::new( + 2, + Vector::new([0.0, 0.0]), + identity_covariance(), + small_noise(), + 44, + ) + .unwrap() + .with_resample_threshold(0.0); + let recovering_particle = filter.particles()[0].into_array(); + + struct Scores { + recovering_particle: [f64; 2], + recovering_score: f64, + other_score: f64, + } + + impl Likelihood<2, f64> for Scores { + fn log_weight(&self, predicted: &[f64; 2], _measurement: &[f64; 2]) -> f64 { + if *predicted == self.recovering_particle { + self.recovering_score + } else { + self.other_score + } + } + } + + // Push one displayed weight below f64's range without resampling the particle away. + filter + .update( + &MeasureBoth, + &Scores { + recovering_particle, + recovering_score: -1000.0, + other_score: 0.0, + }, + Vector::new([0.0, 0.0]), + ) + .unwrap(); + assert_eq!(filter.weights()[0], 0.0); + + // A later observation reverses the evidence. The particle must still have a finite internal + // prior even though its exported linear weight rounded to zero. + filter + .update( + &MeasureBoth, + &Scores { + recovering_particle, + recovering_score: 0.0, + other_score: -2000.0, + }, + Vector::new([0.0, 0.0]), + ) + .unwrap(); + assert!( + filter.weights()[0] > 1.0 - 1e-12, + "the previously underflowed particle should recover: {:?}", + filter.weights() + ); +}