docs(roadmap): add specs for items 67, 70 and 74

ROADMAP.md

@@ -28,17 +28,18 @@ Difficulty legend:
 |----|-------------------------------------------------------|-------------------------|------------|
 | 66 | SE(2) transform, twists, exp/log, pose error          | `math`                  | ★★☆☆☆      |
 | 67 | Kabsch / Umeyama point-set alignment                  | `solvers`               | ★★☆☆☆      |
+| 74 | Residual-form Kalman update with χ² gating            | `filters/active`        | ★★☆☆☆      |
 | 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 66–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
-NT66–NT73; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization
-families and also have non-robotics consumers. Item 66 has a spec under [`roadmap/`](roadmap/README.md);
-items 67–73 have none yet. All are *(float-first)*.
+Items 66–74 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
+NT66–NT74; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization
+families and also have non-robotics consumers. Items 66, 67, 70 and 74 have specs under
+[`roadmap/`](roadmap/README.md); items 68, 69 and 71–73 have none 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
@@ -64,6 +65,20 @@ the library does not yet expose. Detailed below under
   and fiducial registration.
 - **Algorithm / paper:** W. Kabsch, *Acta Cryst.* A32, 1976; S. Umeyama, *IEEE T-PAMI*, 13(4), 1991.
 - **Reuses:** `SingularValueDecomposition`.
+- **Spec:** [roadmap/solvers/PointSetAlignment](roadmap/solvers/PointSetAlignment/implementation.md).
+
+### 74. Residual-form Kalman update with χ² gating
+- **What:** Stateless Kalman measurement update from a caller-formed residual `z ⊖ h(x)`, Jacobian and
+  noise: Joseph-form correction, normalised innovation squared, and a χ² gate that rejects outliers
+  without touching the state.
+- **Embedded value:** Covers what `ExtendedKalmanFilter` cannot: wrapped angle residuals (bearings,
+  headings), a measurement model chosen per observation (landmarks), and gating and data association
+  by Mahalanobis distance.
+- **Algorithm / paper:** P. Maybeck, *Stochastic Models, Estimation, and Control*, Vol. 1 (1979), Ch. 5;
+  Y. Bar-Shalom, X.-R. Li, T. Kirubarajan, *Estimation with Applications to Tracking and Navigation*
+  (2001), Ch. 5.
+- **Reuses:** `CholeskyDecomposition`, `ConsistencyMetrics`, `MatrixOperations`; item 54 for angle residuals.
+- **Spec:** [roadmap/filters/active/ResidualKalmanUpdate](roadmap/filters/active/ResidualKalmanUpdate/implementation.md).
 
 ---
 
@@ -93,7 +108,8 @@ the library does not yet expose. Detailed below under
   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 deterministic PRNG; `math::Statistics`.
+- **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).
 
 ---
 

roadmap/README.md

@@ -80,7 +80,7 @@ with `EXPECT_NEAR` and `math::Tolerance<float>()` (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)
+`AlphaBetaFilter` (8) · `ComplementaryFilter` (9) · `AhrsMadgwickMahony` (33) · `SquareRootKalmanFilter` (39) · `ParticleFilter` (70) · `ResidualKalmanUpdate` (74)
 
 ### `controllers`
 `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20)
@@ -107,4 +107,4 @@ with `EXPECT_NEAR` and `math::Tolerance<float>()` (or an explicit tolerance).
 `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32)
 
 ### `solvers`
-`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43)
+`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `PointSetAlignment` (67)

roadmap/filters/active/ParticleFilter/explanation.md

@@ -0,0 +1,35 @@
+# 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).

roadmap/filters/active/ParticleFilter/implementation.md

@@ -0,0 +1,97 @@
+# 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<typename T, std::size_t StateSize, std::size_t NumParticles>   # static_assert(std::is_floating_point_v<T>); NumParticles ≥ 2
+class ParticleFilter:
+    using StateVector = math::Vector<T, StateSize>
+    std::array<StateVector, NumParticles> buffers[2]      # current and resampling target
+    std::size_t                           active{ 0 }
+    std::array<T, NumParticles>           weights         # normalised, Σ = 1
+    std::array<T, NumParticles>           scratch         # log-domain work area
+```
+
+## Interface
+
+```cpp
+template<class Draw>        void Initialize(Draw&& draw)                   # particles[i] = draw(i); weights = 1/N
+template<class Propagate>   void Predict(Propagate&& propagate)            # propagate(particle&, i) in place; hot path
+template<class LogLikelihood> 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<const StateVector> Particles() const;  std::span<StateVector> Particles()
+std::span<const T>           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<T>)`.
+
+## Deployment
+
+- Header: `numerical/filters/active/ParticleFilter.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on
+  `Predict`/`Update`/`Resample`, and `extern template class ParticleFilter<float, 3, 64>;` under
+  `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD` (the template members are instantiated by the tests).
+- Coverage: `numerical/filters/active/ParticleFilter.cpp` → `template class ParticleFilter<float, 3, 64>;`
+- 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`.

roadmap/filters/active/ParticleFilter/tests.md

@@ -0,0 +1,53 @@
+# Particle Filter — Unit Test Plan (Pseudocode)
+
+> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap.
+
+## Fixture
+
+```cpp
+class TestParticleFilter : public ::testing::Test:
+    ParticleFilter<float, 1, 41> grid      # Initialize(i ⇒ −2 + 4i/40): an evenly spaced prior on [−2, 2]
+    ParticleFilter<float, 1, 4>  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, <name>)
+```
+
+## 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`.

roadmap/filters/active/ResidualKalmanUpdate/explanation.md

@@ -0,0 +1,35 @@
+# Residual-Form Kalman Update — Overview
+
+## What it is
+The measurement-update half of a Kalman filter, packaged as a standalone step. The caller states how far
+the measurement is from the prediction (the residual), how that measurement depends on the state and how
+noisy it is. The step corrects the state and its uncertainty, or refuses if the measurement is
+implausible.
+
+## Why it matters (embedded)
+Real estimators rarely fit a fixed "measure, subtract, correct" mould. A bearing of 179° against a
+prediction of −179° is a 2° error, not 358°. A robot seeing one landmark among many needs a different
+measurement model each time. A reflection or a mismatched landmark must be thrown out before it
+corrupts the estimate. Letting the caller form the residual and checking it against the predicted
+uncertainty covers all three in a few dozen operations for small states.
+
+## How it works (intuition)
+Combining the predicted uncertainty with the measurement noise gives the expected spread of the residual.
+Dividing the actual residual by that spread (in the matrix sense) yields the normalised innovation
+squared — how many "standard deviations" away the measurement is. If that exceeds a χ² gate the
+measurement is ignored. Otherwise the Kalman gain blends prediction and measurement, and the covariance is
+updated in Joseph form, which keeps it a valid covariance even with rounding.
+
+## Key parameters
+- **residual** `z ⊖ h(x)` — formed by the caller (wrapped for angles).
+- **measurement Jacobian `H`** and **noise `R`**.
+- **gate** — χ² threshold on the NIS (e.g. 9.21 for 2-D at 99 %).
+
+## Reference
+P. S. Maybeck, *Stochastic Models, Estimation, and Control*, Vol. 1 (1979), Ch. 5 (Joseph form);
+Y. Bar-Shalom, X.-R. Li, T. Kirubarajan, *Estimation with Applications to Tracking and Navigation* (2001),
+Ch. 5 (validation gates).
+
+## See also
+`ExtendedKalmanFilter` (fixed-model filter), `ConsistencyMetrics` (NIS/NEES), `AngleUtilities` (item 54);
+robotics-toolbox-cpp's odometry fusion (E1) and landmark localization (E3).