Skip to content

Commit bf5cc90

Browse files
feat(filters): add ParticleFilter (NT70) (#369)
Bounded sequential importance resampling: two compile-time particle buffers, caller-supplied draw, motion and log-likelihood callables, a log-domain update with max subtraction that rejects an impossible measurement without touching the weights, the effective sample size, systematic resampling from a caller-provided uniform, and weighted-mean and maximum-weight estimates. Claude-Session: https://claude.ai/code/session_014WUWh44uw7k4JjMd6YkAwN Co-authored-by: Claude <noreply@anthropic.com>
1 parent 182e068 commit bf5cc90

13 files changed

Lines changed: 489 additions & 210 deletions

File tree

‎README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
2020
| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space |
2121
| [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) |
2222
| [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) |
23-
| [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 |
23+
| [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 |
2424
| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt |
2525
| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) |
2626
| [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) |

‎ROADMAP.md‎

Lines changed: 4 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -28,15 +28,14 @@ Difficulty legend:
2828
|----|-------------------------------------------------------|-------------------------|------------|
2929
| 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ |
3030
| 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ |
31-
| 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ |
3231
| 71 | Error-state (multiplicative) EKF on Lie groups | `filters/active` | ★★★★☆ |
3332
| 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ |
3433
| 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ |
3534

36-
Items 68–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
37-
NT68–NT73; NT53–NT67 and NT74 are deployed). They serve its mobile, aerial, legged, marine and
38-
localization families and also have non-robotics consumers. Item 70 has a spec under
39-
[`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none yet. All are *(float-first)*.
35+
Items 68, 69 and 71–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there
36+
as NT68, NT69 and NT71–NT73; NT53–NT67, NT70 and NT74 are deployed). They serve its aerial, legged and
37+
marine families and also have non-robotics consumers. None has a spec under
38+
[`roadmap/`](roadmap/README.md) yet. All are *(float-first)*.
4039

4140
Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family
4241
unit-test reference [`TESTING.md`](TESTING.md) depends on but which
@@ -68,16 +67,6 @@ All Tier 2 items are deployed.
6867
49(5), 2013; J. Bordignon, *Constrained control allocation* (PhD thesis, Virginia Tech, 1996).
6968
- **Reuses:** Items 62 and 65.
7069

71-
### 70. Particle filter
72-
- **What:** Sequential importance resampling with a compile-time particle count, systematic resampling
73-
triggered by the effective sample size, and weighted-mean / MAP estimates.
74-
- **Embedded value:** Multimodal, non-Gaussian estimation — Monte Carlo localization, terrain-aided
75-
navigation, fault detection — on fixed memory.
76-
- **Algorithm / paper:** N. Gordon, D. Salmond, A. Smith, *IEE Proc. F*, 140(2), 1993; A. Doucet,
77-
A. Johansen, "A tutorial on particle filtering and smoothing" (2009).
78-
- **Reuses:** A caller-injected uniform number for resampling; caller-supplied motion and likelihood callables.
79-
- **Spec:** [roadmap/filters/active/ParticleFilter](roadmap/filters/active/ParticleFilter/implementation.md).
80-
8170
---
8271

8372
## Tier 4 — Advanced ★★★★☆

‎doc/filters/README.md‎

Lines changed: 9 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -8,14 +8,15 @@ Digital filtering algorithms for noise reduction, state estimation, and signal c
88

99
Filters that incorporate feedback and a dynamic internal model of the system.
1010

11-
| Algorithm | Description |
12-
|---------------------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------|
13-
| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise |
14-
| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) |
15-
| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required |
16-
| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence |
17-
| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness |
18-
| [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 |
11+
| Algorithm | Description |
12+
|---------------------------------------------------------------|------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
13+
| [Kalman Filter](active/KalmanFilter.md) | Optimal recursive state estimator for linear systems with Gaussian noise |
14+
| [Extended Kalman Filter](active/ExtendedKalmanFilter.md) | Nonlinear state estimator using first-order linearization (Jacobians) |
15+
| [Unscented Kalman Filter](active/UnscentedKalmanFilter.md) | Nonlinear state estimator using sigma points; no Jacobians required |
16+
| [Kalman Smoother](active/KalmanSmoother.md) | Offline RTS smoother providing MMSE estimates over the full observation sequence |
17+
| [Square-Root Kalman Filter](active/SquareRootKalmanFilter.md) | Numerically robust Kalman filter propagating a Cholesky factor to guarantee positive-definiteness |
18+
| [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 |
19+
| [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 |
1920

2021
### Passive Filters
2122

Lines changed: 98 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,98 @@
1+
# Particle Filter (Bounded SIR, Systematic Resampling)
2+
3+
## Overview & Motivation
4+
5+
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.
6+
7+
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.
8+
The representation handles any dynamics, any likelihood and any number of modes, at a cost proportional to the number of particles.
9+
10+
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.
11+
12+
## Mathematical Theory
13+
14+
### Sequential Importance Resampling
15+
16+
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:
17+
18+
$$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)})$$
19+
20+
Prediction samples the motion model, and the update multiplies each weight by the measurement likelihood.
21+
22+
### Log-Domain Update
23+
24+
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:
25+
26+
$$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}}$$
27+
28+
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.
29+
30+
### Effective Sample Size
31+
32+
When a few particles carry nearly all the weight, the rest are wasted computation. The effective sample size measures this:
33+
34+
$$N_{\text{eff}} = \frac{1}{\sum_i \left(w^{(i)}\right)^2}$$
35+
36+
It equals $N$ for uniform weights and $1$ when one particle holds everything.
37+
38+
### Systematic Resampling
39+
40+
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:
41+
42+
$$p_k = \frac{u + k}{N}, \qquad k = 0, \ldots, N - 1$$
43+
44+
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.
45+
46+
## Complexity Analysis
47+
48+
| Operation | Time | Space | Notes |
49+
|----------------|--------|----------------|----------------------------------------------------------------|
50+
| Predict | $O(N)$ | $O(1)$ | One motion-model call per particle |
51+
| Update | $O(N)$ | $O(N)$ | One likelihood, one logarithm and one exponential per particle |
52+
| Resample | $O(N)$ | $O(N)$ | One pass over the cumulative weights |
53+
| Particle store | — | $O(N \cdot d)$ | Two particle buffers plus weights and scratch |
54+
55+
For $N$ particles of dimension $d$. A 3-state filter with 500 particles uses 16 KB in single precision.
56+
57+
## Step-by-Step Walkthrough
58+
59+
1. **Uniform prior**: 41 particles evenly spaced on $[-2, 2]$ start with weights $1/41$ and an effective sample size of 41.
60+
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$.
61+
3. **Underflow**: subtracting $1000$ from every log-likelihood leaves the normalised weights unchanged. Without the log-domain update, every weight would underflow to zero.
62+
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$.
63+
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.
64+
6. **Threshold**: with a threshold of half the particles (2), no resampling happens. With $0.9$ of them (3.6), it does.
65+
7. **Impossible measurement**: a likelihood of $-\infty$ everywhere is rejected, and the weights stay as they were.
66+
67+
## Pitfalls & Edge Cases
68+
69+
- **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.
70+
- **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.
71+
- **Angles**: the component-wise mean fails for headings across $\pm\pi$. Compute a circular mean from the weights, or use the highest-weight particle.
72+
- **Too few particles**: the required count grows quickly with the state dimension. Particle filters suit low-dimensional states.
73+
- **Kidnapping**: once all particles are wrong, the filter cannot recover by itself. Inject a few random particles when the average likelihood drops.
74+
75+
## Variants & Generalizations
76+
77+
- **Auxiliary and regularised particle filters**: better proposals and jittered resampling against impoverishment.
78+
- **Rao–Blackwellised filters**: particles for the nonlinear part and Kalman filters for the rest, as in FastSLAM.
79+
- **KLD sampling**: the particle count adapts to the spread of the belief.
80+
- **Unscented and extended Kalman filters**: Gaussian alternatives for unimodal, mildly nonlinear problems.
81+
82+
## Applications
83+
84+
- Monte Carlo localization of mobile robots in occupancy grids.
85+
- Visual tracking of objects through occlusions.
86+
- Terrain-aided navigation and fault diagnosis with discrete modes.
87+
88+
## Connections to Other Algorithms
89+
90+
- [Kalman Filter](KalmanFilter.md): the optimal linear-Gaussian special case.
91+
- [Unscented Kalman Filter](UnscentedKalmanFilter.md): deterministic sigma points instead of random particles, for unimodal beliefs.
92+
- [Residual-Form Kalman Update](ResidualKalmanUpdate.md): the Gaussian update for problems where a particle filter is too costly.
93+
94+
## References & Further Reading
95+
96+
- 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.
97+
- Doucet, A. and Johansen, A.M. (2009). "A tutorial on particle filtering and smoothing: fifteen years later." *Handbook of Nonlinear Filtering*, Oxford University Press.
98+
- Thrun, S., Burgard, W. and Fox, D. (2005). *Probabilistic Robotics*. MIT Press. Chapter 4.

‎numerical/filters/active/CMakeLists.txt‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,6 +19,7 @@ target_sources(numerical.filters.active PRIVATE
1919
KalmanFilter.hpp
2020
KalmanFilterBase.hpp
2121
KalmanSmoother.hpp
22+
ParticleFilter.hpp
2223
ResidualKalmanUpdate.hpp
2324
SquareRootKalmanFilter.hpp
2425
UnscentedKalmanFilter.hpp
@@ -32,6 +33,7 @@ numerical_add_coverage_sources(numerical.filters.active
3233
KalmanFilter.cpp
3334
KalmanFilterBase.cpp
3435
KalmanSmoother.cpp
36+
ParticleFilter.cpp
3537
ResidualKalmanUpdate.cpp
3638
SquareRootKalmanFilter.cpp
3739
UnscentedKalmanFilter.cpp
Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
#include "numerical/filters/active/ParticleFilter.hpp"
2+
3+
namespace filters
4+
{
5+
template class ParticleFilter<float, 3, 64>;
6+
template class ParticleFilter<float, 1, 41>;
7+
template class ParticleFilter<float, 1, 4>;
8+
template void ParticleFilter<float, 1, 41>::Initialize<math::Vector<float, 1> (&)(std::size_t)>(math::Vector<float, 1> (&)(std::size_t));
9+
template void ParticleFilter<float, 1, 4>::Initialize<math::Vector<float, 1> (&)(std::size_t)>(math::Vector<float, 1> (&)(std::size_t));
10+
template bool ParticleFilter<float, 1, 41>::Update<float (&)(const math::Vector<float, 1>&)>(float (&)(const math::Vector<float, 1>&));
11+
template bool ParticleFilter<float, 1, 4>::Update<float (&)(const math::Vector<float, 1>&)>(float (&)(const math::Vector<float, 1>&));
12+
template void ParticleFilter<float, 1, 4>::Predict<void (&)(math::Vector<float, 1>&, std::size_t)>(void (&)(math::Vector<float, 1>&, std::size_t));
13+
}

0 commit comments

Comments
 (0)