|
| 1 | +# Particle Filter (Bounded SIR, Systematic Resampling) — Implementation Pseudocode |
| 2 | + |
| 3 | +> Roadmap ref: #70 (Tier 3) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) |
| 4 | +
|
| 5 | +Sequential importance resampling with a compile-time particle count. The caller supplies the motion |
| 6 | +model (including its noise draws) and the measurement log-likelihood as callables. The filter keeps |
| 7 | +normalised weights, measures degeneracy with the effective sample size, and resamples systematically |
| 8 | +from a caller-provided uniform number. With no allocation and no internal random generator, a seeded |
| 9 | +run is bit-for-bit reproducible. |
| 10 | + |
| 11 | +## Data structures |
| 12 | + |
| 13 | +```cpp |
| 14 | +template<typename T, std::size_t StateSize, std::size_t NumParticles> # static_assert(std::is_floating_point_v<T>); NumParticles ≥ 2 |
| 15 | +class ParticleFilter: |
| 16 | + using StateVector = math::Vector<T, StateSize> |
| 17 | + std::array<StateVector, NumParticles> buffers[2] # current and resampling target |
| 18 | + std::size_t active{ 0 } |
| 19 | + std::array<T, NumParticles> weights # normalised, Σ = 1 |
| 20 | + std::array<T, NumParticles> scratch # log-domain work area |
| 21 | +``` |
| 22 | +
|
| 23 | +## Interface |
| 24 | +
|
| 25 | +```cpp |
| 26 | +template<class Draw> void Initialize(Draw&& draw) # particles[i] = draw(i); weights = 1/N |
| 27 | +template<class Propagate> void Predict(Propagate&& propagate) # propagate(particle&, i) in place; hot path |
| 28 | +template<class LogLikelihood> bool Update(LogLikelihood&& logLikelihood) # w ∝ w·exp(ℓ(x)); false if every ℓ = −∞ |
| 29 | +T EffectiveSampleSize() const # 1 / Σ wᵢ² |
| 30 | +void Resample(T u) # systematic, u ∈ [0, 1); weights → 1/N |
| 31 | +bool ResampleIfNeeded(T essFraction, T u) # resample when ESS < essFraction·N |
| 32 | +StateVector Mean() const # Σ wᵢ xᵢ, component-wise |
| 33 | +const StateVector& MaximumWeight() const # highest-weight particle |
| 34 | +std::span<const StateVector> Particles() const; std::span<StateVector> Particles() |
| 35 | +std::span<const T> Weights() const |
| 36 | +``` |
| 37 | + |
| 38 | +Callables are template parameters, not `infra::Function`, so captured state of any size is allowed and |
| 39 | +the calls inline. |
| 40 | + |
| 41 | +## Algorithm (pseudocode) |
| 42 | + |
| 43 | +```text |
| 44 | +function Update(ℓ): # OPTIMIZE_FOR_SPEED; log domain against under/overflow |
| 45 | + for i: scratch[i] = log(weights[i]) + ℓ(xᵢ) # log 0 = −∞ keeps dead particles dead |
| 46 | + m = max_i scratch[i] |
| 47 | + if m == −∞: return false # every particle impossible: weights unchanged, caller decides |
| 48 | + for i: weights[i] = exp(scratch[i] − m) |
| 49 | + normalise weights to Σ = 1 |
| 50 | + return true |
| 51 | +
|
| 52 | +function EffectiveSampleSize(): return 1 / Σ weights[i]² |
| 53 | +
|
| 54 | +function Resample(u): # systematic: one uniform, N evenly spaced pointers |
| 55 | + c = weights[0]; j = 0 |
| 56 | + for k in [0, N): |
| 57 | + target = (u + k) / N |
| 58 | + while target > c and j < N − 1: j += 1; c += weights[j] |
| 59 | + buffers[1 − active][k] = buffers[active][j] |
| 60 | + active = 1 − active; weights = 1/N |
| 61 | +
|
| 62 | +function ResampleIfNeeded(f, u): |
| 63 | + if EffectiveSampleSize() < f·N: Resample(u); return true |
| 64 | + return false |
| 65 | +``` |
| 66 | + |
| 67 | +## Complexity & memory |
| 68 | + |
| 69 | +- `Predict` / `Update`: `O(N)` callable invocations; `Update` adds `N` `exp` and `N` `log`. `Resample`: `O(N)`. |
| 70 | +- Memory: `2·N·StateSize + 2·N` scalars plus an index. A 3-state, 500-particle filter uses 16 KB in |
| 71 | + `float`. No heap. |
| 72 | + |
| 73 | +## Numerical / embedded notes |
| 74 | + |
| 75 | +- **Log domain.** Likelihoods of long scans underflow `float` (`exp(−200) = 0`). Subtracting the maximum |
| 76 | + log-weight keeps the best particle at weight 1 before normalisation, so the update never divides by 0. |
| 77 | +- **Systematic resampling** has the lowest variance of the common schemes and costs one random number |
| 78 | + per resampling, injected as `u` so the caller owns the generator (seeded, deterministic tests). |
| 79 | +- **Resample only when needed.** Resampling every step adds sample impoverishment. `ResampleIfNeeded(0.5, u)` |
| 80 | + is the usual ESS rule (Doucet & Johansen, 2009). |
| 81 | +- `Mean` is component-wise. For angles (headings in localization) the caller computes the circular mean |
| 82 | + from `Weights()`; `MaximumWeight` gives the MAP-style point estimate. |
| 83 | +- Particles are mutable through `Particles()`, so the caller can inject random particles for |
| 84 | + kidnapped-robot recovery (augmented MCL). |
| 85 | +- Float-only: `static_assert(std::is_floating_point_v<T>)`. |
| 86 | + |
| 87 | +## Deployment |
| 88 | + |
| 89 | +- Header: `numerical/filters/active/ParticleFilter.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on |
| 90 | + `Predict`/`Update`/`Resample`, and `extern template class ParticleFilter<float, 3, 64>;` under |
| 91 | + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD` (the template members are instantiated by the tests). |
| 92 | +- Coverage: `numerical/filters/active/ParticleFilter.cpp` → `template class ParticleFilter<float, 3, 64>;` |
| 93 | +- Test: `numerical/filters/active/test/TestParticleFilter.cpp` |
| 94 | +- Doc: `doc/filters/active/ParticleFilter.md` (per `doc/TEMPLATE.md`) + row in `doc/filters/README.md` |
| 95 | +- CMake: `.hpp` → `target_sources(numerical.filters.active PRIVATE ...)`; `.cpp` → |
| 96 | + `numerical_add_coverage_sources(numerical.filters.active ...)`; test → `numerical.filters.active_test`. |
| 97 | +- Generic pattern: see `roadmap/DEPLOYMENT.md`. |
0 commit comments