gabrielfrasantos · Nov 6, 2026 · Nov 6, 2026
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -0,0 +1,13 @@
+#include "numerical/filters/active/ParticleFilter.hpp"
+
+namespace filters
+{
+    template class ParticleFilter<float, 3, 64>;
+    template class ParticleFilter<float, 1, 41>;
+    template class ParticleFilter<float, 1, 4>;
+    template void ParticleFilter<float, 1, 41>::Initialize<math::Vector<float, 1> (&)(std::size_t)>(math::Vector<float, 1> (&)(std::size_t));
+    template void ParticleFilter<float, 1, 4>::Initialize<math::Vector<float, 1> (&)(std::size_t)>(math::Vector<float, 1> (&)(std::size_t));
+    template bool ParticleFilter<float, 1, 41>::Update<float (&)(const math::Vector<float, 1>&)>(float (&)(const math::Vector<float, 1>&));
+    template bool ParticleFilter<float, 1, 4>::Update<float (&)(const math::Vector<float, 1>&)>(float (&)(const math::Vector<float, 1>&));
+    template void ParticleFilter<float, 1, 4>::Predict<void (&)(math::Vector<float, 1>&, std::size_t)>(void (&)(math::Vector<float, 1>&, std::size_t));
+}