#![allow(
clippy::many_single_char_names,
clippy::needless_range_loop,
clippy::cast_precision_loss
)]
use crate::sampling::Rng;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum ParticleError {
WeightCollapse,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum Resampling {
Multinomial,
Residual,
Stratified,
Systematic,
}
impl Resampling {
#[must_use]
pub fn indices<const M: usize>(self, weights: &[f64; M], rng: &mut Rng) -> [usize; M] {
match self {
Self::Multinomial => multinomial(weights, rng),
Self::Residual => residual(weights, rng),
Self::Stratified => comb(weights, &stratified_points(rng)),
Self::Systematic => comb(weights, &systematic_points(rng)),
}
}
}
#[derive(Debug, Clone)]
pub struct ParticleFilter<const N: usize, const M: usize> {
particles: [[f64; N]; M],
weights: [f64; M],
}
impl<const N: usize, const M: usize> ParticleFilter<N, M> {
#[must_use]
pub const fn from_particles(particles: [[f64; N]; M]) -> Self {
const { assert!(M >= 2, "a particle filter needs at least two particles") };
Self {
particles,
weights: [1.0 / M as f64; M],
}
}
#[must_use]
pub fn from_mean_and_covariance_sqrt(
mean: &[f64; N],
covariance_sqrt: &[[f64; N]; N],
rng: &mut Rng,
) -> Self {
const { assert!(M >= 2, "a particle filter needs at least two particles") };
let mut particles = [[0.0; N]; M];
for particle in &mut particles {
let draw = rng.correlated_normal(covariance_sqrt);
for i in 0..N {
particle[i] = mean[i] + draw[i];
}
}
Self {
particles,
weights: [1.0 / M as f64; M],
}
}
#[must_use]
pub const fn particles(&self) -> &[[f64; N]; M] {
&self.particles
}
#[must_use]
pub const fn weights(&self) -> &[f64; M] {
&self.weights
}
pub fn predict(
&mut self,
dynamics: impl Fn(&[f64; N]) -> [f64; N],
process_noise_sqrt: &[[f64; N]; N],
rng: &mut Rng,
) {
for particle in &mut self.particles {
let propagated = dynamics(particle);
let noise = rng.correlated_normal(process_noise_sqrt);
for i in 0..N {
particle[i] = propagated[i] + noise[i];
}
}
}
pub fn update(
&mut self,
log_likelihood: impl Fn(&[f64; N]) -> f64,
) -> Result<(), ParticleError> {
let mut logs = [f64::NEG_INFINITY; M];
let mut peak = f64::NEG_INFINITY;
for (slot, (particle, weight)) in logs
.iter_mut()
.zip(self.particles.iter().zip(&self.weights))
{
if *weight <= 0.0 {
continue;
}
let value = log_likelihood(particle) + libm::log(*weight);
if value.is_nan() {
continue;
}
*slot = value;
if value > peak {
peak = value;
}
}
if !peak.is_finite() {
return Err(ParticleError::WeightCollapse);
}
let mut total = 0.0;
let mut posterior = [0.0; M];
for (slot, &value) in posterior.iter_mut().zip(&logs) {
*slot = libm::exp(value - peak);
total += *slot;
}
debug_assert!(total >= 1.0, "the peak particle contributes exactly 1");
for (weight, value) in self.weights.iter_mut().zip(&posterior) {
*weight = value / total;
}
Ok(())
}
#[must_use]
pub fn effective_sample_size(&self) -> f64 {
let mut sum_squares = 0.0;
for &w in &self.weights {
sum_squares += w * w;
}
debug_assert!(sum_squares > 0.0, "the weights are never all zero");
1.0 / sum_squares
}
pub fn resample(&mut self, scheme: Resampling, rng: &mut Rng) {
let indices = scheme.indices(&self.weights, rng);
let source = self.particles;
for (slot, &index) in self.particles.iter_mut().zip(&indices) {
*slot = source[index];
}
self.weights = [1.0 / M as f64; M];
}
pub fn resample_if_degenerate(
&mut self,
fraction: f64,
scheme: Resampling,
rng: &mut Rng,
) -> bool {
if self.effective_sample_size() >= fraction * M as f64 {
return false;
}
self.resample(scheme, rng);
true
}
#[must_use]
pub fn mean(&self) -> [f64; N] {
let mut mean = [0.0; N];
for (particle, &w) in self.particles.iter().zip(&self.weights) {
for i in 0..N {
mean[i] += w * particle[i];
}
}
mean
}
#[must_use]
pub fn covariance(&self) -> [[f64; N]; N] {
let mean = self.mean();
let mut cov = [[0.0; N]; N];
for (particle, &w) in self.particles.iter().zip(&self.weights) {
for i in 0..N {
let di = particle[i] - mean[i];
for j in 0..N {
cov[i][j] += w * di * (particle[j] - mean[j]);
}
}
}
cov
}
}
fn cumulative<const M: usize>(weights: &[f64; M]) -> [f64; M] {
let mut cdf = [0.0; M];
let mut running = 0.0;
for (slot, &w) in cdf.iter_mut().zip(weights) {
running += w;
*slot = running;
}
cdf[M - 1] = 1.0;
cdf
}
fn comb<const M: usize>(weights: &[f64; M], points: &[f64; M]) -> [usize; M] {
let cdf = cumulative(weights);
let mut indices = [0usize; M];
let mut cursor = 0;
for (slot, &point) in indices.iter_mut().zip(points) {
cursor += cdf[cursor..M - 1]
.iter()
.take_while(|&&edge| point >= edge)
.count();
*slot = cursor;
}
indices
}
fn stratified_points<const M: usize>(rng: &mut Rng) -> [f64; M] {
let mut points = [0.0; M];
for (i, slot) in points.iter_mut().enumerate() {
*slot = (i as f64 + rng.unit()) / M as f64;
}
points
}
fn systematic_points<const M: usize>(rng: &mut Rng) -> [f64; M] {
let offset = rng.unit();
let mut points = [0.0; M];
for (i, slot) in points.iter_mut().enumerate() {
*slot = (i as f64 + offset) / M as f64;
}
points
}
fn multinomial<const M: usize>(weights: &[f64; M], rng: &mut Rng) -> [usize; M] {
let cdf = cumulative(weights);
let mut indices = [0usize; M];
for slot in &mut indices {
let point = rng.unit();
*slot = cdf.partition_point(|&edge| point >= edge);
}
indices
}
fn residual<const M: usize>(weights: &[f64; M], rng: &mut Rng) -> [usize; M] {
let mut indices = [0usize; M];
let mut filled = 0;
let mut remainder = [0.0; M];
let mut remainder_total = 0.0;
for (i, &w) in weights.iter().enumerate() {
let copies = libm::floor(w * M as f64);
let mut taken = 0.0;
while taken < copies && filled < M {
indices[filled] = i;
filled += 1;
taken += 1.0;
}
remainder[i] = w * M as f64 - copies;
remainder_total += remainder[i];
}
if filled == M {
return indices;
}
if remainder_total <= 0.0 {
for slot in &mut indices[filled..] {
*slot = M - 1;
}
return indices;
}
for value in &mut remainder {
*value /= remainder_total;
}
let drawn = multinomial(&remainder, rng);
for (slot, &index) in indices[filled..].iter_mut().zip(&drawn) {
*slot = index;
}
indices
}
#[cfg(test)]
mod tests {
#![allow(clippy::float_cmp, clippy::expect_used)]
use super::*;
const WEIGHTS: [f64; 5] = [0.1, 0.2, 0.4, 0.25, 0.05];
#[test]
fn every_scheme_returns_valid_indices() {
let mut rng = Rng::new(3);
for scheme in [
Resampling::Multinomial,
Resampling::Residual,
Resampling::Stratified,
Resampling::Systematic,
] {
for _ in 0..200 {
let indices = scheme.indices(&WEIGHTS, &mut rng);
assert_eq!(indices.len(), 5);
assert!(
indices.iter().all(|&i| i < 5),
"{scheme:?} produced {indices:?}"
);
}
}
}
#[test]
fn a_heavy_particle_survives_the_low_variance_schemes() {
let mut rng = Rng::new(11);
for scheme in [Resampling::Stratified, Resampling::Systematic] {
for _ in 0..2000 {
let indices = scheme.indices(&WEIGHTS, &mut rng);
assert!(
indices.contains(&2),
"{scheme:?} dropped the weight-0.4 particle: {indices:?}"
);
}
}
}
#[test]
fn the_deterministic_half_of_residual_resampling_is_deterministic() {
let mut rng = Rng::new(101);
let indices = residual(&[0.5, 0.25, 0.25, 0.0], &mut rng);
assert_eq!(indices, [0, 0, 1, 2]);
}
#[test]
fn residual_resampling_fills_the_tail_when_no_remainder_is_left() {
let mut rng = Rng::new(102);
let indices = residual(&[0.25, 0.25, 0.25, 0.0], &mut rng);
assert_eq!(indices, [0, 1, 2, 3]);
}
#[test]
fn residual_resampling_cannot_overrun_on_over_unity_weights() {
let mut rng = Rng::new(103);
let indices = residual(&[0.6, 0.6, 0.6, 0.6], &mut rng);
assert_eq!(indices, [0, 0, 1, 1]);
}
#[test]
fn the_effective_sample_size_brackets_its_range() {
let mut filter = ParticleFilter::<1, 8>::from_particles([[0.0]; 8]);
assert!((filter.effective_sample_size() - 8.0).abs() < 1e-12);
filter.weights = [0.0; 8];
filter.weights[3] = 1.0;
assert!((filter.effective_sample_size() - 1.0).abs() < 1e-12);
}
#[test]
fn a_total_weight_collapse_is_an_error() {
let mut filter = ParticleFilter::<1, 4>::from_particles([[0.0], [1.0], [2.0], [3.0]]);
let outcome = filter.update(|_| f64::NEG_INFINITY);
assert_eq!(outcome, Err(ParticleError::WeightCollapse));
assert_eq!(
*filter.weights(),
[0.25; 4],
"a failed update must not mutate"
);
}
#[test]
fn the_update_survives_underflowing_likelihoods() {
let mut filter = ParticleFilter::<1, 3>::from_particles([[0.0], [1.0], [2.0]]);
filter
.update(|x| -800.0 - x[0] * libm::log(2.0))
.expect("the log domain carries this");
let want = [4.0 / 7.0, 2.0 / 7.0, 1.0 / 7.0];
for i in 0..3 {
assert!(
(filter.weights()[i] - want[i]).abs() < 1e-12,
"weight {i} = {}",
filter.weights()[i]
);
}
}
#[test]
fn the_drawn_prior_starts_uniformly_weighted() {
let mut rng = Rng::new(21);
let filter = ParticleFilter::<2, 512>::from_mean_and_covariance_sqrt(
&[1.0, -2.0],
&[[0.5, 0.0], [0.0, 0.25]],
&mut rng,
);
assert_eq!(*filter.weights(), [1.0 / 512.0; 512]);
assert!((filter.effective_sample_size() - 512.0).abs() < 1e-12);
let mean = filter.mean();
assert!(
(mean[0] - 1.0).abs() < 0.05 && (mean[1] + 2.0).abs() < 0.03,
"the draw is centred on {mean:?}, not on (1, −2)"
);
}
#[test]
fn the_prediction_is_the_dynamics_plus_an_independent_draw() {
let mut rng = Rng::new(8);
let mut noiseless = ParticleFilter::<1, 3>::from_particles([[0.0], [1.0], [2.0]]);
noiseless.predict(|x| [2.0 * x[0] + 1.0], &[[0.0]], &mut rng);
assert_eq!(*noiseless.particles(), [[1.0], [3.0], [5.0]]);
let mut noisy = ParticleFilter::<1, 3>::from_particles([[7.0]; 3]);
noisy.predict(|x| [x[0]], &[[1.0]], &mut rng);
let moved = noisy.particles();
assert!(
moved[0][0] != moved[1][0] && moved[1][0] != moved[2][0],
"one draw was shared across the sample: {moved:?}"
);
}
#[test]
fn sequential_updates_multiply_the_likelihoods() {
let particles = [[0.0], [1.0], [2.0], [3.0]];
let mut twice = ParticleFilter::<1, 4>::from_particles(particles);
twice.update(|x| -x[0]).expect("finite likelihoods");
twice.update(|x| -2.0 * x[0]).expect("finite likelihoods");
let mut once = ParticleFilter::<1, 4>::from_particles(particles);
once.update(|x| -3.0 * x[0]).expect("finite likelihoods");
for i in 0..4 {
assert!(
(twice.weights()[i] - once.weights()[i]).abs() < 1e-15,
"particle {i}: {} after two updates, {} after one",
twice.weights()[i],
once.weights()[i]
);
}
}
#[test]
fn the_resampling_trigger_is_the_effective_sample_size() {
let mut filter = ParticleFilter::<1, 8>::from_particles([
[0.0],
[1.0],
[2.0],
[3.0],
[4.0],
[5.0],
[6.0],
[7.0],
]);
let mut rng = Rng::new(13);
assert!(
!filter.resample_if_degenerate(0.5, Resampling::Systematic, &mut rng),
"a uniformly weighted sample is not degenerate"
);
filter.weights = [0.25, 0.25, 0.25, 0.25, 0.0, 0.0, 0.0, 0.0];
assert_eq!(filter.effective_sample_size(), 4.0);
assert!(
!filter.resample_if_degenerate(0.5, Resampling::Systematic, &mut rng),
"N_eff = 4 is not below 0.5·8"
);
assert!(
filter.resample_if_degenerate(0.51, Resampling::Systematic, &mut rng),
"N_eff = 4 is below 0.51·8"
);
assert_eq!(*filter.weights(), [0.125; 8]);
}
#[test]
fn the_weighted_moments_are_the_moments_of_the_weights() {
let mut filter = ParticleFilter::<2, 4>::from_particles([
[0.0, 1.0],
[2.0, 1.0],
[4.0, 3.0],
[6.0, 7.0],
]);
filter.weights = [0.5, 0.25, 0.125, 0.125];
assert_eq!(filter.mean(), [1.75, 2.0]);
let cov = filter.covariance();
assert_eq!(cov[0][0], 4.4375);
assert_eq!(cov[0][1], 3.75);
assert_eq!(cov[1][0], 3.75);
assert_eq!(cov[1][1], 4.0);
}
#[test]
fn resampling_drops_the_zero_weight_particles() {
let mut filter = ParticleFilter::<1, 4>::from_particles([[0.0], [1.0], [2.0], [3.0]]);
filter.weights = [0.0, 0.5, 0.5, 0.0];
let mut rng = Rng::new(17);
filter.resample(Resampling::Systematic, &mut rng);
assert_eq!(*filter.weights(), [0.25; 4]);
for particle in filter.particles() {
assert!(
particle[0] == 1.0 || particle[0] == 2.0,
"a zero-weight particle survived: {particle:?}"
);
}
}
}