From 780ea9d0d84652c52d1be3065c566d315430cb6a Mon Sep 17 00:00:00 2001 From: Claude Date: Sat, 24 Oct 2026 11:49:47 +0000 Subject: [PATCH] docs(roadmap): add specs for items 67, 70 and 74 MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit These are the generic pieces that robotics-toolbox-cpp's localization family needs: - 67 PointSetAlignment (Kabsch/Umeyama): weighted rigid or similarity fit with the reflection fix, a 2-D closed form and 3-D degeneracy rejection. Used for point-to-point ICP and calibration. - 70 ParticleFilter: bounded SIR with log-domain weights, effective sample size, and systematic resampling from a caller-supplied uniform. Motion and likelihood are passed as callables. - 74 ResidualKalmanUpdate (new item): stateless Joseph-form update from a caller-formed residual, returning the NIS, with a χ² gate. ExtendedKalmanFilter can't serve wrapped bearings, per-landmark measurement models or Mahalanobis gating, because it forms z − h(x) itself and does not expose S. All test vectors were checked in double precision. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Mmp77ieEy16GxJL8KsLsVx --- ROADMAP.md | 26 ++++- roadmap/README.md | 4 +- .../active/ParticleFilter/explanation.md | 35 +++++++ .../active/ParticleFilter/implementation.md | 97 +++++++++++++++++++ .../filters/active/ParticleFilter/tests.md | 53 ++++++++++ .../ResidualKalmanUpdate/explanation.md | 35 +++++++ .../ResidualKalmanUpdate/implementation.md | 94 ++++++++++++++++++ .../active/ResidualKalmanUpdate/tests.md | 49 ++++++++++ .../solvers/PointSetAlignment/explanation.md | 34 +++++++ .../PointSetAlignment/implementation.md | 93 ++++++++++++++++++ roadmap/solvers/PointSetAlignment/tests.md | 52 ++++++++++ 11 files changed, 565 insertions(+), 7 deletions(-) create mode 100644 roadmap/filters/active/ParticleFilter/explanation.md create mode 100644 roadmap/filters/active/ParticleFilter/implementation.md create mode 100644 roadmap/filters/active/ParticleFilter/tests.md create mode 100644 roadmap/filters/active/ResidualKalmanUpdate/explanation.md create mode 100644 roadmap/filters/active/ResidualKalmanUpdate/implementation.md create mode 100644 roadmap/filters/active/ResidualKalmanUpdate/tests.md create mode 100644 roadmap/solvers/PointSetAlignment/explanation.md create mode 100644 roadmap/solvers/PointSetAlignment/implementation.md create mode 100644 roadmap/solvers/PointSetAlignment/tests.md diff --git a/ROADMAP.md b/ROADMAP.md index 586914ef..27285615 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -28,6 +28,7 @@ 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` | ★★★☆☆ | @@ -35,10 +36,10 @@ Difficulty legend: | 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). --- diff --git a/roadmap/README.md b/roadmap/README.md index 9a6061de..3a369149 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) +`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()` (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) diff --git a/roadmap/filters/active/ParticleFilter/explanation.md b/roadmap/filters/active/ParticleFilter/explanation.md new file mode 100644 index 00000000..dd48dcfe --- /dev/null +++ b/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). diff --git a/roadmap/filters/active/ParticleFilter/implementation.md b/roadmap/filters/active/ParticleFilter/implementation.md new file mode 100644 index 00000000..4939bfc7 --- /dev/null +++ b/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 # 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 new file mode 100644 index 00000000..3506934d --- /dev/null +++ b/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 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`. diff --git a/roadmap/filters/active/ResidualKalmanUpdate/explanation.md b/roadmap/filters/active/ResidualKalmanUpdate/explanation.md new file mode 100644 index 00000000..782285bd --- /dev/null +++ b/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). diff --git a/roadmap/filters/active/ResidualKalmanUpdate/implementation.md b/roadmap/filters/active/ResidualKalmanUpdate/implementation.md new file mode 100644 index 00000000..cef8c9c4 --- /dev/null +++ b/roadmap/filters/active/ResidualKalmanUpdate/implementation.md @@ -0,0 +1,94 @@ +# Residual-Form Kalman Update with χ² Gating — Implementation Pseudocode + +> Roadmap ref: #74 (Tier 2) · Target: `numerical/filters/active` · Namespace `filters` · Type: `float` (templated on `T`, instantiated for `float` only) + +A stateless Kalman measurement update for estimators that own their state and must form the +innovation themselves. Typical cases are angles that must be wrapped (bearings, headings), a measurement +model chosen per observation (one landmark among many), and measurements that must be gated or +associated by Mahalanobis distance before they are applied. The caller passes the residual `r = z ⊖ h(x)`, +the Jacobian `H` and the noise `R`. The update returns the normalised innovation squared (NIS) and either +applies the Joseph-form update or rejects it at the gate. `ExtendedKalmanFilter` cannot serve these +cases: it forms `z − h(x)` itself, fixes `h` at construction, and hides `S`. + +## Data structures + +```cpp +enum class KalmanUpdateStatus : std::uint8_t { applied, gated, singular } + +template # static_assert(std::is_floating_point_v); instantiated for float +struct KalmanUpdateResult: + KalmanUpdateStatus status + T nis # rᵀ S⁻¹ r (0 when singular) + +template +class ResidualKalmanUpdate # stateless; static members only + using StateVector = math::Vector; using Covariance = math::SquareMatrix + using Residual = math::Vector; using Jacobian = math::Matrix + using MeasurementNoise = math::SquareMatrix +``` + +## Interface + +```cpp +static std::optional Nis(const Residual& r, const Jacobian& H, const Covariance& P, const MeasurementNoise& R) +static KalmanUpdateResult Apply(StateVector& x, Covariance& P, const Residual& r, const Jacobian& H, + const MeasurementNoise& R, T gate = std::numeric_limits::infinity()) # hot path +``` + +`Nis` lets a caller score several candidate associations before committing one with `Apply`. The caller +normalises the state afterwards (for example re-wraps a heading), because only it knows which components +are angles. + +## Algorithm (pseudocode) + +```text +function Nis(r, H, P, R): + S = CongruenceTransform(H, P) + R # H·P·Hᵀ + R; upstream MatrixOperations + return ConsistencyMetrics::Nis(r, S) # Cholesky solve; nullopt if S is not SPD + +function Apply(x, P, r, H, R, gate): # OPTIMIZE_FOR_SPEED + S = CongruenceTransform(H, P) + R + L = CholeskyDecomposition::TryFactor(S) + if L is nullopt: return { singular, 0 } # x, P untouched + y = L-solve(S, r); nis = r·y # S⁻¹r via the factor + if nis > gate: return { gated, nis } # x, P untouched + PHt = P·Hᵀ + K = (L-solve(S, PHtᵀ))ᵀ # K = P·Hᵀ·S⁻¹, M triangular-solve pairs, no explicit inverse + x += K·r + A = I − K·H + P = Symmetrize(A·P·Aᵀ + K·R·Kᵀ) # Joseph form: stays symmetric PSD in float + return { applied, nis } +``` + +## Complexity & memory + +- `O(N²·M + N·M² + M³)`: one `M×M` Cholesky and `N`-by-`M` products. For the localization cases + (`N ≤ 4`, `M ≤ 2`) that is under 200 multiply-adds. +- Stack only: `S`, its factor, `K`, `A`. No heap. + +## Numerical / embedded notes + +- **Gate values.** Gate against the χ² quantile for `M` degrees of freedom: 6.63 (M = 1) and 9.21 (M = 2) + at 99 %, 3.84 / 5.99 at 95 %. `ConsistencyMetrics` holds the two-sided 95 % bounds for NEES/NIS + monitoring; gating uses only the upper tail. +- **Joseph form.** `(I − KH)P(I − KH)ᵀ + KRKᵀ` is valid for any gain and stays PSD under `float` rounding, + unlike `(I − KH)P`. `Symmetrize` removes the asymmetry the products leave. +- **Residual convention.** The residual is `z ⊖ h(x)`: for an angle component, the caller wraps it with + `AngleDifference` (item 54). Passing an unwrapped `±2π` jump is the classic bearing-EKF divergence, and + the test shows it. +- **Sequential updates.** Independent measurements (several landmarks) can be applied one after + another, each with its own `H` and `R`; for a linear model that equals one stacked update. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `numerical/filters/active/ResidualKalmanUpdate.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on + `Apply`, and `extern template class ResidualKalmanUpdate;` / `` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/filters/active/ResidualKalmanUpdate.cpp` → the same instantiations. +- Test: `numerical/filters/active/test/TestResidualKalmanUpdate.cpp` +- Doc: `doc/filters/active/ResidualKalmanUpdate.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`. +- Reuses: `CholeskyDecomposition`, `ConsistencyMetrics`, `MatrixOperations` (`CongruenceTransform`, `Symmetrize`). +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/filters/active/ResidualKalmanUpdate/tests.md b/roadmap/filters/active/ResidualKalmanUpdate/tests.md new file mode 100644 index 00000000..6307b763 --- /dev/null +++ b/roadmap/filters/active/ResidualKalmanUpdate/tests.md @@ -0,0 +1,49 @@ +# Residual-Form Kalman Update — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestResidualKalmanUpdate : public ::testing::Test: + Vector x{ 1.0, 0.5 } + SquareMatrix P{ { 0.5, 0.1 }, { 0.1, 0.3 } } + Matrix H{ { 1, 0 } }; SquareMatrix R{ 0.2 } +# each case below is a TEST_F(TestResidualKalmanUpdate, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +scalar_update_matches_the_kalman_equations: + Act: Apply(x, P, r = (0.4), H, R) # z = 1.4 + Assert: status applied, nis ≈ 0.228571; x ≈ (1.285714, 0.557143); P ≈ [[0.142857, 0.028571], [0.028571, 0.285714]] +vector_update_matches_the_kalman_equations: + Arrange: H₂ = [[1, 0], [1, 1]], R₂ = diag(0.2, 0.1), r = (1.4, 1.2) − H₂x = (0.4, −0.3) + Assert: nis ≈ 0.934146; x ≈ (1.097561, 0.212195); P ≈ [[0.092683, −0.063415], [−0.063415, 0.117073]] +nis_matches_the_applied_update: + Assert: Nis((0.4), H, P, R) ≈ 0.228571 == Apply(...).nis +gate_rejects_an_outlier_without_touching_the_state: + Act: Apply(x, P, r = (3.0), H, R, 6.63) # z = 4.0 + Assert: status gated, nis ≈ 12.857143; x and P unchanged +singular_innovation_covariance_is_reported: + Arrange: P = 0, R = 0 + Assert: status singular; x and P unchanged; Nis(...) == nullopt +wrapped_residual_keeps_an_angle_estimate_sane: + Arrange: x = (3.1), P = (0.04), R = (0.01), z = −3.1 + Act: Apply with r = AngleDifference(z, 3.1) = 0.083185, then WrapToPi(x) + Assert: x ≈ −3.116637; P ≈ 0.008; nis ≈ 0.138396 # the raw residual −6.2 would drive x to −1.86 +joseph_form_stays_symmetric: + Assert: after 100 scalar updates of the fixture, P(0,1) == P(1,0) and both eigenvalues > 0 +``` + +## Reference vectors + +- Fixture: `S = 0.7`, `K = (0.714286, 0.142857)`. +- 2-D: `S = [[0.7, 0.6], [0.6, 1.1]]`, `NIS = 0.934146`. +- Angle: residual `0.083185` (wrapped) against `−6.2` (raw). + +## Edge cases + +- `gate = ∞` (default) never gates. +- `nis` exactly at the gate is applied (`>` test). diff --git a/roadmap/solvers/PointSetAlignment/explanation.md b/roadmap/solvers/PointSetAlignment/explanation.md new file mode 100644 index 00000000..cc13bde1 --- /dev/null +++ b/roadmap/solvers/PointSetAlignment/explanation.md @@ -0,0 +1,34 @@ +# Point-Set Alignment (Kabsch / Umeyama) — Overview + +## What it is +Given two sets of matching points — the same landmarks seen in two frames, a scan and a map, a +calibration target in two poses — it finds the rotation, translation and, optionally, the uniform scale +that best overlays one set on the other in the least-squares sense. + +## Why it matters (embedded) +This is the core step of scan matching (ICP), sensor-to-sensor and hand-eye calibration, and fiducial +or motion-capture registration. The solution is closed-form: one small singular-value decomposition in +3-D, a single arctangent in 2-D. It runs in a bounded time with no iteration and no memory beyond a +few 3×3 matrices. + +## How it works (intuition) +First both point sets are shifted so their weighted centres coincide; the remaining mismatch is pure +rotation (and scale). The cross-covariance matrix of the centred sets says how directions in one set +correlate with directions in the other, and its singular-value decomposition gives the rotation that +maximises this agreement. If the best orthogonal match happens to be a mirror image, the weakest axis is +flipped so the result stays a real rotation. The scale is the ratio of the matched spread to the source +spread, and the translation moves the rotated source centre onto the target centre. + +## Key parameters +- **correspondences** — `target[i]` matches `source[i]`. +- **weights** — per-point confidence (0 drops a point). +- **estimate scale** — rigid (Kabsch) or similarity (Umeyama) fit. + +## Reference +W. Kabsch, "A solution for the best rotation to relate two sets of vectors," *Acta Crystallographica* +A32, 1976; S. Umeyama, "Least-squares estimation of transformation parameters between two point +patterns," *IEEE T-PAMI*, 13(4), 1991. + +## See also +`SingularValueDecomposition` (the 3-D solve), `SE3Transform` / `SE2Transform` (items 53, 66: wrap the result); +robotics-toolbox-cpp's ICP scan matching (E5) and hand-eye calibration (C1). diff --git a/roadmap/solvers/PointSetAlignment/implementation.md b/roadmap/solvers/PointSetAlignment/implementation.md new file mode 100644 index 00000000..dc100f75 --- /dev/null +++ b/roadmap/solvers/PointSetAlignment/implementation.md @@ -0,0 +1,93 @@ +# Point-Set Alignment (Kabsch / Umeyama) — Implementation Pseudocode + +> Roadmap ref: #67 (Tier 2) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) + +Weighted least-squares rigid (or similarity) transform between two corresponding point sets: +find `R ∈ SO(d)`, `t` and optionally `s > 0` minimising `Σ wᵢ ‖qᵢ − (s·R·pᵢ + t)‖²`. This is the inner step +of point-to-point ICP, hand-eye and frame calibration, fiducial and motion-capture registration. In 3-D +it is one 3×3 SVD with Kabsch's reflection fix; in 2-D a closed-form `atan2`. + +## Data structures + +```cpp +template # static_assert(std::is_floating_point_v); static_assert(Dim == 2 || Dim == 3) +struct Alignment: + math::SquareMatrix rotation # proper: det = +1 + math::Vector translation + T scale{ 1 } # 1 unless estimated + T rms # weighted residual RMS after alignment + +template +class PointSetAlignment # stateless; static members only +``` + +## Interface + +```cpp +static std::optional> Align(std::span> source, + std::span> target, + bool estimateScale = false, + std::span weights = {}) # empty = all 1 +``` + +`target[i]` corresponds to `source[i]`. Returns `nullopt` when the sizes differ, when fewer than `Dim` +points carry positive weight, or when the geometry leaves the rotation undetermined (see below). + +## Algorithm (pseudocode) + +```text +function Align(p, q, estimateScale, w): # OPTIMIZE_FOR_SPEED + W = Σ wᵢ; μp = Σ wᵢ pᵢ / W; μq = Σ wᵢ qᵢ / W + a_i = pᵢ − μp; b_i = qᵢ − μq + σp² = Σ wᵢ ‖a_i‖² / W; σq² = Σ wᵢ ‖b_i‖² / W # spreads: σp² sets the scale + Σ = Σ wᵢ b_i a_iᵀ / W # Dim×Dim cross-covariance + + if Dim == 2: # closed form, no SVD, never a reflection + c = Σ₀₀ + Σ₁₁; s_ = Σ₁₀ − Σ₀₁ # Σ wᵢ (a·b) and Σ wᵢ (a × b), over W + if Hypot(c, s_) ≤ ε·σp·σq: return nullopt # all points coincide + θ = atan2(s_, c); R = Rot(θ); trace = Hypot(c, s_) + else: + SVD: Σ = U·diag(d)·Vᵀ # solvers::SingularValueDecomposition + if d₁ ≤ ε·d₀: return nullopt # collinear (or coincident) points: rotation about the line is free + E = diag(1, 1, sign(det U · det V)) # Kabsch: never return a reflection + R = U·E·Vᵀ; trace = d₀ + d₁ + E₂₂·d₂ + + s = estimateScale ? trace / σp² : 1 # Umeyama (1991), eq. 42 + t = μq − s·R·μp + rms = √( Σ wᵢ ‖qᵢ − (s·R·pᵢ + t)‖² / W ) + return { R, t, s, rms } +``` + +`ε = 1e-5`. `det U` and `det V` use the explicit 3×3 cofactor formula. + +## Complexity & memory + +- `O(N·Dim²)` accumulation plus one 3×3 SVD (3-D) or one `atan2` (2-D). One pass for the moments and + one for the residual. +- Stack only: a few `Dim×Dim` matrices. The point sets stay in the caller's storage. No heap. + +## Numerical / embedded notes + +- **Reflection fix.** The unconstrained Procrustes solution `U·Vᵀ` can be a reflection (`det = −1`) for + noisy or near-planar data; `E` flips the weakest axis instead. The test feeds a mirrored point set and + checks that the result is a proper rotation with a non-zero residual. +- **Degeneracy.** In 3-D a rank-1 cross-covariance (collinear points) leaves the rotation about the line + free, so `Align` refuses instead of returning an arbitrary answer. Coplanar points are fine: the sign + fix resolves them. In 2-D only coincident points are degenerate. +- **Centring first.** Accumulating `Σ` on centred coordinates avoids the `float` cancellation of the + textbook `Σ pqᵀ − N·μp μqᵀ` form when the clouds are far from the origin (scan matching in map + coordinates). +- Weights let ICP (robotics E5) down-weight or drop outliers without copying the point sets. +- Float-only: `static_assert(std::is_floating_point_v)`. + +## Deployment + +- Header: `numerical/solvers/PointSetAlignment.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Align`, and + `extern template class PointSetAlignment;` / `` under + `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. +- Coverage: `numerical/solvers/PointSetAlignment.cpp` → the same instantiations. +- Test: `numerical/solvers/test/TestPointSetAlignment.cpp` +- Doc: `doc/solvers/PointSetAlignment.md` (per `doc/TEMPLATE.md`) + row in `doc/solvers/README.md` +- CMake: `.hpp` → `target_sources(numerical.solver PRIVATE ...)`; `.cpp` → + `numerical_add_coverage_sources(numerical.solver ...)`; test → `numerical.solvers_test`. +- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/PointSetAlignment/tests.md b/roadmap/solvers/PointSetAlignment/tests.md new file mode 100644 index 00000000..9bc6d281 --- /dev/null +++ b/roadmap/solvers/PointSetAlignment/tests.md @@ -0,0 +1,52 @@ +# Point-Set Alignment — Unit Test Plan (Pseudocode) + +> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. + +## Fixture + +```cpp +class TestPointSetAlignment : public ::testing::Test: + std::array, 4> planar{ (0, 0), (1, 0), (0, 2), (−1, 1) } + std::array, 5> spatial{ (0, 0, 0), (1, 0, 0), (0, 2, 0), (0, 0, 3), (1, 1, 1) } + Matrix3 rotation{ RotationAboutAxis((1, 2, 3)/‖·‖, 0.7) } # Geometry3D + Vector3 offset{ 0.2, −0.4, 1.0 } +# each case below is a TEST_F(TestPointSetAlignment, ) +``` + +## Test cases (Arrange / Act / Assert) + +```cpp +planar_rigid_motion_is_recovered_exactly: + Arrange: target = Rot(30°)·planar + (0.5, −1) + Assert: angle ≈ π/6, translation ≈ (0.5, −1), rms ≈ 0 +spatial_rigid_motion_is_recovered_exactly: + Arrange: target = rotation·spatial + offset + Assert: rotation ≈ the fixture rotation (1e-5), translation ≈ offset, rms ≈ 0 +umeyama_recovers_the_scale: + Arrange: target = 2.5·rotation·spatial + offset, estimateScale = true + Assert: scale ≈ 2.5, translation ≈ offset, rms ≈ 0 +mirrored_points_still_give_a_proper_rotation: + Arrange: target = spatial with z negated + Assert: det(rotation) ≈ +1, rms ≈ 0.925196 # a reflection would fit with rms 0 +zero_weight_removes_an_outlier: + Arrange: target = rotation·spatial + offset, target[4] += (5, 5, 5), weights { 1, 1, 1, 1, 0 } + Assert: rotation ≈ fixture rotation, rms ≈ 0 # with unit weights rms = 3.442415 +noisy_planar_fit_matches_the_reference: + Arrange: target as in the first case plus noise { (0.01, −0.02), (0, 0.015), (−0.01, 0), (0.02, 0.01) } + Assert: angle ≈ 0.528695, translation ≈ (0.508305, −0.996831), rms ≈ 0.016549 +degenerate_inputs_are_rejected: + Assert: collinear { (0,0,0), (1,1,1), (2,2,2) } ⇒ nullopt; one 2-D point ⇒ nullopt; + size mismatch ⇒ nullopt +``` + +## Reference vectors + +- Fixture rotation `Rot((1,2,3)/√14, 0.7)` = + `[[0.781639, −0.482929, 0.394740], [0.550117, 0.832030, −0.071392], [−0.293958, 0.272956, 0.916015]]`. +- Mirrored `spatial` (z → −z): best proper rotation leaves `rms = 0.925196`. +- Collinear set: singular values `(2, 0, 0)` ⇒ rejected. + +## Edge cases + +- Exactly `Dim` non-degenerate points (2 in 2-D, 3 non-collinear in 3-D) ⇒ exact solution. +- Translated clouds far from the origin (offset `10³`) keep `rms ≈ 0` to `1e-3`, thanks to centring.