diff --git a/README.md b/README.md index 21272d0..98dd3ff 100644 --- a/README.md +++ b/README.md @@ -20,7 +20,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space | | [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) | | [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) | -| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Residual-Form Kalman Update, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | +| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Residual-Form Kalman Update, Particle Filter, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS | | [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | | [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log), SE(2) Transform (Planar Twists, Exp/Log) | diff --git a/ROADMAP.md b/ROADMAP.md index 53cab8d..8777096 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -28,15 +28,14 @@ Difficulty legend: |----|-------------------------------------------------------|-------------------------|------------| | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | | 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ | -| 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ | | 71 | Error-state (multiplicative) EKF on Lie groups | `filters/active` | ★★★★☆ | | 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ | | 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ | -Items 68–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as -NT68–NT73; NT53–NT67 and NT74 are deployed). They serve its mobile, aerial, legged, marine and -localization families and also have non-robotics consumers. Item 70 has a spec under -[`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none yet. All are *(float-first)*. +Items 68, 69 and 71–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there +as NT68, NT69 and NT71–NT73; NT53–NT67, NT70 and NT74 are deployed). They serve its aerial, legged and +marine families and also have non-robotics consumers. None has a spec under +[`roadmap/`](roadmap/README.md) yet. All are *(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family unit-test reference [`TESTING.md`](TESTING.md) depends on but which @@ -68,16 +67,6 @@ All Tier 2 items are deployed. 49(5), 2013; J. Bordignon, *Constrained control allocation* (PhD thesis, Virginia Tech, 1996). - **Reuses:** Items 62 and 65. -### 70. Particle filter -- **What:** Sequential importance resampling with a compile-time particle count, systematic resampling - triggered by the effective sample size, and weighted-mean / MAP estimates. -- **Embedded value:** Multimodal, non-Gaussian estimation — Monte Carlo localization, terrain-aided - navigation, fault detection — on fixed memory. -- **Algorithm / paper:** N. Gordon, D. Salmond, A. Smith, *IEE Proc. F*, 140(2), 1993; A. Doucet, - A. Johansen, "A tutorial on particle filtering and smoothing" (2009). -- **Reuses:** A caller-injected uniform number for resampling; caller-supplied motion and likelihood callables. -- **Spec:** [roadmap/filters/active/ParticleFilter](roadmap/filters/active/ParticleFilter/implementation.md). - --- ## Tier 4 — Advanced ★★★★☆ diff --git a/doc/filters/README.md b/doc/filters/README.md index f262929..4fca2eb 100644 --- a/doc/filters/README.md +++ b/doc/filters/README.md @@ -8,14 +8,15 @@ Digital filtering algorithms for noise reduction, state estimation, and signal c Filters that incorporate feedback and a dynamic internal model of the system. -| Algorithm | Description | -|---------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------| -| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise | -| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) | -| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required | -| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence | -| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness | -| [Residual-Form Kalman Update](active/ResidualKalmanUpdate.md) | Stateless Joseph-form update from a caller-formed residual, with normalised innovation squared and a χ² gate for outliers and association | +| Algorithm | Description | +|---------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise | +| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) | +| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required | +| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence | +| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness | +| [Residual-Form Kalman Update](active/ResidualKalmanUpdate.md) | Stateless Joseph-form update from a caller-formed residual, with normalised innovation squared and a χ² gate for outliers and association | +| [Particle Filter](active/ParticleFilter.md) | Bounded sequential importance resampling with caller-supplied motion and log-likelihood callables, log-domain weights and systematic resampling on low effective sample size | ### Passive Filters diff --git a/doc/filters/active/ParticleFilter.md b/doc/filters/active/ParticleFilter.md new file mode 100644 index 0000000..230666e --- /dev/null +++ b/doc/filters/active/ParticleFilter.md @@ -0,0 +1,98 @@ +# Particle Filter (Bounded SIR, Systematic Resampling) + +## Overview & Motivation + +Kalman filters represent the state belief with one Gaussian. That works when the dynamics and measurements are close to linear and the belief has one mode. It fails when a robot could be in one of several similar corridors, when a target may have turned left or right, or when the measurement model is far from Gaussian. + +A **particle filter** represents the belief with a set of samples (particles), each carrying a weight. Prediction moves every particle through the motion model with its own noise draw. A measurement reweights the particles by its likelihood. Resampling then replaces improbable particles with copies of probable ones. +The representation handles any dynamics, any likelihood and any number of modes, at a cost proportional to the number of particles. + +This implementation is sized at compile time and allocates nothing. The caller supplies the motion model and the measurement likelihood as callables, and the random numbers as arguments, so a seeded run is bit-for-bit reproducible. + +## Mathematical Theory + +### Sequential Importance Resampling + +The belief at time $k$ is $\{(x_k^{(i)}, w_k^{(i)})\}_{i=1}^N$ with $\sum_i w_k^{(i)} = 1$. One cycle is: + +$$x_k^{(i)} \sim p(x_k \mid x_{k-1}^{(i)}, u_k), \qquad w_k^{(i)} \propto w_{k-1}^{(i)}\, p(z_k \mid x_k^{(i)})$$ + +Prediction samples the motion model, and the update multiplies each weight by the measurement likelihood. + +### Log-Domain Update + +Likelihoods of long measurement vectors underflow single precision quickly ($e^{-200} = 0$). The update therefore works with log-likelihoods $\ell_i$ and subtracts their maximum before exponentiating: + +$$s_i = \log w^{(i)} + \ell_i, \qquad w^{(i)} \leftarrow \frac{e^{s_i - \max_j s_j}}{\sum_m e^{s_m - \max_j s_j}}$$ + +The best particle always gets an unnormalised weight of $1$, so normalisation never divides by zero. If every particle has $\ell = -\infty$, the measurement is impossible under the current belief. The update then reports failure and leaves the weights unchanged, so the caller can decide what to do. + +### Effective Sample Size + +When a few particles carry nearly all the weight, the rest are wasted computation. The effective sample size measures this: + +$$N_{\text{eff}} = \frac{1}{\sum_i \left(w^{(i)}\right)^2}$$ + +It equals $N$ for uniform weights and $1$ when one particle holds everything. + +### Systematic Resampling + +Resampling draws $N$ new particles in proportion to the weights. Systematic resampling uses one uniform number $u \in [0, 1)$ and $N$ evenly spaced pointers into the cumulative weights: + +$$p_k = \frac{u + k}{N}, \qquad k = 0, \ldots, N - 1$$ + +Each pointer selects the first particle whose cumulative weight reaches it. This has the lowest variance of the common schemes and costs one random number per resampling. Resampling every step impoverishes the sample set, so it is usually triggered only when $N_{\text{eff}}$ falls below a fraction of $N$, commonly one half. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|----------------|--------|----------------|----------------------------------------------------------------| +| Predict | $O(N)$ | $O(1)$ | One motion-model call per particle | +| Update | $O(N)$ | $O(N)$ | One likelihood, one logarithm and one exponential per particle | +| Resample | $O(N)$ | $O(N)$ | One pass over the cumulative weights | +| Particle store | — | $O(N \cdot d)$ | Two particle buffers plus weights and scratch | + +For $N$ particles of dimension $d$. A 3-state filter with 500 particles uses 16 KB in single precision. + +## Step-by-Step Walkthrough + +1. **Uniform prior**: 41 particles evenly spaced on $[-2, 2]$ start with weights $1/41$ and an effective sample size of 41. +2. **Measurement**: a Gaussian measurement $z = 0.5$ with $\sigma = 0.5$ moves the weighted mean to $0.498$ (slightly pulled towards zero by the truncated prior). The effective sample size drops to $17.7$, and the heaviest particle sits at $0.5$. +3. **Underflow**: subtracting $1000$ from every log-likelihood leaves the normalised weights unchanged. Without the log-domain update, every weight would underflow to zero. +4. **Second measurement**: $z = 0.7$ moves the mean to $0.600$, close to the average of the two measurements, and the effective sample size drops to $12.5$. +5. **Resampling**: four particles $0, 1, 2, 3$ with weights $0.1, 0.2, 0.3, 0.4$ have an effective sample size of $3.33$. With $u = 0.5$, the pointers $0.125, 0.375, 0.625, 0.875$ select particles $1, 2, 3, 3$, and the weights become uniform. +6. **Threshold**: with a threshold of half the particles (2), no resampling happens. With $0.9$ of them (3.6), it does. +7. **Impossible measurement**: a likelihood of $-\infty$ everywhere is rejected, and the weights stay as they were. + +## Pitfalls & Edge Cases + +- **Sample impoverishment**: resampling too often collapses the set onto a few states. Resample only when the effective sample size is low, and keep enough motion noise. +- **Overconfident likelihoods**: a likelihood that is too sharp kills most particles in one step. Add a floor, such as a uniform outlier term, or temper the likelihood. +- **Angles**: the component-wise mean fails for headings across $\pm\pi$. Compute a circular mean from the weights, or use the highest-weight particle. +- **Too few particles**: the required count grows quickly with the state dimension. Particle filters suit low-dimensional states. +- **Kidnapping**: once all particles are wrong, the filter cannot recover by itself. Inject a few random particles when the average likelihood drops. + +## Variants & Generalizations + +- **Auxiliary and regularised particle filters**: better proposals and jittered resampling against impoverishment. +- **Rao–Blackwellised filters**: particles for the nonlinear part and Kalman filters for the rest, as in FastSLAM. +- **KLD sampling**: the particle count adapts to the spread of the belief. +- **Unscented and extended Kalman filters**: Gaussian alternatives for unimodal, mildly nonlinear problems. + +## Applications + +- Monte Carlo localization of mobile robots in occupancy grids. +- Visual tracking of objects through occlusions. +- Terrain-aided navigation and fault diagnosis with discrete modes. + +## Connections to Other Algorithms + +- [Kalman Filter](KalmanFilter.md): the optimal linear-Gaussian special case. +- [Unscented Kalman Filter](UnscentedKalmanFilter.md): deterministic sigma points instead of random particles, for unimodal beliefs. +- [Residual-Form Kalman Update](ResidualKalmanUpdate.md): the Gaussian update for problems where a particle filter is too costly. + +## References & Further Reading + +- Gordon, N.J., Salmond, D.J. and Smith, A.F.M. (1993). "Novel approach to nonlinear/non-Gaussian Bayesian state estimation." *IEE Proceedings F*, 140(2), 107–113. +- Doucet, A. and Johansen, A.M. (2009). "A tutorial on particle filtering and smoothing: fifteen years later." *Handbook of Nonlinear Filtering*, Oxford University Press. +- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapter 4. diff --git a/numerical/filters/active/CMakeLists.txt b/numerical/filters/active/CMakeLists.txt index dd63a43..83794c3 100644 --- a/numerical/filters/active/CMakeLists.txt +++ b/numerical/filters/active/CMakeLists.txt @@ -19,6 +19,7 @@ target_sources(numerical.filters.active PRIVATE KalmanFilter.hpp KalmanFilterBase.hpp KalmanSmoother.hpp + ParticleFilter.hpp ResidualKalmanUpdate.hpp SquareRootKalmanFilter.hpp UnscentedKalmanFilter.hpp @@ -32,6 +33,7 @@ numerical_add_coverage_sources(numerical.filters.active KalmanFilter.cpp KalmanFilterBase.cpp KalmanSmoother.cpp + ParticleFilter.cpp ResidualKalmanUpdate.cpp SquareRootKalmanFilter.cpp UnscentedKalmanFilter.cpp diff --git a/numerical/filters/active/ParticleFilter.cpp b/numerical/filters/active/ParticleFilter.cpp new file mode 100644 index 0000000..e30a7a5 --- /dev/null +++ b/numerical/filters/active/ParticleFilter.cpp @@ -0,0 +1,13 @@ +#include "numerical/filters/active/ParticleFilter.hpp" + +namespace filters +{ + template class ParticleFilter; + template class ParticleFilter; + template class ParticleFilter; + template void ParticleFilter::Initialize (&)(std::size_t)>(math::Vector (&)(std::size_t)); + template void ParticleFilter::Initialize (&)(std::size_t)>(math::Vector (&)(std::size_t)); + template bool ParticleFilter::Update&)>(float (&)(const math::Vector&)); + template bool ParticleFilter::Update&)>(float (&)(const math::Vector&)); + template void ParticleFilter::Predict&, std::size_t)>(void (&)(math::Vector&, std::size_t)); +} diff --git a/numerical/filters/active/ParticleFilter.hpp b/numerical/filters/active/ParticleFilter.hpp new file mode 100644 index 0000000..d281097 --- /dev/null +++ b/numerical/filters/active/ParticleFilter.hpp @@ -0,0 +1,192 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include +#include +#include + +namespace filters +{ + template + class ParticleFilter + { + static_assert(std::is_floating_point_v, "ParticleFilter supports floating-point types"); + static_assert(StateSize > 0, "ParticleFilter needs a state"); + static_assert(NumParticles >= 2, "ParticleFilter needs at least two particles"); + + public: + using StateVector = math::Vector; + + ParticleFilter(); + + template + void Initialize(Draw&& draw); + template + OPTIMIZE_FOR_SPEED void Predict(Propagate&& propagate); + template + OPTIMIZE_FOR_SPEED bool Update(LogLikelihood&& logLikelihood); + + T EffectiveSampleSize() const; + OPTIMIZE_FOR_SPEED void Resample(T u); + bool ResampleIfNeeded(T essFraction, T u); + + StateVector Mean() const; + const StateVector& MaximumWeight() const; + std::span Particles() const; + std::span Particles(); + std::span Weights() const; + + private: + static constexpr T uniformWeight{ T{ 1 } / static_cast(NumParticles) }; + + std::array, 2> buffers{}; + std::size_t active{ 0 }; + std::array weights{}; + std::array scratch{}; + }; + + template + ParticleFilter::ParticleFilter() + { + weights.fill(uniformWeight); + } + + template + template + void ParticleFilter::Initialize(Draw&& draw) + { + auto& particles{ buffers[active] }; + for (std::size_t i = 0; i < NumParticles; ++i) + particles[i] = draw(i); + weights.fill(uniformWeight); + } + + template + template + OPTIMIZE_FOR_SPEED void ParticleFilter::Predict(Propagate&& propagate) + { + auto& particles{ buffers[active] }; + for (std::size_t i = 0; i < NumParticles; ++i) + propagate(particles[i], i); + } + + template + template + OPTIMIZE_FOR_SPEED bool ParticleFilter::Update(LogLikelihood&& logLikelihood) + { + const auto& particles{ buffers[active] }; + T maximum{ -std::numeric_limits::infinity() }; + for (std::size_t i = 0; i < NumParticles; ++i) + { + scratch[i] = math::Log(weights[i]) + logLikelihood(particles[i]); + maximum = scratch[i] > maximum ? scratch[i] : maximum; + } + + if (maximum == -std::numeric_limits::infinity()) + return false; + + T sum{}; + for (std::size_t i = 0; i < NumParticles; ++i) + { + weights[i] = math::Exp(scratch[i] - maximum); + sum += weights[i]; + } + + for (auto& weight : weights) + weight /= sum; + return true; + } + + template + T ParticleFilter::EffectiveSampleSize() const + { + T sumOfSquares{}; + for (const auto weight : weights) + sumOfSquares += weight * weight; + return T{ 1 } / sumOfSquares; + } + + template + OPTIMIZE_FOR_SPEED void ParticleFilter::Resample(T u) + { + const auto& source{ buffers[active] }; + auto& target{ buffers[1 - active] }; + T cumulative{ weights[0] }; + std::size_t index{ 0 }; + for (std::size_t k = 0; k < NumParticles; ++k) + { + const T pointer{ (u + static_cast(k)) / static_cast(NumParticles) }; + while (pointer > cumulative && index < NumParticles - 1) + { + ++index; + cumulative += weights[index]; + } + target[k] = source[index]; + } + + active = 1 - active; + weights.fill(uniformWeight); + } + + template + bool ParticleFilter::ResampleIfNeeded(T essFraction, T u) + { + if (EffectiveSampleSize() >= essFraction * static_cast(NumParticles)) + return false; + + Resample(u); + return true; + } + + template + auto ParticleFilter::Mean() const -> StateVector + { + const auto& particles{ buffers[active] }; + StateVector mean{}; + for (std::size_t i = 0; i < NumParticles; ++i) + mean = mean + particles[i] * weights[i]; + return mean; + } + + template + auto ParticleFilter::MaximumWeight() const -> const StateVector& + { + std::size_t best{ 0 }; + for (std::size_t i = 1; i < NumParticles; ++i) + best = weights[i] > weights[best] ? i : best; + return buffers[active][best]; + } + + template + auto ParticleFilter::Particles() const -> std::span + { + return buffers[active]; + } + + template + auto ParticleFilter::Particles() -> std::span + { + return buffers[active]; + } + + template + std::span ParticleFilter::Weights() const + { + return weights; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ParticleFilter; + extern template class ParticleFilter; + extern template class ParticleFilter; + extern template void ParticleFilter::Initialize (&)(std::size_t)>(math::Vector (&)(std::size_t)); + extern template void ParticleFilter::Initialize (&)(std::size_t)>(math::Vector (&)(std::size_t)); + extern template bool ParticleFilter::Update&)>(float (&)(const math::Vector&)); + extern template bool ParticleFilter::Update&)>(float (&)(const math::Vector&)); + extern template void ParticleFilter::Predict&, std::size_t)>(void (&)(math::Vector&, std::size_t)); +#endif +} diff --git a/numerical/filters/active/test/CMakeLists.txt b/numerical/filters/active/test/CMakeLists.txt index b16050a..75c2946 100644 --- a/numerical/filters/active/test/CMakeLists.txt +++ b/numerical/filters/active/test/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(numerical.filters.active_test PRIVATE TestExtendedKalmanFilter.cpp TestKalmanFilter.cpp TestKalmanSmoother.cpp + TestParticleFilter.cpp TestResidualKalmanUpdate.cpp TestSquareRootKalmanFilter.cpp TestUnscentedKalmanFilter.cpp diff --git a/numerical/filters/active/test/TestParticleFilter.cpp b/numerical/filters/active/test/TestParticleFilter.cpp new file mode 100644 index 0000000..3570e24 --- /dev/null +++ b/numerical/filters/active/test/TestParticleFilter.cpp @@ -0,0 +1,168 @@ +#include "numerical/filters/active/ParticleFilter.hpp" +#include +#include +#include +#include + +namespace +{ + using Vector = math::Vector; + using Grid = filters::ParticleFilter; + using Four = filters::ParticleFilter; + + Vector GridPoint(std::size_t i) + { + return Vector{ -2.0f + 4.0f * static_cast(i) / 40.0f }; + } + + Vector Index(std::size_t i) + { + return Vector{ static_cast(i) }; + } + + float Gaussian(const Vector& x, float z, float sigma) + { + const float r{ (z - x[0]) / sigma }; + return -0.5f * r * r; + } + + float MeasurementAt05(const Vector& x) + { + return Gaussian(x, 0.5f, 0.5f); + } + + float MeasurementAt05Shifted(const Vector& x) + { + return Gaussian(x, 0.5f, 0.5f) - 1000.0f; + } + + float MeasurementAt07(const Vector& x) + { + return Gaussian(x, 0.7f, 0.5f); + } + + float Ramp(const Vector& x) + { + return std::log(0.1f * (x[0] + 1.0f)); + } + + float Impossible(const Vector&) + { + return -std::numeric_limits::infinity(); + } + + void Shift(Vector& x, std::size_t) + { + x[0] += 1.0f; + } + + class TestParticleFilter + : public ::testing::Test + { + protected: + TestParticleFilter() + { + grid.Initialize(GridPoint); + four.Initialize(Index); + four.Update(Ramp); + } + + static void ExpectWeights(const Four& filter, float w0, float w1, float w2, float w3) + { + const auto weights{ filter.Weights() }; + EXPECT_NEAR(weights[0], w0, 1e-6f); + EXPECT_NEAR(weights[1], w1, 1e-6f); + EXPECT_NEAR(weights[2], w2, 1e-6f); + EXPECT_NEAR(weights[3], w3, 1e-6f); + } + + static void ExpectParticles(const Four& filter, float p0, float p1, float p2, float p3) + { + const auto particles{ filter.Particles() }; + EXPECT_EQ(particles[0][0], p0); + EXPECT_EQ(particles[1][0], p1); + EXPECT_EQ(particles[2][0], p2); + EXPECT_EQ(particles[3][0], p3); + } + + Grid grid; + Four four; + }; +} + +TEST_F(TestParticleFilter, initialize_sets_uniform_weights) +{ + for (const float weight : grid.Weights()) + EXPECT_NEAR(weight, 1.0f / 41.0f, 1e-7f); + EXPECT_NEAR(grid.EffectiveSampleSize(), 41.0f, 1e-4f); +} + +TEST_F(TestParticleFilter, update_normalises_in_the_log_domain) +{ + Grid shifted; + shifted.Initialize(GridPoint); + + EXPECT_TRUE(grid.Update(MeasurementAt05)); + EXPECT_TRUE(shifted.Update(MeasurementAt05Shifted)); + + EXPECT_NEAR(grid.Mean()[0], 0.498389f, 1e-5f); + EXPECT_NEAR(grid.EffectiveSampleSize(), 17.690938f, 1e-4f); + EXPECT_NEAR(grid.MaximumWeight()[0], 0.5f, 1e-6f); + float sum{ 0.0f }; + for (std::size_t i = 0; i < 41; ++i) + { + sum += grid.Weights()[i]; + EXPECT_NEAR(shifted.Weights()[i], grid.Weights()[i], 1e-5f); + } + EXPECT_NEAR(sum, 1.0f, 1e-5f); +} + +TEST_F(TestParticleFilter, sequential_updates_multiply_likelihoods) +{ + grid.Update(MeasurementAt05); + grid.Update(MeasurementAt07); + + EXPECT_NEAR(grid.Mean()[0], 0.599970f, 1e-5f); + EXPECT_NEAR(grid.EffectiveSampleSize(), 12.532656f, 1e-4f); +} + +TEST_F(TestParticleFilter, systematic_resampling_is_deterministic) +{ + Four fresh; + fresh.Initialize(Index); + fresh.Update(Ramp); + + EXPECT_NEAR(four.EffectiveSampleSize(), 3.333333f, 1e-5f); + four.Resample(0.5f); + fresh.Resample(0.0f); + + ExpectParticles(four, 1.0f, 2.0f, 3.0f, 3.0f); + ExpectWeights(four, 0.25f, 0.25f, 0.25f, 0.25f); + ExpectParticles(fresh, 0.0f, 1.0f, 2.0f, 3.0f); +} + +TEST_F(TestParticleFilter, resample_if_needed_respects_the_threshold) +{ + EXPECT_FALSE(four.ResampleIfNeeded(0.5f, 0.5f)); + ExpectParticles(four, 0.0f, 1.0f, 2.0f, 3.0f); + ExpectWeights(four, 0.1f, 0.2f, 0.3f, 0.4f); + + EXPECT_TRUE(four.ResampleIfNeeded(0.9f, 0.5f)); + ExpectParticles(four, 1.0f, 2.0f, 3.0f, 3.0f); +} + +TEST_F(TestParticleFilter, predict_moves_every_particle_and_keeps_weights) +{ + four.Predict(Shift); + + EXPECT_NEAR(four.Mean()[0], 3.0f, 1e-5f); + EXPECT_EQ(four.Particles()[3][0], 4.0f); + ExpectWeights(four, 0.1f, 0.2f, 0.3f, 0.4f); +} + +TEST_F(TestParticleFilter, impossible_measurement_leaves_weights_unchanged) +{ + EXPECT_FALSE(four.Update(Impossible)); + + ExpectWeights(four, 0.1f, 0.2f, 0.3f, 0.4f); +} diff --git a/roadmap/README.md b/roadmap/README.md index af0b1e1..d37a960 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -80,7 +80,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `ExponentialMovingAverage` (1) · `MovingAverage` (2) · `MedianFilter` (6) · `CicFilter` (14) · `BiquadCascade` (15) · `NotchCombFilter` (16) · `SavitzkyGolayFilter` (22) · `IirFilterDesign` (45) ### `filters/active` -`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) · `ParticleFilter` (70) +`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) ### `controllers` `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) diff --git a/roadmap/filters/active/ParticleFilter/explanation.md b/roadmap/filters/active/ParticleFilter/explanation.md deleted file mode 100644 index dd48dcf..0000000 --- a/roadmap/filters/active/ParticleFilter/explanation.md +++ /dev/null @@ -1,35 +0,0 @@ -# Particle Filter — Overview - -## What it is -A state estimator that represents its belief as a fixed set of samples ("particles") with weights, -instead of a single mean and covariance. Each particle is moved by the motion model, re-weighted by how -well it explains the measurement, and the set is periodically resampled to concentrate on the likely -states. - -## Why it matters (embedded) -Kalman filters assume a single Gaussian belief. Robot localization in a symmetric building, terrain-aided -navigation and fault detection have several competing hypotheses or strongly non-linear sensors, and a -particle filter handles those directly. With a compile-time particle count it runs in fixed memory and -fixed time per step, and with the random numbers supplied by the caller it is fully reproducible. - -## How it works (intuition) -Every particle is a guess of the state. The prediction step moves each guess as the system would move, -with the caller adding random disturbances. The update step scores each guess against the measurement, -and the score becomes its weight. After a few updates most weight sits on a few particles, which wastes -the rest; the effective sample size detects this. Resampling then draws a new, equally weighted set in -which good particles are duplicated and poor ones disappear. The systematic scheme uses one random -number and a comb of evenly spaced pointers, which keeps the extra randomness small. - -## Key parameters -- **number of particles** — accuracy versus time and memory (compile-time). -- **resampling threshold** — fraction of the particle count below which the effective sample size triggers resampling. -- **motion model and likelihood** — supplied by the caller. - -## Reference -N. J. Gordon, D. J. Salmond, A. F. M. Smith, "Novel approach to nonlinear/non-Gaussian Bayesian state -estimation," *IEE Proc. F*, 140(2), 1993; A. Doucet, A. M. Johansen, "A tutorial on particle filtering and -smoothing: fifteen years later," in *Handbook of Nonlinear Filtering* (2009). - -## See also -`ExtendedKalmanFilter`, `UnscentedKalmanFilter` (Gaussian alternatives); robotics-toolbox-cpp's Monte Carlo -localization (E4). diff --git a/roadmap/filters/active/ParticleFilter/implementation.md b/roadmap/filters/active/ParticleFilter/implementation.md deleted file mode 100644 index 4939bfc..0000000 --- a/roadmap/filters/active/ParticleFilter/implementation.md +++ /dev/null @@ -1,97 +0,0 @@ -# Particle Filter (Bounded SIR, Systematic Resampling) — Implementation Pseudocode - -> Roadmap ref: #70 (Tier 3) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) - -Sequential importance resampling with a compile-time particle count. The caller supplies the motion -model (including its noise draws) and the measurement log-likelihood as callables. The filter keeps -normalised weights, measures degeneracy with the effective sample size, and resamples systematically -from a caller-provided uniform number. With no allocation and no internal random generator, a seeded -run is bit-for-bit reproducible. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); NumParticles ≥ 2 -class ParticleFilter: - using StateVector = math::Vector - std::array buffers[2] # current and resampling target - std::size_t active{ 0 } - std::array weights # normalised, Σ = 1 - std::array scratch # log-domain work area -``` - -## Interface - -```cpp -template void Initialize(Draw&& draw) # particles[i] = draw(i); weights = 1/N -template void Predict(Propagate&& propagate) # propagate(particle&, i) in place; hot path -template bool Update(LogLikelihood&& logLikelihood) # w ∝ w·exp(ℓ(x)); false if every ℓ = −∞ -T EffectiveSampleSize() const # 1 / Σ wᵢ² -void Resample(T u) # systematic, u ∈ [0, 1); weights → 1/N -bool ResampleIfNeeded(T essFraction, T u) # resample when ESS < essFraction·N -StateVector Mean() const # Σ wᵢ xᵢ, component-wise -const StateVector& MaximumWeight() const # highest-weight particle -std::span Particles() const; std::span Particles() -std::span Weights() const -``` - -Callables are template parameters, not `infra::Function`, so captured state of any size is allowed and -the calls inline. - -## Algorithm (pseudocode) - -```text -function Update(ℓ): # OPTIMIZE_FOR_SPEED; log domain against under/overflow - for i: scratch[i] = log(weights[i]) + ℓ(xᵢ) # log 0 = −∞ keeps dead particles dead - m = max_i scratch[i] - if m == −∞: return false # every particle impossible: weights unchanged, caller decides - for i: weights[i] = exp(scratch[i] − m) - normalise weights to Σ = 1 - return true - -function EffectiveSampleSize(): return 1 / Σ weights[i]² - -function Resample(u): # systematic: one uniform, N evenly spaced pointers - c = weights[0]; j = 0 - for k in [0, N): - target = (u + k) / N - while target > c and j < N − 1: j += 1; c += weights[j] - buffers[1 − active][k] = buffers[active][j] - active = 1 − active; weights = 1/N - -function ResampleIfNeeded(f, u): - if EffectiveSampleSize() < f·N: Resample(u); return true - return false -``` - -## Complexity & memory - -- `Predict` / `Update`: `O(N)` callable invocations; `Update` adds `N` `exp` and `N` `log`. `Resample`: `O(N)`. -- Memory: `2·N·StateSize + 2·N` scalars plus an index. A 3-state, 500-particle filter uses 16 KB in - `float`. No heap. - -## Numerical / embedded notes - -- **Log domain.** Likelihoods of long scans underflow `float` (`exp(−200) = 0`). Subtracting the maximum - log-weight keeps the best particle at weight 1 before normalisation, so the update never divides by 0. -- **Systematic resampling** has the lowest variance of the common schemes and costs one random number - per resampling, injected as `u` so the caller owns the generator (seeded, deterministic tests). -- **Resample only when needed.** Resampling every step adds sample impoverishment. `ResampleIfNeeded(0.5, u)` - is the usual ESS rule (Doucet & Johansen, 2009). -- `Mean` is component-wise. For angles (headings in localization) the caller computes the circular mean - from `Weights()`; `MaximumWeight` gives the MAP-style point estimate. -- Particles are mutable through `Particles()`, so the caller can inject random particles for - kidnapped-robot recovery (augmented MCL). -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/filters/active/ParticleFilter.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Predict`/`Update`/`Resample`, and `extern template class ParticleFilter;` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD` (the template members are instantiated by the tests). -- Coverage: `numerical/filters/active/ParticleFilter.cpp` → `template class ParticleFilter;` -- Test: `numerical/filters/active/test/TestParticleFilter.cpp` -- Doc: `doc/filters/active/ParticleFilter.md` (per `doc/TEMPLATE.md`) + row in `doc/filters/README.md` -- CMake: `.hpp` → `target_sources(numerical.filters.active PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.filters.active ...)`; test → `numerical.filters.active_test`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/filters/active/ParticleFilter/tests.md b/roadmap/filters/active/ParticleFilter/tests.md deleted file mode 100644 index 3506934..0000000 --- a/roadmap/filters/active/ParticleFilter/tests.md +++ /dev/null @@ -1,53 +0,0 @@ -# Particle Filter — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestParticleFilter : public ::testing::Test: - ParticleFilter grid # Initialize(i ⇒ −2 + 4i/40): an evenly spaced prior on [−2, 2] - ParticleFilter four # Initialize(i ⇒ i), then Update(x ⇒ log(0.1·(x + 1))): weights (0.1, 0.2, 0.3, 0.4) - Gaussian(z, σ) = [z, σ](x) ⇒ −½((z − x)/σ)² -# each case below is a TEST_F(TestParticleFilter, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -initialize_sets_uniform_weights: - Assert: every grid weight ≈ 1/41; EffectiveSampleSize() ≈ 41 -update_normalises_in_the_log_domain: - Act: grid.Update(Gaussian(0.5, 0.5)) - Assert: Mean() ≈ 0.498389; EffectiveSampleSize() ≈ 17.690938; MaximumWeight() ≈ 0.5; Σ weights ≈ 1 - Act: the same on a fresh grid with ℓ − 1000 - Assert: identical weights (no underflow) -sequential_updates_multiply_likelihoods: - Act: grid.Update(Gaussian(0.5, 0.5)); grid.Update(Gaussian(0.7, 0.5)) - Assert: Mean() ≈ 0.599970; EffectiveSampleSize() ≈ 12.532656 -systematic_resampling_is_deterministic: - Assert: four.EffectiveSampleSize() ≈ 3.333333 - after four.Resample(0.5): particles (1, 2, 3, 3), weights all 0.25 - on a fresh `four`, Resample(0): particles (0, 1, 2, 3) -resample_if_needed_respects_the_threshold: - Assert: four.ResampleIfNeeded(0.5, 0.5) == false (ESS 3.33 ≥ 2), particles unchanged - four.ResampleIfNeeded(0.9, 0.5) == true (ESS 3.33 < 3.6) -predict_moves_every_particle_and_keeps_weights: - Act: four.Predict([](x, i) { x[0] += 1; }) - Assert: Mean() ≈ 3.0 (2.0 + 1); weights still (0.1, 0.2, 0.3, 0.4) -impossible_measurement_leaves_weights_unchanged: - Assert: four.Update(x ⇒ −∞) == false; weights still (0.1, 0.2, 0.3, 0.4) -``` - -## Reference vectors - -- Grid prior on `[−2, 2]` (41 points), `z = 0.5, σ = 0.5`: posterior mean `0.498389` (the discretised - Gaussian's mean is slightly pulled to 0 by the truncated prior), ESS `17.690938`. -- Second measurement `z = 0.7`: mean `0.599970` (≈ (0.5 + 0.7)/2), ESS `12.532656`. -- Systematic pointers for `u = 0.5`, `N = 4`: `0.125, 0.375, 0.625, 0.875` against the cumulative weights - `0.1, 0.3, 0.6, 1.0` ⇒ indices `1, 2, 3, 3`. - -## Edge cases - -- `u` close to 1 (`0.9999`) still selects valid indices (the `j < N − 1` guard). -- A particle with weight 0 is never selected by `Resample`.