From 046c0b0fb667964350882548aa6bcead9f2d57ef Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:32:03 +0000 Subject: [PATCH 1/6] feat(math): add vector-valued cubic spline interpolation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 61: C² cubic spline through up to MaxKnots knots with clamped or natural ends. All channels share the abscissa and one TridiagonalSolver factorization. Evaluate clamps x to the knot range, finds the interval by binary search and returns value, slope and curvature per channel. Checked on 2000 random fits beyond the unit tests: knots reproduced, one-sided first and second derivatives agree at interior knots (≈1e-13 in double), and end conditions hold. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 16 +- ROADMAP.md | 10 - doc/math/CubicSpline.md | 114 +++++++++++ doc/math/README.md | 31 +-- numerical/math/CMakeLists.txt | 2 + numerical/math/CubicSpline.cpp | 6 + numerical/math/CubicSpline.hpp | 223 +++++++++++++++++++++ numerical/math/test/CMakeLists.txt | 1 + numerical/math/test/TestCubicSpline.cpp | 143 +++++++++++++ roadmap/README.md | 2 +- roadmap/math/CubicSpline/explanation.md | 36 ---- roadmap/math/CubicSpline/implementation.md | 116 ----------- roadmap/math/CubicSpline/tests.md | 76 ------- 13 files changed, 514 insertions(+), 262 deletions(-) create mode 100644 doc/math/CubicSpline.md create mode 100644 numerical/math/CubicSpline.cpp create mode 100644 numerical/math/CubicSpline.hpp create mode 100644 numerical/math/test/TestCubicSpline.cpp delete mode 100644 roadmap/math/CubicSpline/explanation.md delete mode 100644 roadmap/math/CubicSpline/implementation.md delete mode 100644 roadmap/math/CubicSpline/tests.md diff --git a/README.md b/README.md index 89f27dcf..dbc2d218 100644 --- a/README.md +++ b/README.md @@ -21,14 +21,14 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [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 | | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, 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 | -| [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 | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers | +| [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 | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/ROADMAP.md b/ROADMAP.md index f4f382d3..349759d4 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | -| 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ | | 62 | Damped pseudo-inverse + null-space projector | `solvers` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | @@ -65,15 +64,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`. - **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md). -### 61. Cubic spline interpolation -- **What:** C² vector-valued spline through `K` knots, clamped or natural ends, shared factorization across - channels. -- **Embedded value:** Smooth interpolation of waypoint lists and calibration tables in linear time on fixed - storage. -- **Algorithm / paper:** C. de Boor, *A Practical Guide to Splines* (2001), Ch. IV. -- **Reuses:** Item 59. -- **Spec:** [roadmap/math/CubicSpline](roadmap/math/CubicSpline/implementation.md). - ### 62. Damped pseudo-inverse and null-space projector - **What:** `A⁺_λ·b` and the exact projector `I − A⁺A` for any `Rows × Cols` matrix from one SVD. - **Embedded value:** Singularity-robust inversion for redundant robots, over-actuated control allocation diff --git a/doc/math/CubicSpline.md b/doc/math/CubicSpline.md new file mode 100644 index 00000000..703f88b0 --- /dev/null +++ b/doc/math/CubicSpline.md @@ -0,0 +1,114 @@ +# Cubic Spline Interpolation (Vector-Valued, Clamped / Natural) + +## Overview & Motivation + +Waypoint lists (robot teach-in programs, CNC paths, planner output), calibration tables and resampled signals all need a smooth curve through given samples. A single high-order polynomial through many points rings between them; a piecewise-linear curve has corners that excite mechanics. A **cubic spline** joins one cubic per interval so that value, slope **and** curvature are continuous ($C^2$), and it stays well behaved however many knots are used. + +Several channels (the joints of a robot, the axes of a path, the columns of a table) share the same abscissae. They are fitted together: the linear system depends only on the knot spacing, so it is factored once and every channel reuses the factors. Fitting is a single linear-time pass on fixed storage; evaluation is a binary search plus a cubic, cheap and deterministic. + +## Mathematical Theory + +### Unknowns and per-interval form + +Knots $x_0 < x_1 < \dots < x_{K-1}$ with spans $h_k = x_{k+1} - x_k$ and values $y_k$ (one component per channel; channels are independent). The unknowns are the knot second derivatives $M_k = y''(x_k)$. On interval $k$, with $\tau = x - x_k \in [0, h_k]$, + +$$y(\tau) = y_k + \beta_k\,\tau + \tfrac{M_k}{2}\,\tau^2 + d_k\,\tau^3, \qquad d_k = \frac{M_{k+1} - M_k}{6\,h_k}, \qquad \beta_k = \frac{y_{k+1} - y_k}{h_k} - \frac{h_k\,(2M_k + M_{k+1})}{6}$$ + +The cubic hits both knot values and both knot curvatures, so value is $C^0$ and curvature is $C^2$ by construction. Only slope continuity remains to be imposed. + +### Interior rows + +Equating the slope at the end of interval $k-1$, $\beta_{k-1} + h_{k-1}(M_{k-1} + M_k)/2$, with $\beta_k$ and writing $s_k = (y_{k+1} - y_k)/h_k$ gives, for $k = 1, \dots, K-2$, + +$$h_{k-1} M_{k-1} + 2(h_{k-1} + h_k)\,M_k + h_k M_{k+1} = 6\,(s_k - s_{k-1})$$ + +Each row couples a knot to its two neighbours only, so the system is tridiagonal with $K-2$ rows. Two end conditions supply the missing two. + +### End conditions + +| Condition | First row | Last row | +|-----------|-----------|----------| +| **Clamped** (end slopes $\sigma_0$, $\sigma_1$) | $2h_0 M_0 + h_0 M_1 = 6\,(s_0 - \sigma_0)$ | $h_{K-2} M_{K-2} + 2h_{K-2} M_{K-1} = 6\,(\sigma_1 - s_{K-2})$ | +| **Natural** | $M_0 = 0$ | $M_{K-1} = 0$ | + +Clamping fixes the end slopes and leaves the end second derivatives free; natural ends do the reverse. Default slopes are zero. + +### Solvability + +Every interior row has diagonal $2(h_{k-1} + h_k)$ against off-diagonal sum $h_{k-1} + h_k$; clamped end rows have $2h$ against $h$; natural rows have $1$ against $0$. All rows are strictly diagonally dominant, so the matrix is nonsingular and the [Thomas algorithm](TridiagonalSolver.md) is stable without pivoting. The matrix contains only spans and the end-condition kind, never values or slopes: one factorization serves all channels, and only the right-hand sides differ. + +### Evaluation + +Clamp $x$ to $[x_0, x_{K-1}]$, find the interval by binary search, then use Horner's scheme per channel: + +$$y = y_k + \tau\,\bigl(\beta_k + \tau\,(\tfrac{M_k}{2} + \tau\,d_k)\bigr), \quad y' = \beta_k + \tau\,(M_k + 3 d_k \tau), \quad y'' = M_k + 6 d_k \tau$$ + +An abscissa exactly on an interior knot selects the interval starting there; the last knot selects the final interval. Both choices give the same result because the spline is $C^2$. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Factorization | $O(K)$ | three bands | Once per fit, shared by all channels | +| Fit | $O(D\,K)$ | $K_{\max}(2 + 2D)$ scalars | $D$ channels, each one right-hand side build and one solve of $O(K)$; no iteration | +| Evaluate | $O(\log K + D)$ | $O(1)$ | Binary search, then three Horner polynomials per channel | + +Storage holds the knots, their reciprocal spans, the values and the second derivatives: $K_{\max}(2 + 2D)$ scalars, where $K_{\max}$ is the compile-time capacity, plus the three bands of the solver. All of it is bounded and on the stack or inside the object; no heap. + +## Step-by-Step Walkthrough + +Knots $(0, 1, 2)$, values $(0, 1, 0)$, so $h_0 = h_1 = 1$ and $s_0 = 1$, $s_1 = -1$. + +**Clamped, zero end slopes.** The system is +$$\begin{pmatrix} 2 & 1 & 0 \\ 1 & 4 & 1 \\ 0 & 1 & 2 \end{pmatrix} M = \begin{pmatrix} 6 \\ -12 \\ 6 \end{pmatrix}$$ + +| $k$ | Pivot $p_k$ | $c'_k$ | Forward $y_k$ | Back $M_k$ | +|-----|-------------|--------|---------------|------------| +| 0 | $2$ | $1/2$ | $3$ | $3 + 3 = 6$ | +| 1 | $4 - 1/2 = 7/2$ | $2/7$ | $-30/7$ | $-30/7 - 12/7 = -6$ | +| 2 | $2 - 2/7 = 12/7$ | unused | $6$ | $6$ | + +So $M = (6, -6, 6)$. Interval 0 has $\beta_0 = 1 - (12 - 6)/6 = 0$ and $d_0 = -2$, giving $y(\tau) = 3\tau^2 - 2\tau^3$. Hence $y(0.5) = 0.75 - 0.25 = 0.5$, $y'(0.5) = 3 - 1.5 = 1.5$ and $y''(0.5) = 0$. The curve leaves and enters every knot flat, like a smooth step. + +**Natural.** The end rows reduce to $M_0 = M_2 = 0$, so the middle row is $4M_1 = -12$ and $M = (0, -3, 0)$. Interval 0 has $\beta_0 = 1 + 3/6 = 1.5$ and $d_0 = -0.5$, giving $y(\tau) = 1.5\tau - 0.5\tau^3$. Hence $y(0.5) = 0.75 - 0.0625 = 0.6875$ and $y'(0.5) = 1.125$. The end slope $y'(0) = 1.5$ is free, and the curvature is zero there. + +### Verification + +Random fits are checked for interpolation of every knot, for continuity of first and second derivative across each interior knot, and for the end conditions (prescribed slopes when clamped, zero curvature when natural). Continuity holds to about $10^{-13}$ in double precision and to the single-precision rounding level in float. + +## Pitfalls & Edge Cases + +- **Invalid knots.** Fewer than 2 knots, more than the capacity, or spacing $x_{k+1} - x_k \le \varepsilon \max(1, |x_k|)$ is rejected, where $\varepsilon$ is the machine epsilon of the scalar type. Near-coincident knots would make slope quotients and the second derivatives huge. +- **Failure leaves no fit.** A rejected or unfactorable fit discards any earlier fit; evaluating before a successful fit is a precondition violation. +- **Tolerance is a floor, not a guarantee.** Spacing only a few $\varepsilon$ wide passes the test yet produces enormous slopes; keep sample spacing meaningful relative to the data. +- **Overshoot.** A cubic spline interpolates; it does not bound the curve between knots, so limits (velocity, position) must be checked on the evaluated curve. +- **Outside the domain.** The abscissa is clamped, so the output is the end-knot sample, including its slope and curvature, which need not be zero. Position holds while the reported velocity does not drop to zero. +- **Pick the end condition per application.** Clamped when end velocities matter (start and stop at rest); natural when they do not, accepting free end slopes. +- **Degenerate cases.** Two clamped knots give exactly the cubic Hermite polynomial. Two natural knots give the straight line. Collinear data with a matching clamped slope give a zero right-hand side and so $M \equiv 0$: the line is reproduced exactly. +- **Floating-point only.** The reciprocal spans and the epsilon test have no meaning for integer types. + +## Variants & Generalizations + +- **Not-a-knot / periodic ends.** Other end rows keep the tridiagonal shape (not-a-knot) or add corner entries solved by Sherman–Morrison (periodic). +- **Shape-preserving cubics.** Monotone Hermite interpolation chooses slopes locally; it removes overshoot but gives up $C^2$ for $C^1$. +- **B-spline form.** Same curve family with local basis functions; suited to non-interpolating fits and variable degree. +- **Smoothing and least-squares splines.** Trade exact interpolation for noise rejection; see polynomial fitting for the non-interpolating alternative. + +## Applications + +- **Joint-space and Cartesian trajectories** through waypoints, with continuous velocity and acceleration. +- **CNC and planner paths** parameterized by time or arc length. +- **Calibration tables and resampling** of sensor signals onto a new grid. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| [Tridiagonal Solver](TridiagonalSolver.md) | Solves the second-derivative system; its factorization is reused across channels | +| [Polynomial Profile](../motion_profiles/PolynomialProfile.md) | The two-knot clamped case is the same cubic Hermite polynomial; the spline generalizes it to many knots with $C^2$ joins | +| Polynomial fitting | Least-squares fit instead of interpolation | + +## References & Further Reading + +- de Boor, C., *A Practical Guide to Splines*, Springer, 1978 (rev. ed. 2001). +- Burden, R. L. & Faires, J. D., *Numerical Analysis*, §3.5 (cubic spline interpolation). diff --git a/doc/math/README.md b/doc/math/README.md index a583ccce..c088a1f0 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -4,18 +4,19 @@ Core mathematical primitives for numerical computation. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------| -| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | -| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | -| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | -| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | -| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | -| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | -| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | -| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | -| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | -| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | -| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | -| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | -| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | +| Algorithm | Description | +|-----------------------------|-----------------------------------------------------------------------------------------------| +| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | +| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | +| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | +| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | +| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | +| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | +| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | +| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | +| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | +| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | +| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | +| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | +| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index a3afc00b..388487f3 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -21,6 +21,7 @@ target_sources(numerical.math PRIVATE ConstexprMath.hpp ComplexNumber.hpp ConsistencyMetrics.hpp + CubicSpline.hpp Cordic.hpp Geometry3D.hpp GivensRotation.hpp @@ -48,6 +49,7 @@ numerical_add_coverage_sources(numerical.math CholeskyDecomposition.cpp ComplexNumber.cpp ConsistencyMetrics.cpp + CubicSpline.cpp Cordic.cpp Matrix.cpp MatrixExponential.cpp diff --git a/numerical/math/CubicSpline.cpp b/numerical/math/CubicSpline.cpp new file mode 100644 index 00000000..ef34073d --- /dev/null +++ b/numerical/math/CubicSpline.cpp @@ -0,0 +1,6 @@ +#include "numerical/math/CubicSpline.hpp" + +namespace math +{ + template class CubicSpline; +} diff --git a/numerical/math/CubicSpline.hpp b/numerical/math/CubicSpline.hpp new file mode 100644 index 00000000..e7477d9f --- /dev/null +++ b/numerical/math/CubicSpline.hpp @@ -0,0 +1,223 @@ +#pragma once + +#include "infra/util/ReallyAssert.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/TridiagonalSolver.hpp" +#include +#include +#include +#include +#include +#include + +namespace math +{ + enum class SplineEnd : std::uint8_t + { + Clamped, + Natural + }; + + template + struct SplineBoundary + { + static_assert(std::is_floating_point_v, "SplineBoundary supports floating-point types only"); + + SplineEnd condition{ SplineEnd::Clamped }; + Vector startSlope{}; + Vector endSlope{}; + }; + + template + struct SplinePoint + { + static_assert(std::is_floating_point_v, "SplinePoint supports floating-point types only"); + + Vector value{}; + Vector firstDerivative{}; + Vector secondDerivative{}; + }; + + template + class CubicSpline + { + static_assert(std::is_floating_point_v, "CubicSpline supports floating-point types only"); + static_assert(MaxKnots >= 2, "CubicSpline requires MaxKnots >= 2"); + + public: + using Value = Vector; + using Knots = std::array; + using Values = std::array; + + bool Fit(const Knots& x, const Values& y, std::size_t n, const SplineBoundary& boundary = {}); + OPTIMIZE_FOR_SPEED SplinePoint Evaluate(T x) const; + T First() const; + T Last() const; + + private: + using Band = typename TridiagonalSolver::Band; + + static bool IsStrictlyIncreasing(const Knots& x, std::size_t n); + bool FactorSystem(TridiagonalSolver& solver, SplineEnd condition) const; + Band RightHandSide(std::size_t channel, const SplineBoundary& boundary) const; + std::size_t Interval(T x) const; + + Knots knots{}; + Values values{}; + Values secondDerivatives{}; + Knots inverseSpan{}; + std::size_t count{ 0 }; + }; + + template + bool CubicSpline::Fit(const Knots& x, const Values& y, std::size_t n, const SplineBoundary& boundary) + { + count = 0; + if (n < 2 || n > MaxKnots || !IsStrictlyIncreasing(x, n)) + return false; + + knots = x; + values = y; + for (std::size_t k = 0; k + 1 < n; ++k) + inverseSpan[k] = T{ 1 } / (x[k + 1] - x[k]); + count = n; + + TridiagonalSolver solver; + if (!FactorSystem(solver, boundary.condition)) + { + count = 0; + return false; + } + + for (std::size_t j = 0; j < Dimension; ++j) + { + const Band m{ solver.Solve(RightHandSide(j, boundary)) }; + for (std::size_t k = 0; k < count; ++k) + secondDerivatives[k][j] = m[k]; + } + + return true; + } + + template + OPTIMIZE_FOR_SPEED SplinePoint CubicSpline::Evaluate(T x) const + { + really_assert(count >= 2); + + x = std::min(std::max(x, knots[0]), knots[count - 1]); + const std::size_t k{ Interval(x) }; + const T tau{ x - knots[k] }; + const T h{ knots[k + 1] - knots[k] }; + const T invH{ inverseSpan[k] }; + + SplinePoint point{}; + for (std::size_t j = 0; j < Dimension; ++j) + { + const T m0{ secondDerivatives[k][j] }; + const T m1{ secondDerivatives[k + 1][j] }; + const T d{ (m1 - m0) * invH / T{ 6 } }; + const T beta{ (values[k + 1][j] - values[k][j]) * invH - h * (T{ 2 } * m0 + m1) / T{ 6 } }; + + point.value[j] = values[k][j] + tau * (beta + tau * (m0 / T{ 2 } + tau * d)); + point.firstDerivative[j] = beta + tau * (m0 + T{ 3 } * d * tau); + point.secondDerivative[j] = m0 + T{ 6 } * d * tau; + } + + return point; + } + + template + T CubicSpline::First() const + { + really_assert(count >= 2); + return knots[0]; + } + + template + T CubicSpline::Last() const + { + really_assert(count >= 2); + return knots[count - 1]; + } + + template + bool CubicSpline::IsStrictlyIncreasing(const Knots& x, std::size_t n) + { + for (std::size_t k = 0; k + 1 < n; ++k) + if (x[k + 1] - x[k] <= std::numeric_limits::epsilon() * std::max(T{ 1 }, math::Abs(x[k]))) + return false; + + return true; + } + + template + bool CubicSpline::FactorSystem(TridiagonalSolver& solver, SplineEnd condition) const + { + Band sub{}; + Band diag{}; + Band sup{}; + const std::size_t last{ count - 1 }; + + for (std::size_t k = 1; k < last; ++k) + { + const T before{ knots[k] - knots[k - 1] }; + const T after{ knots[k + 1] - knots[k] }; + sub[k] = before; + diag[k] = T{ 2 } * (before + after); + sup[k] = after; + } + + if (condition == SplineEnd::Natural) + { + diag[0] = T{ 1 }; + diag[last] = T{ 1 }; + } + else + { + const T firstSpan{ knots[1] - knots[0] }; + const T lastSpan{ knots[last] - knots[last - 1] }; + diag[0] = T{ 2 } * firstSpan; + sup[0] = firstSpan; + sub[last] = lastSpan; + diag[last] = T{ 2 } * lastSpan; + } + + return solver.Factor(sub, diag, sup, count); + } + + template + auto CubicSpline::RightHandSide(std::size_t channel, const SplineBoundary& boundary) const -> Band + { + Band rhs{}; + const std::size_t last{ count - 1 }; + const auto slope{ [this, channel](std::size_t k) + { + return (values[k + 1][channel] - values[k][channel]) * inverseSpan[k]; + } }; + + for (std::size_t k = 1; k < last; ++k) + rhs[k] = T{ 6 } * (slope(k) - slope(k - 1)); + + if (boundary.condition == SplineEnd::Clamped) + { + rhs[0] = T{ 6 } * (slope(0) - boundary.startSlope[channel]); + rhs[last] = T{ 6 } * (boundary.endSlope[channel] - slope(last - 1)); + } + + return rhs; + } + + template + std::size_t CubicSpline::Interval(T x) const + { + const auto upper{ std::upper_bound(knots.begin() + 1, knots.begin() + count, x) }; + const auto index{ static_cast(upper - knots.begin()) - 1 }; + return std::min(index, count - 2); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class CubicSpline; +#endif +} diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index f635a61c..d0e0e44b 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -17,6 +17,7 @@ target_sources(numerical.math_test PRIVATE TestGradientCheck.cpp TestMath.cpp TestConsistencyMetrics.cpp + TestCubicSpline.cpp TestCordic.cpp TestGivensRotation.cpp TestHouseholderTransform.cpp diff --git a/numerical/math/test/TestCubicSpline.cpp b/numerical/math/test/TestCubicSpline.cpp new file mode 100644 index 00000000..636f03c8 --- /dev/null +++ b/numerical/math/test/TestCubicSpline.cpp @@ -0,0 +1,143 @@ +#include "numerical/math/CubicSpline.hpp" +#include +#include +#include + +namespace +{ + using Spline = math::CubicSpline; + using Value = Spline::Value; + using Boundary = math::SplineBoundary; + + class TestCubicSpline : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-4f }; + Spline::Knots knots{ 0.0f, 1.0f, 2.5f, 3.0f, 4.5f }; + Spline::Values values{ Value{ 0.0f, 0.5f, 0.0f }, Value{ 1.0f, 0.2f, 0.0f }, Value{ -0.5f, 0.2f, 0.0f }, + Value{ 0.3f, -0.4f, 0.0f }, Value{ 2.0f, 0.0f, 0.0f } }; + Spline spline; + + void SetUp() override + { + ASSERT_TRUE(spline.Fit(knots, values, 5)); + } + + static void ExpectVectorNear(const Value& actual, const Value& expected, float eps) + { + for (std::size_t j = 0; j < 3; ++j) + EXPECT_NEAR(actual[j], expected[j], eps) << "channel " << j; + } + }; +} + +TEST_F(TestCubicSpline, passes_through_every_knot) +{ + for (std::size_t k = 0; k < 5; ++k) + ExpectVectorNear(spline.Evaluate(knots[k]).value, values[k], 1e-5f); +} + +TEST_F(TestCubicSpline, first_and_second_derivatives_continuous_at_interior_knots) +{ + constexpr float delta{ 1e-4f }; + + for (std::size_t k = 1; k < 4; ++k) + { + const auto before{ spline.Evaluate(knots[k] - delta) }; + const auto after{ spline.Evaluate(knots[k] + delta) }; + + ExpectVectorNear(before.firstDerivative, after.firstDerivative, 1e-2f); + ExpectVectorNear(before.secondDerivative, after.secondDerivative, 1e-2f); + } +} + +TEST_F(TestCubicSpline, knot_second_derivatives_match_reference) +{ + const std::array channel0{ 5.665169f, -5.330337f, 5.991011f, -0.737079f, -1.898127f }; + const std::array channel1{ -1.666292f, 1.532584f, -2.797753f, 3.384270f, -2.225468f }; + + for (std::size_t k = 0; k < 5; ++k) + { + const auto secondDerivative{ spline.Evaluate(knots[k]).secondDerivative }; + + EXPECT_NEAR(secondDerivative[0], channel0[k], tolerance); + EXPECT_NEAR(secondDerivative[1], channel1[k], tolerance); + EXPECT_EQ(secondDerivative[2], 0.0f); + } +} + +TEST_F(TestCubicSpline, clamped_end_slopes_are_honoured) +{ + ASSERT_TRUE(spline.Fit(knots, values, 5, Boundary{ math::SplineEnd::Clamped, Value{ 0.4f, 0.0f, 0.0f }, Value{ -0.7f, 0.0f, 0.0f } })); + + EXPECT_NEAR(spline.Evaluate(0.0f).firstDerivative[0], 0.4f, tolerance); + EXPECT_NEAR(spline.Evaluate(4.5f).firstDerivative[0], -0.7f, tolerance); + EXPECT_NEAR(spline.Evaluate(0.0f).secondDerivative[0], 4.296629f, tolerance); + EXPECT_NEAR(spline.Evaluate(4.5f).secondDerivative[0], -3.637453f, tolerance); +} + +TEST_F(TestCubicSpline, natural_ends_have_zero_second_derivative) +{ + ASSERT_TRUE(spline.Fit(knots, values, 5, Boundary{ math::SplineEnd::Natural })); + + ExpectVectorNear(spline.Evaluate(0.0f).secondDerivative, Value{}, tolerance); + ExpectVectorNear(spline.Evaluate(4.5f).secondDerivative, Value{}, tolerance); + EXPECT_NEAR(spline.Evaluate(0.0f).firstDerivative[0], 1.680287f, tolerance); + EXPECT_NEAR(spline.Evaluate(4.5f).firstDerivative[0], 0.783154f, tolerance); +} + +TEST_F(TestCubicSpline, collinear_knots_reproduce_straight_line) +{ + const Spline::Knots line{ 0.0f, 0.5f, 1.0f, 1.5f, 2.0f }; + Spline::Values onLine{}; + for (std::size_t k = 0; k < 5; ++k) + onLine[k] = Value{ 1.0f + 0.8f * line[k], 1.0f + 0.8f * line[k], 1.0f + 0.8f * line[k] }; + const Value slope{ 0.8f, 0.8f, 0.8f }; + + ASSERT_TRUE(spline.Fit(line, onLine, 5, Boundary{ math::SplineEnd::Clamped, slope, slope })); + + const auto point{ spline.Evaluate(0.7f) }; + ExpectVectorNear(point.value, Value{ 1.56f, 1.56f, 1.56f }, 1e-5f); + ExpectVectorNear(point.firstDerivative, slope, 1e-5f); + for (float x : { 0.0f, 0.3f, 0.7f, 1.2f, 2.0f }) + ExpectVectorNear(spline.Evaluate(x).secondDerivative, Value{}, 1e-5f); +} + +TEST_F(TestCubicSpline, three_knot_reference_values) +{ + ASSERT_TRUE(spline.Fit(Spline::Knots{ 0.0f, 1.0f, 2.0f }, Spline::Values{ Value{}, Value{ 1.0f, 0.0f, 0.0f }, Value{} }, 3)); + + const auto half{ spline.Evaluate(0.5f) }; + EXPECT_NEAR(half.value[0], 0.5f, 1e-5f); + EXPECT_NEAR(half.firstDerivative[0], 1.5f, 1e-5f); + EXPECT_NEAR(half.secondDerivative[0], 0.0f, 1e-5f); + EXPECT_NEAR(spline.Evaluate(0.0f).secondDerivative[0], 6.0f, 1e-5f); + EXPECT_NEAR(spline.Evaluate(1.0f).secondDerivative[0], -6.0f, 1e-5f); + EXPECT_NEAR(spline.Evaluate(2.0f).secondDerivative[0], 6.0f, 1e-5f); + EXPECT_NEAR(spline.Evaluate(1.0f).firstDerivative[0], 0.0f, 1e-5f); +} + +TEST_F(TestCubicSpline, evaluate_clamps_outside_domain) +{ + const auto below{ spline.Evaluate(-1.0f) }; + const auto start{ spline.Evaluate(0.0f) }; + const auto above{ spline.Evaluate(5.5f) }; + const auto end{ spline.Evaluate(4.5f) }; + + EXPECT_EQ(spline.First(), 0.0f); + EXPECT_EQ(spline.Last(), 4.5f); + for (std::size_t j = 0; j < 3; ++j) + { + EXPECT_EQ(below.value[j], start.value[j]); + EXPECT_EQ(below.firstDerivative[j], start.firstDerivative[j]); + EXPECT_EQ(above.value[j], end.value[j]); + EXPECT_EQ(above.secondDerivative[j], end.secondDerivative[j]); + } +} + +TEST_F(TestCubicSpline, fit_rejects_invalid_knots) +{ + EXPECT_FALSE(spline.Fit(knots, values, 1)); + EXPECT_FALSE(spline.Fit(knots, values, 9)); + EXPECT_FALSE(spline.Fit(Spline::Knots{ 0.0f, 1.0f, 1.0f, 2.0f }, values, 4)); +} diff --git a/roadmap/README.md b/roadmap/README.md index ab332c7c..176a2919 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -102,7 +102,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) · `CubicSpline` (61) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) ### `optimization` `LevenbergMarquardt` (64) diff --git a/roadmap/math/CubicSpline/explanation.md b/roadmap/math/CubicSpline/explanation.md deleted file mode 100644 index ec463731..00000000 --- a/roadmap/math/CubicSpline/explanation.md +++ /dev/null @@ -1,36 +0,0 @@ -# Cubic Spline Interpolation — Overview - -## What it is -A smooth curve through a list of points: between consecutive knots each channel is a cubic -polynomial, and the pieces join so value, slope **and** curvature are continuous (C²). The ends are -either **clamped** (prescribed slopes) or **natural** (zero curvature). Several channels that share the -same knots are fitted together. - -## Why it matters (embedded) -Waypoint lists (robot teach-in programs, CNC paths, planner output), calibration tables and resampled -signals all need a smooth interpolant that does not ring like a high-order polynomial. Fitting is one -linear-time pass on fixed storage; each evaluation is a binary search plus a cubic — cheap and -deterministic. - -## How it works (intuition) -The unknowns are the curvatures at the knots. Once known, each segment is fixed: it must hit both knot -values and match both knot curvatures. Requiring neighbouring segments to have the same slope at every -interior knot gives one equation per knot that involves only the knot and its two neighbours — a -**tridiagonal** system. The end conditions add the first and last equations. The system is diagonally -dominant, so the Thomas algorithm solves it in linear time without pivoting, and its elimination -factors depend only on the knot spacing, so every channel reuses them. - -## Key parameters -- **Knots `x_k`** — strictly increasing abscissae (times, arc length, sensor readings). -- **Values `y_k`** — one vector per knot. -- **End condition** — clamped (end slopes, default zero) or natural (zero end curvature). -- **`MaxKnots`** — compile-time capacity of the bounded storage. - -## Reference -C. de Boor, *A Practical Guide to Splines*, rev. ed. (2001), Ch. IV; L. Biagiotti, C. Melchiorri, -*Trajectory Planning for Automatic Machines and Robots* (2008), Ch. 4. - -## See also -`TridiagonalSolver` (#59, the solve), `PolynomialProfile` (#55, the two-knot special case), -`PolynomialFitting` (least-squares fit rather than interpolation); robotics-toolbox-cpp's joint-space -spline trajectory, a thin wrapper over this interpolant. diff --git a/roadmap/math/CubicSpline/implementation.md b/roadmap/math/CubicSpline/implementation.md deleted file mode 100644 index 0ff0055b..00000000 --- a/roadmap/math/CubicSpline/implementation.md +++ /dev/null @@ -1,116 +0,0 @@ -# Cubic Spline Interpolation (vector-valued, clamped / natural) — Implementation Pseudocode - -> Roadmap ref: #61 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -C² piecewise-cubic interpolant through `K` knots `(x_k, y_k)`, with `y_k` a `Dimension`-vector so several -channels that share the abscissa (the joints of a robot, the axes of a CNC path, the channels of a -calibration table) are fitted in one pass. Ends are **clamped** (prescribed slopes) or **natural** (zero -second derivative). Fitting is one tridiagonal solve (#59) whose factorization depends only on the knot -spacing and is shared by all channels. - -## Data structures - -```cpp -enum class SplineEnd : uint8_t { Clamped, Natural } - -template # static_assert(std::is_floating_point_v) -struct SplineBoundary: - SplineEnd condition = SplineEnd::Clamped - math::Vector startSlope{}, endSlope{} # Clamped only; default zero - -template -struct SplinePoint: - math::Vector value, firstDerivative, secondDerivative - -template # static_assert(MaxKnots >= 2); instantiated for float -class CubicSpline: - using Value = math::Vector - std::array knots # x_0 < x_1 < … < x_{K−1} - std::array values # y_k - std::array secondDerivatives # M_k = y''(x_k), solved by Fit - std::array inverseSpan # 1/h_k, h_k = x_{k+1} − x_k - std::size_t count # K, 2 ≤ K ≤ MaxKnots -``` - -## Interface - -```cpp -bool Fit(const std::array& knots, const std::array& values, - std::size_t count, const SplineBoundary& boundary = {}) - # false if count ∉ [2, MaxKnots] or knots not strictly increasing -SplinePoint Evaluate(T x) const # hot path; x clamped to [x_0, x_{K−1}]; precondition: Fit() == true -T First() const; T Last() const # x_0, x_{K−1} -``` - -## Algorithm (pseudocode) - -```text -# Per channel j, unknowns M_k = y''(x_k). On interval k (τ = x − x_k ∈ [0, h_k]): -# y(τ) = y_k + β_k·τ + (M_k/2)·τ² + d_k·τ³ -# β_k = (y_{k+1} − y_k)/h_k − h_k·(2M_k + M_{k+1})/6 # slope at x_k -# d_k = (M_{k+1} − M_k)/(6h_k) -# Value interpolates the knots and the second derivative is shared at knots ⇒ C⁰ and C² by construction. -# Slope continuity at interior knots k = 1..K−2 gives the tridiagonal rows -# h_{k−1}·M_{k−1} + 2(h_{k−1} + h_k)·M_k + h_k·M_{k+1} = 6·[(y_{k+1} − y_k)/h_k − (y_k − y_{k−1})/h_{k−1}] -# End rows: -# Clamped: 2h_0·M_0 + h_0·M_1 = 6·[(y_1 − y_0)/h_0 − s_start] -# h_{K−2}·M_{K−2} + 2h_{K−2}·M_{K−1} = 6·[s_end − (y_{K−1} − y_{K−2})/h_{K−2}] -# Natural: M_0 = 0, M_{K−1} = 0 -# Every row is diagonally dominant (|diag| = 2·Σ|off-diag|, or 1 vs 0) ⇒ Thomas (#59), no pivoting. - -function Fit(x, y, count, boundary): - if count < 2 or count > MaxKnots: return false - for k in 0..count−2: if x[k+1] − x[k] ≤ ε·max(1, |x[k]|): return false - store knots, values, count; inverseSpan[k] = 1/h_k - build sub, diag, sup from h and boundary.condition # stack bands of MaxKnots - if !tridiagonal.Factor(sub, diag, sup, count): return false # math::TridiagonalSolver - for j in 0..Dimension−1: # O(K) per channel - r = right-hand side of channel j (rows above; start/end slope_j when Clamped, 0 when Natural) - M[·][j] = tridiagonal.Solve(r) - return true - -function Evaluate(x): # OPTIMIZE_FOR_SPEED - x = clamp(x, x_0, x_{K−1}) - k = (upper_bound(knots[1 .. K−1], x) index) clamped to [0, K−2] # binary search, O(log K) - τ = x − x_k; h = x_{k+1} − x_k; invH = inverseSpan[k] - for j in 0..Dimension−1: - d = (M_{k+1,j} − M_{k,j})·invH/6 - β = (y_{k+1,j} − y_{k,j})·invH − h·(2M_{k,j} + M_{k+1,j})/6 - value_j = y_{k,j} + τ·(β + τ·(M_{k,j}/2 + τ·d)) - firstDerivative_j = β + τ·(M_{k,j} + 3d·τ) - secondDerivative_j = M_{k,j} + 6d·τ - return { value, firstDerivative, secondDerivative } -``` - -Verified with python3: knots reproduced exactly, one-sided first/second derivatives at interior knots -agree to `2e-15`, clamped end slopes exact, natural end second derivatives zero, collinear knots ⇒ -`M ≡ 0`, two clamped knots ⇒ exactly the cubic Hermite polynomial (`PolynomialProfile`, #55). - -## Complexity & memory - -- `Fit`: `O(K)` shared factorization + `O(K)` per channel ⇒ `O(Dimension·K)`; no pivoting, no iteration. -- `Evaluate`: `O(log K)` interval search + `O(Dimension)` Horner evaluations. -- Memory: `MaxKnots·(2 + 2·Dimension)` scalars in bounded `std::array`s plus the solver's three bands. - No heap. - -## Numerical / embedded notes - -- Fit once, then `Evaluate` is deterministic: one binary search plus fixed arithmetic. -- Reject `h_k ≤ ε`: near-coincident knots make the rows ill-conditioned and `M` huge. -- Clamped ends fix the end slopes but leave the end second derivatives free (generally non-zero); natural - ends zero the second derivatives but leave the slopes free — pick per application. -- A cubic spline interpolates; it does not bound the curve between knots (overshoot is possible). Check - with `Evaluate` where limits matter. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/math/CubicSpline.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Evaluate`, and - `extern template class CubicSpline;` under `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/CubicSpline.cpp` → `template class CubicSpline;` -- Test: `numerical/math/test/TestCubicSpline.cpp` -- Doc: `doc/math/CubicSpline.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestCubicSpline.cpp` → `numerical.math_test`. -- Depends on: #59 (`TridiagonalSolver`). -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/CubicSpline/tests.md b/roadmap/math/CubicSpline/tests.md deleted file mode 100644 index 67625cde..00000000 --- a/roadmap/math/CubicSpline/tests.md +++ /dev/null @@ -1,76 +0,0 @@ -# Cubic Spline Interpolation — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestCubicSpline : public ::testing::Test: - using Spline = CubicSpline - std::array knots{ 0, 1, 2.5, 3, 4.5 } # K = 5 used - std::array, 8> values{ (0, 0.5, 0), (1, 0.2, 0), (−0.5, 0.2, 0), - (0.3, −0.4, 0), (2, 0, 0) } - Spline spline - void SetUp(): ASSERT_TRUE(spline.Fit(knots, values, 5)) # clamped, zero end slopes -# each case below is a TEST_F(TestCubicSpline, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -passes_through_every_knot: - Assert: Evaluate(x_k).value ≈ y_k for k = 0..4, every channel - -first_and_second_derivatives_continuous_at_interior_knots: - Assert: for k = 1..3: Evaluate(x_k − δ) ≈ Evaluate(x_k + δ) in first and second derivative - (δ = 1e-4, tol 1e-2 ≥ |y'''|·2δ), every channel - -knot_second_derivatives_match_reference: - Assert: channel 0: Evaluate(x_k).secondDerivative ≈ (5.665169, −5.330337, 5.991011, −0.737079, −1.898127); - channel 1: ≈ (−1.666292, 1.532584, −2.797753, 3.384270, −2.225468); - channel 2 (all knots 0) ≡ 0 - -clamped_end_slopes_are_honoured: - Arrange: Fit(..., { Clamped, startSlope = (0.4, 0, 0), endSlope = (−0.7, 0, 0) }) - Assert: Evaluate(0).firstDerivative_0 ≈ 0.4, Evaluate(4.5).firstDerivative_0 ≈ −0.7; - Evaluate(0).secondDerivative_0 ≈ 4.296629, Evaluate(4.5).secondDerivative_0 ≈ −3.637453 - -natural_ends_have_zero_second_derivative: - Arrange: Fit(..., { Natural }) - Assert: Evaluate(0).secondDerivative ≈ 0, Evaluate(4.5).secondDerivative ≈ 0; - channel 0 end slopes ≈ 1.680287 and 0.783154 - -collinear_knots_reproduce_straight_line: - Arrange: knots {0, 0.5, 1, 1.5, 2}, y_k = 1 + 0.8·x_k (all channels), Clamped with slope 0.8 - Assert: Evaluate(0.7) ≈ { 1.56, 0.8, 0 }; second derivative ≈ 0 everywhere sampled - -three_knot_reference_values: - Arrange: knots {0, 1, 2}, y {0, 1, 0}, clamped zero - Assert: Evaluate(0.5) ≈ { 0.5, 1.5, 0 }; knot second derivatives (6, −6, 6); Evaluate(1).firstDerivative ≈ 0 - -evaluate_clamps_outside_domain: - Assert: Evaluate(−1) == Evaluate(0) and Evaluate(5.5) == Evaluate(4.5) - -fit_rejects_invalid_knots: - Assert: Fit(count = 1) == false; Fit(count = 9) == false; knots {0, 1, 1, 2} == false -``` - -## Reference vectors - -Computed with python3 (tridiagonal system + Thomas solve, checked against the defining conditions). - -- Fixture channel 0 (`x = 0, 1, 2.5, 3, 4.5`; `y = 0, 1, −0.5, 0.3, 2`): - - clamped zero ⇒ `M = (5.665169, −5.330337, 5.991011, −0.737079, −1.898127)`; - - clamped `(0.4, −0.7)` ⇒ `M = (4.296629, −4.993258, 5.779775, −0.058427, −3.637453)`; - - natural ⇒ `M = (0, −4.081720, 5.605735, −1.400717, 0)`, end slopes `1.680287`, `0.783154`. -- Fixture channel 1 (`y = 0.5, 0.2, 0.2, −0.4, 0`), clamped zero ⇒ - `M = (−1.666292, 1.532584, −2.797753, 3.384270, −2.225468)`. -- Three knots `(0,0), (1,1), (2,0)`, clamped zero ⇒ `M = (6, −6, 6)`, `y(0.5) = 0.5`, `y'(0.5) = 1.5`; - natural ⇒ `M = (0, −3, 0)`, `y(0.5) = 0.6875`. -- Collinear knots with matching clamped slope ⇒ right-hand side ≡ 0 ⇒ `M ≡ 0`. - -## Edge cases - -- `K = 2`, Natural ⇒ `M = 0`: straight line (end slopes free). -- Unequal spacing (fixture) — the formulation never assumes uniform `h`. -- `K = MaxKnots` (8) ⇒ accepted; no out-of-bounds access in the binary search at `x = x_{K−1}`. From 9d1d1c6ffcb4a0726862ac720c4f7ec18d423335 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:32:14 +0000 Subject: [PATCH 2/6] feat(solvers): add damped pseudo-inverse and null-space projector MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 62. One SVD (tall or wide) gives the damped least-squares solve Σ σ/(σ² + λ²)(uᵢᵀb)vᵢ, which falls back to a thresholded undamped inverse for λ = 0, plus the exact orthogonal null-space projector I − Σ_{i Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 10 -- doc/solvers/DampedPseudoInverse.md | 121 ++++++++++++++ doc/solvers/README.md | 1 + numerical/solvers/CMakeLists.txt | 2 + numerical/solvers/DampedPseudoInverse.cpp | 7 + numerical/solvers/DampedPseudoInverse.hpp | 156 ++++++++++++++++++ numerical/solvers/test/CMakeLists.txt | 1 + .../solvers/test/TestDampedPseudoInverse.cpp | 94 +++++++++++ roadmap/README.md | 2 +- .../DampedPseudoInverse/explanation.md | 34 ---- .../DampedPseudoInverse/implementation.md | 86 ---------- roadmap/solvers/DampedPseudoInverse/tests.md | 48 ------ 13 files changed, 384 insertions(+), 180 deletions(-) create mode 100644 doc/solvers/DampedPseudoInverse.md create mode 100644 numerical/solvers/DampedPseudoInverse.cpp create mode 100644 numerical/solvers/DampedPseudoInverse.hpp create mode 100644 numerical/solvers/test/TestDampedPseudoInverse.cpp delete mode 100644 roadmap/solvers/DampedPseudoInverse/explanation.md delete mode 100644 roadmap/solvers/DampedPseudoInverse/implementation.md delete mode 100644 roadmap/solvers/DampedPseudoInverse/tests.md diff --git a/README.md b/README.md index dbc2d218..9908b0b8 100644 --- a/README.md +++ b/README.md @@ -25,7 +25,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [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 | | [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | -| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) | +| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet), Damped Pseudo-Inverse + Null-Space Projector | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | | [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | diff --git a/ROADMAP.md b/ROADMAP.md index 349759d4..0e5bee4d 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -27,7 +27,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | -| 62 | Damped pseudo-inverse + null-space projector | `solvers` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | | 63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | ★★★☆☆ | @@ -64,15 +63,6 @@ the library does not yet expose. Detailed below under - **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`. - **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md). -### 62. Damped pseudo-inverse and null-space projector -- **What:** `A⁺_λ·b` and the exact projector `I − A⁺A` for any `Rows × Cols` matrix from one SVD. -- **Embedded value:** Singularity-robust inversion for redundant robots, over-actuated control allocation - and regularized calibration; the projector exposes spare actuation to a secondary objective. -- **Algorithm / paper:** C. Wampler, *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*, - 7(12), 1977. -- **Reuses:** `SingularValueDecomposition` (tall and wide). -- **Spec:** [roadmap/solvers/DampedPseudoInverse](roadmap/solvers/DampedPseudoInverse/implementation.md). - ### 66. SE(2) transform - **What:** Planar rigid transform `(R(θ), p)` with composition, closed-form inverse, adjoint, exp/log of planar twists `(vx, vy, ω)` and a pose error, ordered linear part first like item 53. diff --git a/doc/solvers/DampedPseudoInverse.md b/doc/solvers/DampedPseudoInverse.md new file mode 100644 index 00000000..96949a4a --- /dev/null +++ b/doc/solvers/DampedPseudoInverse.md @@ -0,0 +1,121 @@ +# Damped Pseudo-Inverse and Null-Space Projector + +## Overview & Motivation + +Mapping desired outputs to inputs through a non-square or ill-conditioned matrix $A$ is everywhere: joint rates from a tool velocity, motor commands from a desired thrust and torque, calibration corrections from residuals. The plain pseudo-inverse divides by every singular value, so near rank loss it explodes and commands huge, saturating inputs. **Damping** replaces the division by a gain that equals $1/\sigma$ for strong directions and fades to zero for weak ones, trading a small, bounded accuracy loss for a bounded answer. + +When there are more inputs than outputs, $A$ has a null space: input motions it cannot feel. A **null-space projector** removes from any vector every component the matrix sees, so a secondary goal (stay away from limits, share load, save energy) can use the spare freedom without disturbing the primary one. Both tools come from one thin SVD, factored once per matrix. + +## Mathematical Theory + +### Factorization and Rank + +For $A \in \mathbb{R}^{m \times n}$, tall or wide, and $k = \min(m, n)$, the thin SVD is $A = U\Sigma V^\top$ with $\sigma_0 \geq \cdots \geq \sigma_{k-1} \geq 0$, left vectors $u_i \in \mathbb{R}^m$ and right vectors $v_i \in \mathbb{R}^n$. With a relative tolerance $\tau$ (default $10^{-4}$) the rank threshold and rank are + +$$\theta = \tau\,\sigma_0, \qquad r = \#\{\,i : \sigma_i > \theta\,\}.$$ + +Since the values are sorted, the counted directions are the leading $r$. The zero matrix has $\sigma_0 = 0$, hence $r = 0$. + +### Damped Gain + +Damped least squares (Tikhonov regularization) minimizes $\|Ax - b\|^2 + \lambda^2\|x\|^2$. Its normal equations give + +$$x = (A^\top A + \lambda^2 I)^{-1}A^\top b = A^\top (AA^\top + \lambda^2 I)^{-1} b,$$ + +and in the singular basis each direction is scaled by the gain + +$$g(\sigma) = \begin{cases} \dfrac{\sigma}{\sigma^2 + \lambda^2} & \lambda > 0, \\[2mm] 1/\sigma & \lambda = 0,\ \sigma > \theta, \\[1mm] 0 & \lambda = 0,\ \sigma \leq \theta. \end{cases}$$ + +For $\sigma \gg \lambda$ the gain is $\approx 1/\sigma$; for $\sigma \ll \lambda$ it is $\approx \sigma/\lambda^2 \to 0$. Its maximum is at $\sigma = \lambda$, so $g \leq 1/(2\lambda)$ always. Without damping the threshold zeroes weak directions, so no division by zero occurs. The solution and the damped inverse are + +$$x = \sum_{i=0}^{k-1} g(\sigma_i)\,(u_i^\top b)\,v_i, \qquad A^\dagger_\lambda = \sum_{i=0}^{k-1} g(\sigma_i)\,v_i u_i^\top.$$ + +Every term lies in the span of the $v_i$, the row space of $A$, so $x$ has no null-space component: for $\lambda = 0$ it is the minimum-norm solution. + +### Null-Space Projector + +$$P = I - \sum_{i 0$, so it is not idempotent either. The thresholded projector is exact: $P^2 = P$, $P^\top = P$, and + +$$AP = \sum_{i \geq r} \sigma_i\,u_i v_i^\top, \qquad \sigma_i \leq \theta,$$ + +which is zero for exact rank loss and at rounding level for full rank. + +### Damping Bias + +For $b \in \operatorname{range}(A)$ the residual is $b - Ax = \sum_i \frac{\lambda^2}{\sigma_i^2 + \lambda^2}(u_i^\top b)\,u_i$, hence + +$$\|Ax - b\| \leq \frac{\lambda^2}{\sigma_{\min}^2 + \lambda^2}\,\|b\| \leq \frac{\lambda^2}{\sigma_{\min}^2}\,\|b\|,$$ + +with $\sigma_{\min}$ the smallest nonzero singular value. Robustness costs accuracy of order $\lambda^2/\sigma_{\min}^2$. Near rank loss the threshold grows the null space: the lost direction is released to the projector by design, while the damped solution stays bounded. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Factor | $O(Mk^2)$ | $O(mk + nk)$ | One Golub–Kahan SVD; wide matrices through their transpose | +| Solve | $O((m + n)\,k)$ | $O(n)$ | $k$ inner products of length $m$, $k$ updates of length $n$; no division | +| Project | $O(n\,r)$ | $O(n)$ | $r$ inner products and updates; $P$ never formed | +| Explicit inverse, projector | $O(mnk)$, $O(n^2 r)$ | $O(mn)$, $O(n^2)$ | Diagnostics; sums of rank-one terms | + +Here $M = \max(m, n)$. The SVD factors are the only persistent storage: $U$ is $m \times k$, $V$ is $n \times k$, plus $k$ singular values. No heap is used, and the factorization is paid once per matrix; later solves and projections are inner products only. + +## Step-by-Step Walkthrough + +**Input:** $A = \begin{pmatrix} 1 & 0 & 1 \\ 0 & 1 & 1 \end{pmatrix}$, $b = (1, 2)$, $\tau = 10^{-4}$. + +**Factor.** $AA^\top = \begin{pmatrix} 2 & 1 \\ 1 & 2 \end{pmatrix}$ has eigenvalues $3$ and $1$, so $\sigma = (\sqrt{3}, 1)$. Then $u_0 = (1, 1)/\sqrt{2}$, $u_1 = (1, -1)/\sqrt{2}$, $v_0 = (1, 1, 2)/\sqrt{6}$, $v_1 = (1, -1, 0)/\sqrt{2}$ (signs are arbitrary). The threshold is $\theta \approx 1.7 \times 10^{-4}$, so $r = 2$; the null space is spanned by $(1, 1, -1)$. + +**Gains and projections** at $\lambda = 0.1$: + +| $i$ | $\sigma_i$ | $u_i^\top b$ | $1/\sigma_i$ | $g(\sigma_i)$ | +|-----|-----------|--------------|--------------|---------------| +| 0 | $1.732051$ | $2.121320$ | $0.577350$ | $0.575432$ | +| 1 | $1$ | $-0.707107$ | $1$ | $0.990099$ | + +**Solution.** Without damping, $x = A^\dagger b = (0, 1, 1)$: it satisfies $Ax = b$ and is orthogonal to the null space ($0 + 1 - 1 = 0$). With $\lambda = 0.1$, $x = (0.003289, 0.993388, 0.996678)$. The residual is $b - Ax = \lambda^2(AA^\top + \lambda^2 I)^{-1}b$, of norm $0.009934$, within the bound $0.022139$ (the looser $\lambda^2\|b\|/\sigma_{\min}^2$ gives $0.022361$). + +**Projector.** With both singular directions counted, + +$$P = I - v_0 v_0^\top - v_1 v_1^\top = \tfrac{1}{3}\begin{pmatrix} 1 & 1 & -1 \\ 1 & 1 & -1 \\ -1 & -1 & 1 \end{pmatrix}, \qquad \operatorname{tr} P = 1 = n - r.$$ + +Projecting $(1, 0, 0)$ gives $(1/3, 1/3, -1/3)$, which $A$ maps to $0$. Projecting the damped solution gives $\approx 0$, since it lies in the row space. + +**Rank loss and check.** For $A = \begin{pmatrix} 1 & 1 & 0 \\ 2 & 2 & 0 \end{pmatrix}$ the singular values are $(\sqrt{10}, 0)$: $r = 1$ and $\operatorname{tr} P = 2$. Undamped, the zero direction gets zero gain, and $A^\dagger(1, 2) = (0.5, 0.5, 0)$. The results were compared with the closed forms $(A^\top A + \lambda^2 I)^{-1}A^\top b$ (tall) and $A^\top (AA^\top + \lambda^2 I)^{-1} b$ (wide), evaluated in double precision, on random $2 \times 3$, $6 \times 7$, $7 \times 6$ and $4 \times 4$ matrices: solutions agree to about $10^{-5}$ and $\|P^2 - P\|$ is about $10^{-6}$. + +## Pitfalls & Edge Cases + +- **Choice of $\lambda$:** too small leaves large gains (up to $1/(2\lambda)$) near singularities; too large biases every direction by about $\lambda^2/\sigma^2$. Choose it from the smallest singular value that must still be tracked. +- **Hard rank decision:** the rank and the projector jump when a singular value crosses $\theta$, so secondary motion can switch abruptly near rank loss; smoothing or hysteresis belongs to the caller. The damped solution also keeps a small component $\sigma/(\sigma^2 + \lambda^2)\,(u^\top b)$ along directions already released to the projector, which the projection does not remove. +- **Degenerate shapes:** a zero matrix gives rank $0$, a zero solution and a projection equal to the identity; undamped, a zero singular value has zero gain, never a division. A tall full-rank matrix has a zero projector and an undamped least-squares solution. +- **Precision and failure:** $\tau$ must exceed the rounding level of $\sigma_0$ (about $10^{-7}$ in single precision), or noise is counted as rank. If the SVD does not converge the call reports failure and the results must not be used; $\lambda$ and $\tau$ must be non-negative. Singular-vector signs are arbitrary, and the solution and projector are invariant to them. + +## Variants & Generalizations + +- **Truncated SVD ($\lambda = 0$):** hard cutoff at $\theta$; exact on retained directions, discontinuous in $A$. +- **Adaptive damping:** $\lambda$ zero away from singularity, rising as $\sigma_{\min}$ falls below a margin (Nakamura and Hanafusa), so accuracy is lost only when needed. +- **Weighted damped least squares:** $\min \|W(Ax - b)\|^2 + \lambda^2\|Mx\|^2$ with task and joint weights, reduced to the plain form by scaling. + +## Applications + +- **Redundant manipulators and damped IK:** joint rates $\dot q = J^\dagger_\lambda \dot x + P z$ track the task velocity and use the null space for a secondary objective (joint-limit avoidance, posture) through gradient projection. +- **Control allocation:** distribute a commanded thrust and torque over more actuators than axes, with the projector holding the spare freedom for load sharing or energy minimization. +- **Regularized calibration:** correction estimates from residuals when the sensitivity matrix is ill-conditioned. + +## Connections to Other Algorithms + +- **Singular Value Decomposition** is the factorization; its plain thresholded pseudo-inverse is the undamped special case, and wide matrices are handled there through the transpose. +- **Condition Number** ($\sigma_0/\sigma_{\min}$) indicates when damping is needed and how to size $\lambda$. +- **QR Decomposition, LU Decomposition, Gaussian Elimination** solve full-rank least-squares or square systems more cheaply but reveal no rank and cannot damp gracefully. +- **Regularization and Levenberg–Marquardt:** the $\lambda^2\|x\|^2$ penalty is ridge regression; Levenberg–Marquardt applies the same damping per iteration to keep a Gauss–Newton step bounded. + +## References & Further Reading + +- C. W. Wampler, "Manipulator inverse kinematic solutions based on vector formulations and damped least-squares methods," *IEEE Trans. Systems, Man, and Cybernetics*, 16(1), 1986. +- Y. Nakamura and H. Hanafusa, "Inverse kinematic solutions with singularity robustness for robot manipulator control," *J. Dynamic Systems, Measurement, and Control*, 108, 1986. +- G. H. Golub and C. F. Van Loan, *Matrix Computations*, 4th ed., Johns Hopkins UP, 2013, §5.5. diff --git a/doc/solvers/README.md b/doc/solvers/README.md index 7a9cdddc..b50e65b9 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -19,3 +19,4 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. | [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | | [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | | [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | +| [Damped Pseudo-Inverse](DampedPseudoInverse.md) | Damped (Tikhonov) least-squares inverse and exact null-space projector from one SVD — tall or wide matrices | diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index 1e5bcf74..ae391dca 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -12,6 +12,7 @@ target_link_libraries(numerical.solver ${NUMERICAL_VISIBILITY} target_sources(numerical.solver PRIVATE ConditionNumber.hpp + DampedPseudoInverse.hpp DiscreteAlgebraicRiccatiEquation.hpp DormandPrince45.hpp DurandKerner.hpp @@ -31,6 +32,7 @@ target_sources(numerical.solver PRIVATE ) numerical_add_coverage_sources(numerical.solver + DampedPseudoInverse.cpp DiscreteAlgebraicRiccatiEquation.cpp DurandKerner.cpp GaussianElimination.cpp diff --git a/numerical/solvers/DampedPseudoInverse.cpp b/numerical/solvers/DampedPseudoInverse.cpp new file mode 100644 index 00000000..dd00d33a --- /dev/null +++ b/numerical/solvers/DampedPseudoInverse.cpp @@ -0,0 +1,7 @@ +#include "numerical/solvers/DampedPseudoInverse.hpp" + +namespace solvers +{ + template class DampedPseudoInverse; + template class DampedPseudoInverse; +} diff --git a/numerical/solvers/DampedPseudoInverse.hpp b/numerical/solvers/DampedPseudoInverse.hpp new file mode 100644 index 00000000..fa86c468 --- /dev/null +++ b/numerical/solvers/DampedPseudoInverse.hpp @@ -0,0 +1,156 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/SingularValueDecomposition.hpp" +#include +#include + +namespace solvers +{ + template + class DampedPseudoInverse + { + static_assert(std::is_floating_point_v, "DampedPseudoInverse supports floating-point types only"); + + using Decomposition = SingularValueDecomposition; + static constexpr std::size_t singularValueCount{ Decomposition::SingularValueCount }; + + public: + explicit DampedPseudoInverse(T damping, T rankTolerance = T{ 1e-4f }); + + bool Factor(const math::Matrix& a); + OPTIMIZE_FOR_SPEED math::Vector Solve(const math::Vector& b) const; + OPTIMIZE_FOR_SPEED math::Vector Project(const math::Vector& x) const; + math::Matrix Inverse() const; + math::SquareMatrix NullSpaceProjector() const; + std::size_t Rank() const; + + private: + T Gain(T sigma) const; + + Decomposition svd; + T damping; + T rankTolerance; + T threshold{}; + std::size_t rank{ 0 }; + }; + + template + DampedPseudoInverse::DampedPseudoInverse(T damping, T rankTolerance) + : damping{ damping } + , rankTolerance{ rankTolerance } + { + really_assert(damping >= T{}); + really_assert(rankTolerance >= T{}); + } + + template + bool DampedPseudoInverse::Factor(const math::Matrix& a) + { + rank = 0; + if (!svd.Decompose(a)) + return false; + + const auto& sigma{ svd.SingularValues() }; + threshold = rankTolerance * sigma[0]; + while (rank < singularValueCount && sigma[rank] > threshold) + ++rank; + + return true; + } + + template + OPTIMIZE_FOR_SPEED math::Vector DampedPseudoInverse::Solve(const math::Vector& b) const + { + const auto& sigma{ svd.SingularValues() }; + const auto& u{ svd.U() }; + const auto& v{ svd.V() }; + + math::Vector x{}; + for (std::size_t i = 0; i < singularValueCount; ++i) + { + T projection{}; + for (std::size_t r = 0; r < Rows; ++r) + projection += u.at(r, i) * b[r]; + + const T weight{ Gain(sigma[i]) * projection }; + for (std::size_t c = 0; c < Cols; ++c) + x[c] += weight * v.at(c, i); + } + + return x; + } + + template + OPTIMIZE_FOR_SPEED math::Vector DampedPseudoInverse::Project(const math::Vector& x) const + { + const auto& v{ svd.V() }; + + math::Vector projected{ x }; + for (std::size_t i = 0; i < rank; ++i) + { + T component{}; + for (std::size_t c = 0; c < Cols; ++c) + component += v.at(c, i) * x[c]; + + for (std::size_t c = 0; c < Cols; ++c) + projected[c] -= component * v.at(c, i); + } + + return projected; + } + + template + math::Matrix DampedPseudoInverse::Inverse() const + { + const auto& sigma{ svd.SingularValues() }; + const auto& u{ svd.U() }; + const auto& v{ svd.V() }; + + math::Matrix inverse{}; + for (std::size_t i = 0; i < singularValueCount; ++i) + { + const T gain{ Gain(sigma[i]) }; + for (std::size_t c = 0; c < Cols; ++c) + for (std::size_t r = 0; r < Rows; ++r) + inverse.at(c, r) += gain * v.at(c, i) * u.at(r, i); + } + + return inverse; + } + + template + math::SquareMatrix DampedPseudoInverse::NullSpaceProjector() const + { + const auto& v{ svd.V() }; + + auto projector{ math::SquareMatrix::Identity() }; + for (std::size_t i = 0; i < rank; ++i) + for (std::size_t r = 0; r < Cols; ++r) + for (std::size_t c = 0; c < Cols; ++c) + projector.at(r, c) -= v.at(r, i) * v.at(c, i); + + return projector; + } + + template + std::size_t DampedPseudoInverse::Rank() const + { + return rank; + } + + template + T DampedPseudoInverse::Gain(T sigma) const + { + if (damping > T{}) + return sigma / (sigma * sigma + damping * damping); + + return sigma > threshold ? T{ 1 } / sigma : T{}; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class DampedPseudoInverse; + extern template class DampedPseudoInverse; +#endif +} diff --git a/numerical/solvers/test/CMakeLists.txt b/numerical/solvers/test/CMakeLists.txt index dfc37bb8..818a9777 100644 --- a/numerical/solvers/test/CMakeLists.txt +++ b/numerical/solvers/test/CMakeLists.txt @@ -11,6 +11,7 @@ numerical_link_qemu_runtime(numerical.solvers_test) target_sources(numerical.solvers_test PRIVATE TestConditionNumber.cpp + TestDampedPseudoInverse.cpp TestDiscreteAlgebraicRiccatiEquation.cpp TestDurandKerner.cpp TestGaussianElimination.cpp diff --git a/numerical/solvers/test/TestDampedPseudoInverse.cpp b/numerical/solvers/test/TestDampedPseudoInverse.cpp new file mode 100644 index 00000000..25820d6b --- /dev/null +++ b/numerical/solvers/test/TestDampedPseudoInverse.cpp @@ -0,0 +1,94 @@ +#include "numerical/solvers/DampedPseudoInverse.hpp" +#include +#include +#include + +namespace +{ + using Matrix23 = math::Matrix; + using Vector2 = math::Vector; + using Vector3 = math::Vector; + + class TestDampedPseudoInverse : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + const Matrix23 a{ { 1.0f, 0.0f, 1.0f }, { 0.0f, 1.0f, 1.0f } }; + const Vector2 b{ 1.0f, 2.0f }; + solvers::DampedPseudoInverse exact{ 0.0f }; + solvers::DampedPseudoInverse damped{ 0.1f }; + + void SetUp() override + { + ASSERT_TRUE(exact.Factor(a)); + ASSERT_TRUE(damped.Factor(a)); + } + + template + static void ExpectNear(const math::Matrix& actual, const math::Matrix& expected, float eps = tolerance) + { + for (std::size_t r = 0; r < Rows; ++r) + for (std::size_t c = 0; c < Cols; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), eps) << "(" << r << ", " << c << ")"; + } + + static float Norm(const Vector2& v) + { + return std::sqrt(v[0] * v[0] + v[1] * v[1]); + } + }; +} + +TEST_F(TestDampedPseudoInverse, undamped_solve_is_minimum_norm_exact_solution) +{ + const Vector3 x{ exact.Solve(b) }; + + ExpectNear(x, Vector3{ 0.0f, 1.0f, 1.0f }); + ExpectNear(Vector2{ a * x }, b); +} + +TEST_F(TestDampedPseudoInverse, damped_solve_error_is_bounded) +{ + const Vector3 x{ damped.Solve(b) }; + const float residual{ Norm(a * x - b) }; + + ExpectNear(x, Vector3{ 0.003289f, 0.993388f, 0.996678f }); + EXPECT_NEAR(residual, 0.009934f, tolerance); + EXPECT_LE(residual, 0.1f * 0.1f / 1.0f * Norm(b)); +} + +TEST_F(TestDampedPseudoInverse, projection_is_invisible_to_the_matrix_despite_damping) +{ + const Vector3 projected{ damped.Project(Vector3{ 1.0f, 0.0f, 0.0f }) }; + + ExpectNear(projected, Vector3{ 1.0f / 3.0f, 1.0f / 3.0f, -1.0f / 3.0f }); + ExpectNear(Vector2{ a * projected }, Vector2{}); +} + +TEST_F(TestDampedPseudoInverse, projector_is_an_orthogonal_projector) +{ + const auto p{ damped.NullSpaceProjector() }; + + ExpectNear(p * p, p); + ExpectNear(p.Transpose(), p); + EXPECT_NEAR(p.Trace(), 1.0f, tolerance); +} + +TEST_F(TestDampedPseudoInverse, undamped_inverse_is_a_right_inverse) +{ + ExpectNear(a * exact.Inverse(), math::SquareMatrix::Identity()); +} + +TEST_F(TestDampedPseudoInverse, solve_has_no_null_space_component) +{ + ExpectNear(damped.Project(damped.Solve(b)), Vector3{}); +} + +TEST_F(TestDampedPseudoInverse, rank_loss_grows_the_null_space) +{ + ASSERT_TRUE(exact.Factor(Matrix23{ { 1.0f, 1.0f, 0.0f }, { 2.0f, 2.0f, 0.0f } })); + + EXPECT_EQ(exact.Rank(), 1); + EXPECT_NEAR(exact.NullSpaceProjector().Trace(), 2.0f, tolerance); + ExpectNear(exact.Solve(Vector2{ 1.0f, 2.0f }), Vector3{ 0.5f, 0.5f, 0.0f }); +} diff --git a/roadmap/README.md b/roadmap/README.md index 176a2919..c5a16a32 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -111,4 +111,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) · `DampedPseudoInverse` (62) · `ActiveSetQp` (65) +`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `ActiveSetQp` (65) diff --git a/roadmap/solvers/DampedPseudoInverse/explanation.md b/roadmap/solvers/DampedPseudoInverse/explanation.md deleted file mode 100644 index e31108d8..00000000 --- a/roadmap/solvers/DampedPseudoInverse/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Damped Pseudo-Inverse and Null-Space Projector — Overview - -## What it is -Two tools from one singular value decomposition: a "best effort" inverse of any matrix that stays -well-behaved when the matrix is nearly singular, and a projector that removes from any vector every -component the matrix can see. - -## Why it matters (embedded) -Mapping desired outputs to inputs through a non-square or ill-conditioned matrix is everywhere: joint -rates from a tool velocity, motor commands from a desired thrust and torque, calibration corrections from -residuals. The plain pseudo-inverse explodes near rank loss and commands huge, saturating inputs; damping -caps that at a small, controlled accuracy cost. When there are more inputs than outputs, the projector -exposes the spare freedom so a second goal (stay away from limits, share load, save energy) can use it -without disturbing the first. - -## How it works (intuition) -The SVD splits the matrix into independent directions, each with a gain. Inverting means dividing by each -gain; damping replaces `1/σ` by `σ/(σ² + λ²)`, which matches `1/σ` for large gains and fades to zero for -tiny ones instead of blowing up. The input directions with (numerically) zero gain form the null space; -subtracting a vector's components along the other input directions leaves only what the matrix cannot -feel. That projector must use the undamped, thresholded split — a damped one leaks into the primary task. - -## Key parameters -- **Damping λ** — robustness near singularity versus accuracy (`error ≈ λ²/σ_min²`). -- **Rank tolerance** — relative threshold below which a direction counts as lost. - -## Reference -C. W. Wampler, "Manipulator inverse kinematic solutions based on vector formulations and damped -least-squares methods," *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*, 7(12), 1977; -G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., §6.1. - -## See also -`SingularValueDecomposition` (the factorization), `LevenbergMarquardt` (#64, uses the same damping per -iteration); robotics-toolbox-cpp's redundancy resolution, a thin task-space wrapper over this. diff --git a/roadmap/solvers/DampedPseudoInverse/implementation.md b/roadmap/solvers/DampedPseudoInverse/implementation.md deleted file mode 100644 index 5841c7aa..00000000 --- a/roadmap/solvers/DampedPseudoInverse/implementation.md +++ /dev/null @@ -1,86 +0,0 @@ -# Damped Pseudo-Inverse and Null-Space Projector — Implementation Pseudocode - -> Roadmap ref: #62 (Tier 2) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Singularity-robust solution of `A·x = b` for any `Rows × Cols` matrix, plus the exact projector onto the -null space of `A`, both from one SVD. The damped (Tikhonov) inverse `A⁺_λ` keeps `x` bounded when `A` -loses rank; the projector `P = I − A⁺A` lets a secondary objective move `x` without changing `A·x`. -Consumers: redundant-manipulator resolution and damped IK (robotics-toolbox-cpp), control allocation -for over-actuated drones and vessels, regularized calibration. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -class DampedPseudoInverse: - SingularValueDecomposition svd # tall or wide; k = min(Rows, Cols) - T damping # λ ≥ 0 — damps the solve only - T rankTolerance # relative: σᵢ > rankTolerance·σ₀ counts toward rank - std::size_t rank # r, set by Factor -``` - -## Interface - -```cpp -DampedPseudoInverse(T damping, T rankTolerance = 1e-4) -bool Factor(const math::Matrix& a) # one SVD; false if it does not converge -math::Vector Solve(const math::Vector& b) const # A⁺_λ·b; hot path -math::Vector Project(const math::Vector& x) const # (I − A⁺A)·x, undamped; hot path -math::Matrix Inverse() const # A⁺_λ -math::Matrix NullSpaceProjector() const # I − A⁺A -std::size_t Rank() const -``` - -## Algorithm (pseudocode) - -```text -function Factor(a): - if !svd.Decompose(a): return false # A = U·Σ·Vᵀ; uᵢ = column i of U (Rows), vᵢ = column i of V (Cols) - τ = rankTolerance·σ₀; rank = #{ σᵢ > τ } - return true - -function Gain(σ): # damped inverse of one singular value - λ > 0 ? σ / (σ² + λ²) : (σ > τ ? 1/σ : 0) - -function Solve(b): # OPTIMIZE_FOR_SPEED — O(Rows·k + Cols·k) - return Σᵢ Gain(σᵢ)·(uᵢ·b)·vᵢ # lies in the row space of A ⇒ minimum norm - -function Project(x): # OPTIMIZE_FOR_SPEED — without forming P, O(Cols·r) - return x − Σ_{i)`. - -## Deployment - -- Header: `numerical/solvers/DampedPseudoInverse.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `Solve`/`Project`, and `extern template class DampedPseudoInverse;` / `` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/solvers/DampedPseudoInverse.cpp` → the same instantiations. -- Test: `numerical/solvers/test/TestDampedPseudoInverse.cpp` -- Doc: `doc/solvers/DampedPseudoInverse.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 ...)`; `TestDampedPseudoInverse.cpp` → `numerical.solvers_test`. -- Depends on: `SingularValueDecomposition` with wide-matrix support. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/DampedPseudoInverse/tests.md b/roadmap/solvers/DampedPseudoInverse/tests.md deleted file mode 100644 index 15875030..00000000 --- a/roadmap/solvers/DampedPseudoInverse/tests.md +++ /dev/null @@ -1,48 +0,0 @@ -# Damped Pseudo-Inverse and Null-Space Projector — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestDampedPseudoInverse : public ::testing::Test: - math::Matrix a{ { 1, 0, 1 }, { 0, 1, 1 } } # σ = (√3, 1), null space ∥ (1, 1, −1) - math::Vector b{ 1, 2 } - DampedPseudoInverse exact{ 0.0f } - DampedPseudoInverse damped{ 0.1f } - void SetUp(): ASSERT_TRUE(exact.Factor(a)) and ASSERT_TRUE(damped.Factor(a)) -# each case below is a TEST_F(TestDampedPseudoInverse, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -undamped_solve_is_minimum_norm_exact_solution: - Assert: exact.Solve(b) ≈ (0, 1, 1) and a·exact.Solve(b) ≈ b -damped_solve_error_is_bounded: - Assert: damped.Solve(b) ≈ (0.003289, 0.993388, 0.996678); - ‖a·damped.Solve(b) − b‖ ≈ 0.009934 ≤ λ²/σ_min²·‖b‖ = 0.022361 -projection_is_invisible_to_the_matrix_despite_damping: - Assert: damped.Project((1, 0, 0)) ≈ (1/3, 1/3, −1/3) and a·damped.Project((1, 0, 0)) ≈ 0 -projector_is_an_orthogonal_projector: - Arrange: P = damped.NullSpaceProjector() - Assert: P·P ≈ P, Pᵀ ≈ P, trace P ≈ 1 (= Cols − rank) -undamped_inverse_is_a_right_inverse: - Assert: a·exact.Inverse() ≈ I₂ -solve_has_no_null_space_component: - Assert: damped.Project(damped.Solve(b)) ≈ 0 -rank_loss_grows_the_null_space: - Arrange: rank-1 matrix { {1, 1, 0}, {2, 2, 0} } (σ = (√10, 0)) - Assert: Rank() == 1; trace NullSpaceProjector() ≈ 2; exact.Solve((1, 2)) ≈ (0.5, 0.5, 0) -``` - -## Reference vectors - -- `a = [[1,0,1],[0,1,1]]`: `σ = (1.732051, 1)`, `A⁺b = (0, 1, 1)`, `P = (1/3)·[[1,1,−1],[1,1,−1],[−1,−1,1]]`. -- `λ = 0.1`: `A⁺_λb = (0.003289, 0.993388, 0.996678)`, residual `0.009934` (python3/numpy). - -## Edge cases - -- Tall matrix (`Rows > Cols`, full column rank) ⇒ `NullSpaceProjector() ≈ 0`, `Solve` is least squares. -- `λ = 0` with an exactly singular direction ⇒ zero gain on it (no division by zero). -- Zero matrix ⇒ `Rank() == 0`, `Solve` returns zero, `Project` is the identity. From 349791261243fc7232c845ceca47687f1af97468 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:32:24 +0000 Subject: [PATCH 3/6] feat(estimators): add incremental QR least squares MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 63: streaming linear least squares in fixed memory. Each row is rotated into an upper-triangular factor with Givens rotations, so YᵀY is never formed. Solve runs a rank-revealing SVD of that factor and returns the minimum-norm parameters, the rank, the identifiable row space and the residual RMS. Checked against double-precision normal equations on 300 random 3-parameter problems with 5–205 rows (parameters agree to ≈3e-6). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 12 +- ROADMAP.md | 10 -- doc/estimators/IncrementalQrLeastSquares.md | 155 ++++++++++++++++++ doc/estimators/README.md | 1 + numerical/estimators/offline/CMakeLists.txt | 2 + .../offline/IncrementalQrLeastSquares.cpp | 7 + .../offline/IncrementalQrLeastSquares.hpp | 129 +++++++++++++++ .../estimators/offline/test/CMakeLists.txt | 1 + .../test/TestIncrementalQrLeastSquares.cpp | 117 +++++++++++++ roadmap/README.md | 2 +- .../IncrementalQrLeastSquares/explanation.md | 34 ---- .../implementation.md | 91 ---------- .../IncrementalQrLeastSquares/tests.md | 45 ----- 13 files changed, 419 insertions(+), 187 deletions(-) create mode 100644 doc/estimators/IncrementalQrLeastSquares.md create mode 100644 numerical/estimators/offline/IncrementalQrLeastSquares.cpp create mode 100644 numerical/estimators/offline/IncrementalQrLeastSquares.hpp create mode 100644 numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp delete mode 100644 roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md delete mode 100644 roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md delete mode 100644 roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md diff --git a/README.md b/README.md index 9908b0b8..bbc3e47d 100644 --- a/README.md +++ b/README.md @@ -14,12 +14,12 @@ Refer to the documentation to quickly integrate and utilize the library's signal ## Documentation -| Category | Description | -|--------------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| -| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | -| [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 | +| Category | Description | +|--------------------------------------------------------------------|----------------------------------------------------------------------| +| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | +| [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, 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 | | [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | diff --git a/ROADMAP.md b/ROADMAP.md index 0e5bee4d..b60bdc8d 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -29,7 +29,6 @@ Difficulty legend: | 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | -| 63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | ★★★☆☆ | | 64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | ★★★☆☆ | | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | | 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ | @@ -84,15 +83,6 @@ the library does not yet expose. Detailed below under ## Tier 3 — Moderate ★★★☆☆ -### 63. Incremental QR least squares -- **What:** Streaming Givens-QR accumulation of an unbounded number of rows into a `P×P` factor, with a - rank-revealing SVD solve (minimum-norm solution, identifiable directions, residual RMS). -- **Embedded value:** Parameter identification from long logs in fixed memory without squaring the - condition number. -- **Algorithm / paper:** G. Golub, C. Van Loan, §5.1.8 and §5.5; Å. Björck (1996), Ch. 2. -- **Reuses:** `GivensRotation`, `SingularValueDecomposition`. -- **Spec:** [roadmap/estimators/offline/IncrementalQrLeastSquares](roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md). - ### 64. Levenberg–Marquardt - **What:** Damped Gauss–Newton for small nonlinear least squares with a problem-supplied retraction (vectors, bounds, rotations) and a pluggable damping policy. diff --git a/doc/estimators/IncrementalQrLeastSquares.md b/doc/estimators/IncrementalQrLeastSquares.md new file mode 100644 index 00000000..1bc271ea --- /dev/null +++ b/doc/estimators/IncrementalQrLeastSquares.md @@ -0,0 +1,155 @@ +# Incremental QR Least Squares + +## Overview & Motivation + +Identification and calibration runs produce long logs — motor current against acceleration, joint +torque against motion — that a microcontroller cannot store. Accumulating the normal equations fits +in memory but squares the condition number, which single precision cannot afford. Real regressors +are also often rank-deficient: a parameter never shows up in the data, or only in a fixed +combination with another, and a plain solver then returns garbage. + +Incremental QR least squares rotates each arriving row into a small triangular summary and forgets +it: memory stays fixed however long the log runs and the data matrix is never squared. The final +solve decomposes the summary into singular directions, inverts the strong ones, drops the weak ones, +and reports which parameter combinations the data could actually determine. + +## Mathematical Theory + +### Streaming QR Factor + +Rows $(\mathbf{y}_k^T, \tau_k)$ with $\mathbf{y}_k \in \mathbb{R}^P$ arrive one at a time, $m$ +unbounded. The task is $\min_{\boldsymbol{\pi}} \|Y\boldsymbol{\pi} - \boldsymbol{\tau}\|_2$ over the +stacked rows, taking the minimum-norm minimizer. Write +$Y = Q \begin{bmatrix} R \\ 0 \end{bmatrix}$ with $Q$ orthogonal and $R$ upper triangular +$P \times P$, and split $Q^T\boldsymbol{\tau} = (\mathbf{d}, \boldsymbol{\tau}_\perp)$. Orthogonal maps +preserve norms, so + +$$\|Y\boldsymbol{\pi} - \boldsymbol{\tau}\|^2 = \|R\boldsymbol{\pi} - \mathbf{d}\|^2 + \rho, +\qquad \rho = \|\boldsymbol{\tau}_\perp\|^2$$ + +The problem lives in $R$, $\mathbf{d}$ and $\rho$, the part of the right-hand side no +$\boldsymbol{\pi}$ can reach: $P^2 + P + 2$ scalars with the row count. $Q$ is never formed. + +**Row update.** Place the new row beneath $[\,R \mid \mathbf{d}\,]$. For $j = 1, \dots, P$, skip the +step if $y_j = 0$; otherwise rotate row $j$ of $R$ and the incoming row in their common plane: + +$$\begin{bmatrix} c & s \\ -s & c \end{bmatrix} +\begin{bmatrix} R_{j,j:P} & d_j \\ y_{j:P} & \tau \end{bmatrix}, +\qquad c = \frac{R_{jj}}{\ell},\; s = \frac{y_j}{\ell},\; \ell = \sqrt{R_{jj}^2 + y_j^2}$$ + +This zeroes $y_j$, leaves $R_{jj} = \ell \ge 0$ and touches only columns $j$ to $P$. After the sweep +the incoming row is zero in every column, so its leftover right-hand side $\tilde{\tau}$ reads +$0 \cdot \boldsymbol{\pi} = \tilde{\tau}$ and adds $\tilde{\tau}^2$ to $\rho$ whatever +$\boldsymbol{\pi}$ is. A zero pivot degenerates to a swap and a zero pair to the identity. + +**Why not normal equations.** $Y^TY = R^TR$, yet the singular values of $R$ are those of $Y$: the +factor works at $\kappa(Y)$, the normal equations at $\kappa(Y)^2$. With single-precision +$\epsilon \approx 1.2 \times 10^{-7}$ they fail near $\kappa(Y) \approx \epsilon^{-1/2} \approx 3 +\times 10^3$; the factor lasts to about $\epsilon^{-1}$. + +### Rank-Revealing Solve + +Take $R = U\Sigma V^T$ with $\sigma_0 \ge \dots \ge \sigma_{P-1} \ge 0$. In the coordinates +$\mathbf{z} = V^T\boldsymbol{\pi}$, + +$$\|R\boldsymbol{\pi} - \mathbf{d}\|^2 = \sum_{i=0}^{P-1} \left(\sigma_i z_i - \mathbf{u}_i^T\mathbf{d}\right)^2$$ + +The rank is $r = \#\{\, i : \sigma_i > \text{tol} \cdot \sigma_0 \,\}$. For $i < r$ the term is zeroed by +$z_i = \mathbf{u}_i^T\mathbf{d} / \sigma_i$; for $i \ge r$ it cannot be removed and $z_i$ is free, so +the minimum-norm choice is $z_i = 0$. The discarded terms are charged to the residual: + +$$\hat{\boldsymbol{\pi}} = \sum_{i < r} \mathbf{v}_i \, \frac{\mathbf{u}_i^T\mathbf{d}}{\sigma_i}, +\qquad +\text{RMS} = \sqrt{\frac{\rho + \sum_{i \ge r} (\mathbf{u}_i^T\mathbf{d})^2}{m}}$$ + +The first $r$ rows of $V^T$ are the combinations $\mathbf{v}_i^T\boldsymbol{\pi}$ the data determine; +the rest are the unidentifiable directions. Moving $\boldsymbol{\pi}$ along them leaves the fit +unchanged, and $\hat{\boldsymbol{\pi}}$ is orthogonal to them. + +Scaling a row and its right-hand side by $\sqrt{w_k}$ gives weighted least squares. Every step is +orthogonal, so row order changes the result only by rounding. With no rows there is no result; a +reset clears the state. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|------------------------|-----------------|----------------|------------------------------------------------------| +| Row, sparse (best) | $O(P)$ | $P^2 + P + 2$ | Zero entries skipped; one short rotation per nonzero | +| Row, dense (worst) | $O(P^2)$ | $P^2 + P + 2$ | $P$ rotations on $P - j + 1$ pairs, $P$ square roots | +| Solve | $O(P^3)$ | $O(P^2)$ extra | One $P \times P$ SVD, independent of $m$ | +| Whole stream, $m$ rows | $O(mP^2 + P^3)$ | $P^2 + P + 2$ | Memory does not grow with $m$ | + +At $P = 60$ the state is about 14 KB in single precision. Accumulation can run at the sample rate; +the solve is an offline or commissioning step. No heap, no recursion. + +## Step-by-Step Walkthrough + +**Full rank.** Rows $(1, x)$ at $x = (0, 1, 2, 3)$, $\boldsymbol{\tau} = (2.1, 4.9, 8.1, 10.9)$: + +| Rows | $R_{00}$ | $R_{01}$ | $R_{11}$ | $d_0$ | $d_1$ | $\rho$ | +|------|----------|----------|----------|--------|--------|--------| +| 1 | 1 | 0 | 0 | 2.1 | 0 | 0 | +| 2 | 1.4142 | 0.7071 | 0.7071 | 4.9497 | 1.9799 | 0 | +| 3 | 1.7321 | 1.7321 | 1.4142 | 8.7180 | 4.2426 | 0.0267 | +| 4 | 2 | 3 | 2.2361 | 13 | 6.6188 | 0.0320 | + +Row 1 meets a zero pivot and is adopted; row 2 rotates by $45^\circ$ and fits two points exactly; +from row 3 on, $\rho$ grows. With $\sigma \approx (4.1000, 1.0908)$, $r = 2$ and back-substitution +gives $\hat{\pi}_1 = d_1 / R_{11} = 2.96$, $\hat{\pi}_0 = (13 - 3 \cdot 2.96)/2 = 2.06$, with +RMS $= \sqrt{0.032 / 4} = 0.089443$. + +**Rank-deficient.** Rows $(1, x, x)$ with $\boldsymbol{\tau} = 2 + 3x$: two equal columns leave the +third row of $R$ zero, $\rho = 0$, and $\sigma = (5.540482,\, 1.141517,\, 0)$, so $r = 2$. Only +$\pi_1 + \pi_2 = 3$ is determined; the minimum-norm split is $\hat{\boldsymbol{\pi}} = (2, 1.5, 1.5)$. +The last row of $V^T$ is the unidentifiable direction $(0, 1, -1)/\sqrt2$. + +Verified against double-precision normal equations on random 3-parameter problems of 5 to 205 rows: +parameters agree to about $3 \times 10^{-6}$. + +## Pitfalls & Edge Cases + +- **Column scaling.** The rank test is relative to $\sigma_0$, so a column in large units hides the + others. Scale columns to comparable magnitude before accumulating. +- **Tolerance.** Too small admits noise-level directions and the solution grows like $1/\sigma_i$; too + large discards real ones, visible as a higher RMS. About $10^{-5}$ is a single-precision start. +- **RMS is not a noise estimate.** It divides by $m$, not $m - r$; scale its square by $m / (m - r)$ + for an unbiased variance. +- **Few or repeated rows.** Fewer rows than parameters is valid, with rank at most $m$; rows that + repeat earlier ones leave the rank unchanged. +- **Long runs.** $\rho$ only grows; once it is about $2^{24}$ times one term, new terms vanish and the + RMS turns optimistic. One NaN row poisons the state until a reset. + +## Variants & Generalizations + +- **Ridge.** Feed $P$ extra rows $\sqrt{\lambda}\,\mathbf{e}_j$ with zero right-hand side. Replacing + $1/\sigma_i$ by $\sigma_i / (\sigma_i^2 + \lambda)$ filters smoothly instead of cutting off. +- **Forgetting.** Scaling $R$, $\mathbf{d}$ by $\sqrt{\lambda}$ and $\rho$ by $\lambda$ before each row + gives exponentially weighted square-root recursive least squares. +- **Merging logs.** Rotating the rows of one summary $[\,R \mid \mathbf{d}\,]$ into another, and adding + the $\rho$ values and counts, combines separately accumulated segments. + +## Applications + +- Inertial-parameter identification of robot arms, rank-deficient by construction +- Motor and drive parameter identification from long logs +- Friction identification: if the axis only ever moves one way, the sign column duplicates the offset + column and the unidentifiable direction says so +- Sensor calibration from data streams too long to store + +## Connections to Other Algorithms + +- [Givens Rotation](../math/GivensRotation.md): the plane rotation that zeroes one entry per step. +- [Singular Value Decomposition](../solvers/SingularValueDecomposition.md): reveals rank and directions. +- [QR Decomposition](../solvers/QrDecomposition.md): the same factor, built row by row, no $Q$ kept. +- [Recursive Least Squares](RecursiveLeastSquares.md): forgetting and a $P \times P$ covariance; here + all rows weigh equally and rank is revealed. +- [Linear Regression](LinearRegression.md): batch normal equations that store the samples. +- [Total Least Squares](TotalLeastSquares.md): also SVD-based, but for noisy regressors, not rank. +- [Friction Compensation](../controllers/FrictionCompensation.md): its Coulomb and viscous parameters + are linear and identifiable this way. + +## References & Further Reading + +- G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., Johns Hopkins University Press, 2013 — + §5.1.8–5.3 (Givens QR, updating) and §5.5 (rank-deficient least squares). +- Å. Björck, *Numerical Methods for Least Squares Problems*, SIAM, 1996. diff --git a/doc/estimators/README.md b/doc/estimators/README.md index 58901867..ff60692a 100644 --- a/doc/estimators/README.md +++ b/doc/estimators/README.md @@ -11,6 +11,7 @@ Statistical estimation algorithms for fitting models to observed data and making | [Total Least Squares](TotalLeastSquares.md) | Errors-in-variables fit (noisy regressors) via the SVD of `[A | b]` | | [Yule-Walker](YuleWalker.md) | Autoregressive model parameter estimation via the Yule-Walker equations | | [Expectation-Maximization](ExpectationMaximization.md) | EM algorithm for Kalman filter parameter identification (Shumway-Stoffer) | +| [Incremental QR Least Squares](IncrementalQrLeastSquares.md) | Streaming Givens QR in fixed memory with a rank-revealing SVD solve (minimum-norm, identifiable combinations) | ## Online Estimators (Streaming) diff --git a/numerical/estimators/offline/CMakeLists.txt b/numerical/estimators/offline/CMakeLists.txt index e20be88d..ef7498ba 100644 --- a/numerical/estimators/offline/CMakeLists.txt +++ b/numerical/estimators/offline/CMakeLists.txt @@ -14,6 +14,7 @@ target_link_libraries(numerical.estimators.offline ${NUMERICAL_VISIBILITY} target_sources(numerical.estimators.offline PRIVATE ExpectationMaximization.hpp + IncrementalQrLeastSquares.hpp LinearRegression.hpp PolynomialFitting.hpp TotalLeastSquares.hpp @@ -22,6 +23,7 @@ target_sources(numerical.estimators.offline PRIVATE numerical_add_coverage_sources(numerical.estimators.offline ExpectationMaximization.cpp + IncrementalQrLeastSquares.cpp LinearRegression.cpp PolynomialFitting.cpp TotalLeastSquares.cpp diff --git a/numerical/estimators/offline/IncrementalQrLeastSquares.cpp b/numerical/estimators/offline/IncrementalQrLeastSquares.cpp new file mode 100644 index 00000000..6dd71a3e --- /dev/null +++ b/numerical/estimators/offline/IncrementalQrLeastSquares.cpp @@ -0,0 +1,7 @@ +#include "numerical/estimators/offline/IncrementalQrLeastSquares.hpp" + +namespace estimators +{ + template class IncrementalQrLeastSquares; + template class IncrementalQrLeastSquares; +} diff --git a/numerical/estimators/offline/IncrementalQrLeastSquares.hpp b/numerical/estimators/offline/IncrementalQrLeastSquares.hpp new file mode 100644 index 00000000..7505eb17 --- /dev/null +++ b/numerical/estimators/offline/IncrementalQrLeastSquares.hpp @@ -0,0 +1,129 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/GivensRotation.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/solvers/SingularValueDecomposition.hpp" +#include +#include +#include + +namespace estimators +{ + template + struct LeastSquaresResult + { + static_assert(std::is_floating_point_v, "LeastSquaresResult supports floating-point types only"); + + math::Vector parameters{}; + std::size_t rank{ 0 }; + math::SquareMatrix rowSpace{}; + T residualRms{}; + }; + + template + class IncrementalQrLeastSquares + { + static_assert(std::is_floating_point_v, "IncrementalQrLeastSquares supports floating-point types only"); + + public: + using Regressor = math::Vector; + using Result = LeastSquaresResult; + + OPTIMIZE_FOR_SPEED void AddRow(const Regressor& row, T rhs); + template + void AddRows(const math::Matrix& block, const math::Vector& rhs); + std::optional Solve(T relativeTolerance) const; + void Reset(); + std::size_t Rows() const; + + private: + math::SquareMatrix r{}; + Regressor d{}; + T residualSquares{}; + std::size_t rows{ 0 }; + }; + + template + OPTIMIZE_FOR_SPEED void IncrementalQrLeastSquares::AddRow(const Regressor& row, T rhs) + { + Regressor x{ row }; + for (std::size_t j = 0; j < Parameters; ++j) + { + if (x[j] == T{}) + continue; + + const auto rotation{ math::ComputeGivens(r.at(j, j), x[j]) }; + for (std::size_t c = j; c < Parameters; ++c) + math::ApplyGivens(rotation, r.at(j, c), x[c]); + math::ApplyGivens(rotation, d[j], rhs); + } + + residualSquares += rhs * rhs; + ++rows; + } + + template + template + void IncrementalQrLeastSquares::AddRows(const math::Matrix& block, const math::Vector& rhs) + { + for (std::size_t i = 0; i < BlockRows; ++i) + AddRow(block.template GetBlock<1, Parameters>(i, 0).Transpose(), rhs[i]); + } + + template + auto IncrementalQrLeastSquares::Solve(T relativeTolerance) const -> std::optional + { + solvers::SingularValueDecomposition svd; + if (rows == 0 || !svd.Decompose(r)) + return std::nullopt; + + const auto& sigma{ svd.SingularValues() }; + const auto& u{ svd.U() }; + const auto& v{ svd.V() }; + const T threshold{ relativeTolerance * sigma[0] }; + + Result result{}; + T unexplained{ residualSquares }; + for (std::size_t i = 0; i < Parameters; ++i) + { + T projection{}; + for (std::size_t k = 0; k < Parameters; ++k) + projection += u.at(k, i) * d[k]; + + if (sigma[i] > threshold) + { + ++result.rank; + for (std::size_t k = 0; k < Parameters; ++k) + result.parameters[k] += v.at(k, i) * projection / sigma[i]; + } + else + unexplained += projection * projection; + } + + result.rowSpace = v.Transpose(); + result.residualRms = math::Sqrt(unexplained / static_cast(rows)); + return result; + } + + template + void IncrementalQrLeastSquares::Reset() + { + r = math::SquareMatrix{}; + d = Regressor{}; + residualSquares = T{}; + rows = 0; + } + + template + std::size_t IncrementalQrLeastSquares::Rows() const + { + return rows; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class IncrementalQrLeastSquares; + extern template class IncrementalQrLeastSquares; +#endif +} diff --git a/numerical/estimators/offline/test/CMakeLists.txt b/numerical/estimators/offline/test/CMakeLists.txt index 6b9ca5b6..e06bff48 100644 --- a/numerical/estimators/offline/test/CMakeLists.txt +++ b/numerical/estimators/offline/test/CMakeLists.txt @@ -11,6 +11,7 @@ numerical_link_qemu_runtime(numerical.estimators.offline_test) target_sources(numerical.estimators.offline_test PRIVATE TestExpectationMaximization.cpp + TestIncrementalQrLeastSquares.cpp TestLinearRegression.cpp TestPolynomialFitting.cpp TestTotalLeastSquares.cpp diff --git a/numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp b/numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp new file mode 100644 index 00000000..f5404a4a --- /dev/null +++ b/numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp @@ -0,0 +1,117 @@ +#include "numerical/estimators/offline/IncrementalQrLeastSquares.hpp" +#include +#include +#include +#include + +namespace +{ + using Line = estimators::IncrementalQrLeastSquares; + using Redundant = estimators::IncrementalQrLeastSquares; + + class TestIncrementalQrLeastSquares : public ::testing::Test + { + protected: + Line line; + Redundant redundant; + const std::array x{ 0.0f, 1.0f, 2.0f, 3.0f }; + const std::array noisy{ 2.1f, 4.9f, 8.1f, 10.9f }; + + void AddNoisyRows(Line& estimator, bool reversed) const + { + for (std::size_t n = 0; n < x.size(); ++n) + { + const std::size_t i{ reversed ? x.size() - 1 - n : n }; + estimator.AddRow(Line::Regressor{ 1.0f, x[i] }, noisy[i]); + } + } + }; +} + +TEST_F(TestIncrementalQrLeastSquares, exact_data_recovers_parameters) +{ + for (float xi : x) + line.AddRow(Line::Regressor{ 1.0f, xi }, 2.0f + 3.0f * xi); + + const auto result{ line.Solve(1e-5f) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result->parameters[0], 2.0f, 1e-5f); + EXPECT_NEAR(result->parameters[1], 3.0f, 1e-5f); + EXPECT_EQ(result->rank, 2); + EXPECT_NEAR(result->residualRms, 0.0f, 1e-5f); +} + +TEST_F(TestIncrementalQrLeastSquares, noisy_data_matches_batch_least_squares) +{ + AddNoisyRows(line, false); + + const auto result{ line.Solve(1e-5f) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_NEAR(result->parameters[0], 2.06f, 1e-5f); + EXPECT_NEAR(result->parameters[1], 2.96f, 1e-5f); + EXPECT_NEAR(result->residualRms, 0.089443f, 1e-5f); +} + +TEST_F(TestIncrementalQrLeastSquares, rank_deficient_regressor_returns_minimum_norm_solution) +{ + for (float xi : x) + redundant.AddRow(Redundant::Regressor{ 1.0f, xi, xi }, 2.0f + 3.0f * xi); + + const auto result{ redundant.Solve(1e-5f) }; + + ASSERT_TRUE(result.has_value()); + EXPECT_EQ(result->rank, 2); + EXPECT_NEAR(result->parameters[0], 2.0f, 1e-5f); + EXPECT_NEAR(result->parameters[1], 1.5f, 1e-5f); + EXPECT_NEAR(result->parameters[2], 1.5f, 1e-5f); + EXPECT_NEAR(result->rowSpace.at(2, 0), 0.0f, 1e-5f); + EXPECT_NEAR(std::abs(result->rowSpace.at(2, 1)), 1.0f / std::sqrt(2.0f), 1e-5f); + EXPECT_NEAR(result->rowSpace.at(2, 1), -result->rowSpace.at(2, 2), 1e-5f); +} + +TEST_F(TestIncrementalQrLeastSquares, row_order_does_not_change_the_result) +{ + Line reversed; + AddNoisyRows(line, false); + AddNoisyRows(reversed, true); + + const auto forward{ line.Solve(1e-5f) }; + const auto backward{ reversed.Solve(1e-5f) }; + + ASSERT_TRUE(forward.has_value()); + ASSERT_TRUE(backward.has_value()); + EXPECT_NEAR(forward->parameters[0], backward->parameters[0], 1e-5f); + EXPECT_NEAR(forward->parameters[1], backward->parameters[1], 1e-5f); + EXPECT_NEAR(forward->residualRms, backward->residualRms, 1e-5f); +} + +TEST_F(TestIncrementalQrLeastSquares, block_rows_equal_single_rows) +{ + Line blocked; + const math::Matrix block{ { 1.0f, x[0] }, { 1.0f, x[1] }, { 1.0f, x[2] }, { 1.0f, x[3] } }; + blocked.AddRows(block, math::Vector{ noisy[0], noisy[1], noisy[2], noisy[3] }); + AddNoisyRows(line, false); + + const auto single{ line.Solve(1e-5f) }; + const auto batch{ blocked.Solve(1e-5f) }; + + ASSERT_TRUE(single.has_value()); + ASSERT_TRUE(batch.has_value()); + EXPECT_EQ(blocked.Rows(), 4); + EXPECT_FLOAT_EQ(batch->parameters[0], single->parameters[0]); + EXPECT_FLOAT_EQ(batch->parameters[1], single->parameters[1]); + EXPECT_FLOAT_EQ(batch->residualRms, single->residualRms); +} + +TEST_F(TestIncrementalQrLeastSquares, no_rows_returns_nullopt) +{ + EXPECT_FALSE(line.Solve(1e-5f).has_value()); + + AddNoisyRows(line, false); + line.Reset(); + + EXPECT_EQ(line.Rows(), 0); + EXPECT_FALSE(line.Solve(1e-5f).has_value()); +} diff --git a/roadmap/README.md b/roadmap/README.md index c5a16a32..4fdf37ba 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -96,7 +96,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `SignalDetectors` (5) · `ConvolutionCorrelation` (11) · `GoertzelAlgorithm` (13) · `RealFastFourierTransform` (25) · `HilbertTransform` (37) · `DiscreteWaveletTransform` (38) ### `estimators/offline` -`PolynomialFitting` (12) · `TotalLeastSquares` (44) · `IncrementalQrLeastSquares` (63) +`PolynomialFitting` (12) · `TotalLeastSquares` (44) ### `estimators/online` `LmsAdaptiveFilter` (21) diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md deleted file mode 100644 index ced17a23..00000000 --- a/roadmap/estimators/offline/IncrementalQrLeastSquares/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Incremental QR Least Squares — Overview - -## What it is -A least-squares solver that takes measurements one row at a time, keeps only a small square summary, -and at the end returns the best-fitting parameters — together with which parameter combinations the -data could actually determine. - -## Why it matters (embedded) -Identification and calibration runs produce long logs: thousands of samples of motor current versus -acceleration, or joint torques versus motion. Storing them all does not fit in a microcontroller, and the -textbook shortcut (normal equations) squares the condition number, which `float` cannot afford. Rotating -each row into a triangular summary keeps memory fixed and accuracy close to a full QR. Real systems are -often rank-deficient — some parameters never show up in the data, or only in fixed combinations — so the -solver must report what is identifiable instead of returning garbage. - -## How it works (intuition) -Keep an upper-triangular matrix and a right-hand-side vector. For each new row, a sequence of plane -rotations zeroes its entries one by one against the triangle; what remains of the right-hand side is -pure misfit and is just accumulated as a squared error. Solving at the end means decomposing the small -triangle into singular directions, inverting the strong ones and dropping the weak ones: the result is -the minimum-norm solution, and the kept directions are the identifiable combinations. - -## Key parameters -- **Parameter count `P`** — compile-time; memory is `P²`. -- **Relative rank tolerance** — which singular values count as identifiable. - -## Reference -G. H. Golub, C. F. Van Loan, *Matrix Computations*, 4th ed., §5.1.8 (Givens QR) and §5.5 (rank-deficient -least squares); Å. Björck, *Numerical Methods for Least Squares Problems* (1996), Ch. 2. - -## See also -`RecursiveLeastSquares` (online, forgetting), `LinearRegression` (batch with stored data), -`TotalLeastSquares`, `SingularValueDecomposition`, `GivensRotation`; robotics-toolbox-cpp's dynamic -parameter identification, which feeds the inertial regressor into this solver. diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md deleted file mode 100644 index a22db7c7..00000000 --- a/roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md +++ /dev/null @@ -1,91 +0,0 @@ -# Incremental QR Least Squares (rank-revealing) — Implementation Pseudocode - -> Roadmap ref: #63 (Tier 3) · Target: `numerical/estimators/offline` · Namespace `estimators` · Type: `float` (templated on `T`, instantiated for `float` only) - -Batch linear least squares `min ‖Y·π − τ‖` over an unbounded number of rows with **fixed memory**: each -row is rotated into an upper-triangular factor with Givens rotations as it arrives, no sample is stored, -and `YᵀY` is never formed. The solve is rank-revealing (SVD of the `P×P` factor), returning the -minimum-norm solution and the identifiable parameter combinations when the regressor is rank-deficient. -Consumers: robot inertial-parameter identification (robotics-toolbox-cpp), motor/drive parameter -identification, sensor calibration from long logs. - -## Data structures - -```cpp -template # static_assert(std::is_floating_point_v); instantiated for float -struct LeastSquaresResult: - math::Vector parameters # minimum-norm solution - std::size_t rank # number of identifiable combinations - math::Matrix rowSpace # first `rank` rows span the identifiable combinations - T residualRms # sqrt(‖Y·π̂ − τ‖² / rows) - -template -class IncrementalQrLeastSquares: - math::SquareMatrix R # upper-triangular factor (STATE) - math::Vector d # Qᵀ·τ (STATE) - T residualSquares # part of ‖τ‖² outside range(R) (STATE) - std::size_t rows -``` - -## Interface - -```cpp -void AddRow(const math::Vector& row, T rhs) # O(P²); hot path of the accumulation -template -void AddRows(const math::Matrix& block, const math::Vector& rhs) # AddRow per row -std::optional> Solve(T relativeTolerance) const # nullopt if no rows -void Reset() -std::size_t Rows() const -``` - -## Algorithm (pseudocode) - -```text -function AddRow(row, rhs): # OPTIMIZE_FOR_SPEED — streaming Givens QR - for j in 0..P−1: # rotate the row into R - if row[j] == 0: continue - g = math::ComputeGivens(R[j][j], row[j]) - rotate (R[j][j..P−1], d[j]) with (row[j..P−1], rhs) using g # zeroes row[j] - residualSquares += rhs² # what is left lies outside range(R) - rows += 1 - -function Solve(tol): - if rows == 0: return nullopt - svd.Decompose(R) # SingularValueDecomposition - rank = #{ σᵢ > tol·σ₀ } - π̂ = Σ_{i)`. - -## Deployment - -- Header: `numerical/estimators/offline/IncrementalQrLeastSquares.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` - on `AddRow`, and `extern template class IncrementalQrLeastSquares;` / `` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/estimators/offline/IncrementalQrLeastSquares.cpp` → the same instantiations. -- Test: `numerical/estimators/offline/test/TestIncrementalQrLeastSquares.cpp` -- Doc: `doc/estimators/IncrementalQrLeastSquares.md` (per `doc/TEMPLATE.md`) + row in `doc/estimators/README.md` -- CMake: `.hpp` → `target_sources(numerical.estimators.offline PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.estimators.offline ...)`; test → `numerical.estimators.offline_test`. -- Depends on: `math::GivensRotation`, `solvers::SingularValueDecomposition`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md b/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md deleted file mode 100644 index 0879ee10..00000000 --- a/roadmap/estimators/offline/IncrementalQrLeastSquares/tests.md +++ /dev/null @@ -1,45 +0,0 @@ -# Incremental QR Least Squares — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestIncrementalQrLeastSquares : public ::testing::Test: - IncrementalQrLeastSquares line # rows (1, x) - IncrementalQrLeastSquares redundant # rows (1, x, x) — rank 2 - std::array x{ 0, 1, 2, 3 } -# each case below is a TEST_F(TestIncrementalQrLeastSquares, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -exact_data_recovers_parameters: - Arrange: AddRow((1, xᵢ), 2 + 3xᵢ) for every i - Assert: Solve(1e-5) → parameters ≈ (2, 3), rank == 2, residualRms ≈ 0 -noisy_data_matches_batch_least_squares: - Arrange: τ = (2.1, 4.9, 8.1, 10.9) - Assert: parameters ≈ (2.06, 2.96), residualRms ≈ 0.089443 -rank_deficient_regressor_returns_minimum_norm_solution: - Arrange: redundant.AddRow((1, xᵢ, xᵢ), 2 + 3xᵢ) - Assert: rank == 2; parameters ≈ (2, 1.5, 1.5); rowSpace row 2 ∥ (0, 1, −1) -row_order_does_not_change_the_result: - Assert: feeding the noisy rows reversed gives the same parameters and residualRms (tol 1e-5) -block_rows_equal_single_rows: - Assert: AddRows(4×2 block, τ) gives the same result as four AddRow calls -no_rows_returns_nullopt: - Assert: Solve(1e-5) == std::nullopt, also after Reset() -``` - -## Reference vectors - -- `x = (0, 1, 2, 3)`, `τ = (2.1, 4.9, 8.1, 10.9)`: least squares `(2.06, 2.96)`, residual RMS `0.089443`. -- Rows `(1, x, x)`, `τ = 2 + 3x`: `σ = (5.540482, 1.141517, 0)`, minimum-norm `(2, 1.5, 1.5)`, null - direction `(0, 1, −1)/√2` (python3/numpy). - -## Edge cases - -- One row only ⇒ rank 1, finite result. -- Large row count (10⁵ rows of the line model) ⇒ same parameters; no growth in memory. -- Rows that are exact multiples of earlier ones ⇒ rank unchanged. From 6045e8b5c98f35ad6a9319586ffa70a99aca2eaa Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:32:48 +0000 Subject: [PATCH 4/6] feat(math): add SE(3) transform with twist/wrench algebra and exp/log MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 53 with linear-first 6-vectors: twists (v; ω), wrenches (f; n). Covers composition, closed-form inverse, adjoint, twist/wrench transforms, exponential, logarithm, SO(3) log and a base-frame pose error. Two changes from the spec, both for float accuracy: - Exp uses unnormalised-axis coefficients sin φ/φ, (1 − cos φ)/φ² (in half-angle form) and (φ − sin φ)/φ³, with short series below φ = 0.1. There is no ω = 0 branch and no cancellation at small angles. - LogRotation takes the axis from the symmetric part of R for every φ > π/2, not only within ε of π. Exp∘Log then round-trips to ≈6e-7 for all φ ≤ π; the near-π-only switch left 3.5e-5 errors at φ ≈ π − 2e-3. Exp matches a double-precision 4×4 matrix exponential to ≈6e-7 on 3000 random twists. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 2 +- ROADMAP.md | 13 -- doc/math/README.md | 1 + doc/math/SE3Transform.md | 168 ++++++++++++++ numerical/math/CMakeLists.txt | 2 + numerical/math/SE3Transform.cpp | 6 + numerical/math/SE3Transform.hpp | 233 ++++++++++++++++++++ numerical/math/test/CMakeLists.txt | 1 + numerical/math/test/TestSE3Transform.cpp | 139 ++++++++++++ roadmap/README.md | 4 +- roadmap/math/SE3Transform/explanation.md | 38 ---- roadmap/math/SE3Transform/implementation.md | 122 ---------- roadmap/math/SE3Transform/tests.md | 57 ----- 13 files changed, 553 insertions(+), 233 deletions(-) create mode 100644 doc/math/SE3Transform.md create mode 100644 numerical/math/SE3Transform.cpp create mode 100644 numerical/math/SE3Transform.hpp create mode 100644 numerical/math/test/TestSE3Transform.cpp delete mode 100644 roadmap/math/SE3Transform/explanation.md delete mode 100644 roadmap/math/SE3Transform/implementation.md delete mode 100644 roadmap/math/SE3Transform/tests.md diff --git a/README.md b/README.md index bbc3e47d..30d48027 100644 --- a/README.md +++ b/README.md @@ -23,7 +23,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal | [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, 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 | | [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 | +| [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) | | [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet), Damped Pseudo-Inverse + Null-Space Projector | | [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | diff --git a/ROADMAP.md b/ROADMAP.md index b60bdc8d..a9ade851 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -26,7 +26,6 @@ Difficulty legend: | # | Component | Target module | Difficulty | |----|-------------------------------------------------------|-------------------------|------------| -| 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ | | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | | 64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | ★★★☆☆ | @@ -50,18 +49,6 @@ the library does not yet expose. Detailed below under ## Tier 2 — Easy ★★☆☆☆ -### 53. SE(3) transform, twists and wrenches -- **What:** Rigid transform (rotation + translation) with closed-form inverse, adjoint, twist/wrench - transforms, exponential/logarithm (SO(3) and SE(3)) and a log-map pose error. 6-vectors are ordered - linear part first: twists `(v; ω)`, wrenches `(f; n)`. -- **Embedded value:** The algebra every pose-level consumer shares — robot kinematics, Jacobians and - task-space control in robotics-toolbox-cpp, drone attitude/position control, visual odometry. One - type and one ordering convention prevent silent block-order mismatches between modules. -- **Algorithm / paper:** K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; R. Murray, Z. Li, - S. Sastry (1994), Ch. 2; J. Solà et al., "A micro Lie theory for state estimation in robotics" (2018). -- **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`. -- **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md). - ### 66. SE(2) transform - **What:** Planar rigid transform `(R(θ), p)` with composition, closed-form inverse, adjoint, exp/log of planar twists `(vx, vy, ω)` and a pose error, ordered linear part first like item 53. diff --git a/doc/math/README.md b/doc/math/README.md index c088a1f0..8388129b 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -20,3 +20,4 @@ Core mathematical primitives for numerical computation. | [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | | [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | | [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | +| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) | diff --git a/doc/math/SE3Transform.md b/doc/math/SE3Transform.md new file mode 100644 index 00000000..8a0a83e1 --- /dev/null +++ b/doc/math/SE3Transform.md @@ -0,0 +1,168 @@ +# SE(3) Transform, Twists and Wrenches + +## Overview & Motivation + +Every pose-level algorithm — manipulator kinematics and Jacobians, pose inverse kinematics, Cartesian interpolation, task-space control, drone navigation, visual odometry — manipulates the same few objects: a rigid transform, a velocity-like 6-vector (a *twist*), a force-like 6-vector (a *wrench*), and the maps between a constant twist and the finite motion it produces. Defining them once, with one ordering convention, prevents the silent sign and ordering mismatches that appear when independently written modules exchange 6-vectors. + +A transform is a rotation plus a translation: twelve scalars, composed with a few $3\times3$ products and inverted in closed form, with no matrix solve. A twist is re-expressed in another frame by the *adjoint*: rotate both parts, then add the lever-arm effect of the frame offset to the linear part. Wrenches follow the dual rule so that power is the same in every frame. The exponential turns "rotate about a line while sliding along it" into a finite motion; the logarithm recovers that motion, which is exactly what pose-error feedback needs. + +## Mathematical Theory + +### Rigid Transform + +A transform $T = (R, p)$ with $R \in SO(3)$ ($R^T R = I$, $\det R = +1$) and $p \in \mathbb{R}^3$ maps coordinates in frame B to frame A, $x_A = R\,x_B + p$. Its homogeneous form is + +$$T = \begin{pmatrix} R & p \\ 0 & 1 \end{pmatrix}.$$ + +$$T_1 T_2 = (R_1 R_2,\; R_1 p_2 + p_1), \qquad T^{-1} = (R^T,\; -R^T p), \qquad T\cdot x = R x + p.$$ + +The composition applies $T_2$ first. The inverse follows from $R^{-1} = R^T$. A free direction is only rotated, $x \mapsto R x$; the translation is ignored. + +### Ordering Convention (normative) + +All 6-vectors are ordered **linear part first**: twists $V = (v;\, \omega)$ and wrenches $W = (f;\, n)$. This matches a geometric Jacobian with linear rows first and the tangent ordering of Sophus and manif. Lynch & Park and Murray et al. use $(\omega;\, v)$; the two conventions differ by a block permutation, so any formula from those texts converts by swapping the blocks (for the adjoint below, the rotation blocks stay on the diagonal and the $[p]_\times R$ block moves to the lower left). + +### Adjoint, Twists and Wrenches + +Let $[a]_\times$ be the skew matrix with $[a]_\times b = a \times b$. A twist expressed in frame B is re-expressed in frame A by + +$$V_A = \mathrm{Ad}_T V_B, \qquad \mathrm{Ad}_T = \begin{pmatrix} R & [p]_\times R \\ 0 & R \end{pmatrix}, \qquad \text{i.e. } v' = R v + p \times R\omega,\;\; \omega' = R\omega.$$ + +The wrench map is the power-consistent dual, $\mathrm{Ad}_T^{-T}$: + +$$f' = R f, \qquad n' = R n + p \times R f.$$ + +Power is frame invariant because the cross terms cancel by the antisymmetry of the scalar triple product: + +$$W' \cdot V' = f \cdot v + n \cdot \omega + \underbrace{Rf \cdot (p \times R\omega) + (p \times Rf) \cdot R\omega}_{=\,0} = W \cdot V.$$ + +The adjoint is a group homomorphism, $\mathrm{Ad}_{AB} = \mathrm{Ad}_A\,\mathrm{Ad}_B$, and $\mathrm{Ad}_{T^{-1}} = \mathrm{Ad}_T^{-1}$. + +### Exponential + +A twist $\xi = (v;\, \omega)$ held for $\theta$ produces the finite motion $e^{[\xi]\theta}$ with $[\xi] = \begin{pmatrix} [\omega]_\times & v \\ 0 & 0 \end{pmatrix}$. It has the block form $\begin{pmatrix} e^{K\theta} & G v \\ 0 & 1 \end{pmatrix}$ with $K = [\omega]_\times$ and $G = \int_0^\theta e^{Ks}\,ds$: Rodrigues' formula for the rotation, integrated term by term for the translation. Without normalising $\omega$, and with $\varphi = \theta\,\|\omega\|$, + +$$R = I + \theta\,a_1\,K + \theta^2 a_2\,K^2, \qquad p = \theta v + \theta^2 a_2\,K v + \theta^3 a_3\,K^2 v,$$ + +$$a_1 = \frac{\sin\varphi}{\varphi}, \qquad a_2 = \frac{1-\cos\varphi}{\varphi^2} = \frac{2\sin^2(\varphi/2)}{\varphi^2}, \qquad a_3 = \frac{\varphi - \sin\varphi}{\varphi^3}.$$ + +Below $\varphi = 0.1$ the coefficients switch to short Taylor series, + +$$a_1 \approx 1 - \tfrac{\varphi^2}{6} + \tfrac{\varphi^4}{120}, \qquad a_2 \approx \tfrac12 - \tfrac{\varphi^2}{24} + \tfrac{\varphi^4}{720}, \qquad a_3 \approx \tfrac16 - \tfrac{\varphi^2}{120} + \tfrac{\varphi^4}{5040},$$ + +so there is no singular branch at $\omega = 0$ (pure translation falls out as $R = I$, $p = \theta v$) and no $0/0$ or cancellation for small $\varphi$. Because $\theta$ scales the whole twist, a non-unit $\omega$ needs no normalisation. + +Re-expressing a twist in another frame generates the same motion conjugated by that frame: + +$$\mathrm{Exp}(\mathrm{Ad}_A\,\xi,\, t) = A\,\mathrm{Exp}(\xi,\, t)\,A^{-1}.$$ + +### Logarithm of a Rotation + +Let $s = \tfrac12\,\mathrm{vee}(R - R^T) = \sin\varphi\,\hat\omega$ and $\cos\varphi = \tfrac12(\operatorname{tr}R - 1)$. The angle is + +$$\varphi = \mathrm{atan2}\big(\|s\|,\; \cos\varphi\big),$$ + +which is accurate near $0$ and $\pi$, unlike $\arccos$. + +- **$\varphi \le \pi/2$:** the rotation vector is $s\,\varphi/\sin\varphi$ (zero when $R = I$). +- **$\varphi > \pi/2$:** $s/\sin\varphi$ would amplify noise as $\sin\varphi \to 0$, so the axis comes from the symmetric part, $\tfrac12(R + R^T) = \cos\varphi\, I + (1-\cos\varphi)\,\hat\omega\hat\omega^T$: + +$$\hat\omega_i \hat\omega_j = \frac{\tfrac12(R_{ij} + R_{ji}) - \delta_{ij}\cos\varphi}{1 - \cos\varphi}.$$ + +The pivot component $i$ is the largest diagonal entry, so $\hat\omega_i^2 \ge \tfrac13$ and the divisions are benign; $\hat\omega_i = \sqrt{\hat\omega_i^2}$, the other components follow from the off-diagonal products, and the sign is taken from $s_i$. At exactly $\pi$ either sign is valid. The result satisfies $\|\omega\| \le \pi$: rotations are re-expressed the short way. + +### Logarithm of a Transform + +With $\omega = \mathrm{Log}(R)$ and $\varphi = \|\omega\|$, the inverse of the integral $G$ is + +$$v = \Big(I - \tfrac12[\omega]_\times + c(\varphi)\,[\omega]_\times^2\Big)\,p, \qquad c(\varphi) = \frac{1 - \tfrac{\varphi}{2}\cot\tfrac{\varphi}{2}}{\varphi^2} \approx \tfrac1{12} + \tfrac{\varphi^2}{720} + \tfrac{\varphi^4}{30240} \;\;(\varphi < 0.1).$$ + +The result is $(v;\, \omega)$ with $\theta = 1$, so $\mathrm{Exp}(\mathrm{Log}(T), 1) = T$. The coefficient stays finite on $[0, \pi]$, reaching $1/\pi^2$ at $\varphi = \pi$. + +### Pose Error + +With target $(R_t, p_t)$ and current $(R_c, p_c)$, the base-frame error is + +$$e = \big(p_t - p_c;\;\; \mathrm{Log}(R_t R_c^T)\big),$$ + +the translation difference stacked on the rotation vector that carries the current orientation onto the target. It has the linear-first ordering of a geometric Jacobian, and $\|e_\omega\| \le \pi$ always points the short way round. + +## Complexity Analysis + +| Operation | Time | Space | Notes | +|-----------|------|-------|-------| +| Compose | O(1) | O(1) | one $3\times3$ product, one matrix–vector product | +| Inverse | O(1) | O(1) | transpose and one matrix–vector product; no solve | +| Apply / rotate | O(1) | O(1) | one matrix–vector product (plus 3 additions for apply) | +| Twist / wrench transform | O(1) | O(1) | two matrix–vector products and one cross product, no $6\times6$ formed | +| Adjoint | O(1) | O(1) | $36$ scalars, one extra $3\times3$ product; built only on request | +| Exp | O(1) | O(1) | two sines of $\varphi$ and $\varphi/2$, none below $\varphi = 0.1$ | +| Log | O(1) | O(1) | one atan2; a half-angle sine/cosine pair above $\varphi = 0.1$; one square root for large angles | + +A transform holds $12$ scalars; the $6\times6$ adjoint appears only when requested. All operations are stack-only: no heap, no recursion, constant work per call. + +## Step-by-Step Walkthrough + +**Apply.** $R = R_z(\pi/2)$, $p = (1,0,0)$: $R\,(1,0,0) = (0,1,0)$, plus $p$ gives $(1,1,0)$. + +**Exp of a screw.** Rotation about $\hat z$ through the point $(1,0,0)$: $\omega = \hat z$, $v = -\hat z \times (1,0,0) = (0,-1,0)$, $\theta = \pi/2$. + +| Step | Value | +|------|-------| +| $\varphi = \theta\|\omega\|$ | $\pi/2 \ge 0.1$, direct forms | +| Coefficients | $a_1 = 2/\pi$, $a_2 = 4/\pi^2$, $a_3 = (\pi/2 - 1)/(\pi/2)^3$ | +| Vectors | $Kv = \hat z \times v = (1,0,0)$, $K^2 v = (0,1,0)$ | +| $R$ | $\theta a_1 = 1$, $\theta^2 a_2 = 1$: $R = I + K + K^2 = R_z(\pi/2)$ | +| $p$ | $\tfrac{\pi}{2}(0,-1,0) + (1,0,0) + (\tfrac{\pi}{2} - 1)(0,1,0) = (1,-1,0)$ | + +Check: the origin sits at $(-1,0,0)$ relative to the axis; a quarter turn takes it to $(0,-1,0)$, and adding the axis point gives $(1,-1,0)$. + +**Log of that transform.** $s = (0,0,1)$, $\cos\varphi = 0$, so $\varphi = \pi/2$ and $\omega = (0,0,\pi/2)$. Then $\omega \times p = (\tfrac\pi2, \tfrac\pi2, 0)$, $\omega \times (\omega \times p) = (-\tfrac{\pi^2}{4}, \tfrac{\pi^2}{4}, 0)$, and $c(\pi/2)\,\pi^2/4 = 1 - \pi/4$, so + +$$v = (1 - \tfrac\pi4 - (1 - \tfrac\pi4),\; -1 - \tfrac\pi4 + (1 - \tfrac\pi4),\; 0) = (0, -\tfrac\pi2, 0).$$ + +The result $\big((0,-\tfrac\pi2,0);\,(0,0,\tfrac\pi2)\big)$ is $\theta\xi$, as expected. + +**Pose error, short way round.** Target rotated $+190^\circ$ about $\hat z$ relative to the current pose: $\cos\varphi = \cos 190^\circ = -0.9848$, $\|s\| = \sin 170^\circ = 0.1736$, so $\varphi = 170^\circ = 2.967$ rad $> \pi/2$. The symmetric part gives $\hat\omega_z^2 = (1 + 0.9848)/1.9848 = 1$, and $s_z = \sin 190^\circ < 0$ selects $-\hat z$. The rotation error is $-170^\circ\cdot\hat z$, not $+190^\circ$. + +## Pitfalls & Edge Cases + +- **Ordering mismatches.** Formulas from texts using $(\omega;\, v)$ must have their blocks swapped; applying the twist adjoint to a wrench breaks power invariance, since wrenches need the dual map. +- **Drift off SO(3).** Long chains of products in single precision accumulate error in $R$. Re-orthonormalise a transform that is integrated over time, not after every single composition. +- **Non-orthonormal input to Log.** The trace and antisymmetric-part formulas assume $R \in SO(3)$; a rotation far from orthonormal gives a meaningless angle. +- **Small angles.** $\sin\varphi/\varphi$, $(1-\cos\varphi)/\varphi^2$ and $(\varphi-\sin\varphi)/\varphi^3$ are $0/0$ or lose all digits near zero; the series below $\varphi = 0.1$ remove both problems with no special case at $\omega = 0$. +- **Angles near $\pi$.** The antisymmetric part vanishes, so $s/\sin\varphi$ is unusable; the symmetric-part axis is used beyond $\pi/2$. At exactly $\pi$ the sign is arbitrary. The rotation vector is discontinuous across $\pi$ because $\pm\pi\hat\omega$ describe the same rotation. +- **Wrong logarithm.** Twice the vector part of a unit quaternion is $2\sin(\varphi/2)\,\hat\omega$, only a small-angle approximation of $\varphi\,\hat\omega$. +- **No general inverse.** Never invert the $4\times4$ matrix; the closed form is exact and cheaper. +- **Measured accuracy.** On $3000$ random twists, including $\varphi \approx 10^{-4}$ and $\varphi \in [3, 3.1405]$, Exp agrees with a double-precision $4\times4$ matrix exponential to $\approx 6\times10^{-7}$, and the Exp–Log round trip is accurate to $\approx 6\times10^{-7}$. + +## Variants & Generalizations + +| Variant | Key difference | +|---------|----------------| +| **Angular-first ordering** $(\omega;\, v)$ | Same algebra with blocks permuted; used by Lynch & Park and Murray et al. | +| **Body vs. spatial twist** | Body twists are $T^{-1}\dot T$, spatial twists $\dot T\,T^{-1}$; related by the adjoint | +| **Quaternion rotation part** | Four scalars and cheap renormalisation; the rotation vector is the vector part scaled by $\varphi/\sin(\varphi/2)$ | +| **Dual quaternions** | Rotation and translation in one algebra; screw interpolation | +| **Similarity group** | Adds a uniform scale for monocular visual odometry | + +## Applications + +- **Manipulator kinematics** — forward kinematics as a product of exponentials; Jacobian columns are adjoint-transformed joint twists. +- **Pose inverse kinematics and task-space control** — the pose error drives a Jacobian-based update with consistent ordering. +- **Cartesian interpolation** — Exp of a scaled Log of the relative transform gives a constant-twist screw path. +- **Force and torque sensing** — moving a measured wrench between sensor, tool and base frames with power preserved. +- **Navigation and odometry** — integrating body twists into poses on drones and in visual odometry. + +## Connections to Other Algorithms + +- **Quaternion** — an alternative rotation representation; renormalisation there is the cheap fix for drift, and its vector part is not the logarithm. +- **Angle Utilities** — the shortest signed angle difference is the one-dimensional analogue of the short-way rotation error: $+190^\circ$ becomes $-170^\circ$. +- **Givens Rotation** — a plane rotation embedded in a coordinate plane is the single-axis special case of the rotation block. +- **Matrix Exponential** — the general matrix exponential of the $4\times4$ twist matrix is the closed form's reference; it is used only for verification, not at run time. + +## References & Further Reading + +- K. M. Lynch and F. C. Park, *Modern Robotics: Mechanics, Planning, and Control*, Cambridge University Press, 2017 — Chapter 3. +- R. M. Murray, Z. Li, and S. S. Sastry, *A Mathematical Introduction to Robotic Manipulation*, CRC Press, 1994 — Chapter 2. +- J. Solà, J. Deray, and D. Atchuthan, "A micro Lie theory for state estimation in robotics," arXiv:1812.01537, 2018. diff --git a/numerical/math/CMakeLists.txt b/numerical/math/CMakeLists.txt index 388487f3..a48155fe 100644 --- a/numerical/math/CMakeLists.txt +++ b/numerical/math/CMakeLists.txt @@ -34,6 +34,7 @@ target_sources(numerical.math PRIVATE QNumber.hpp Quaternion.hpp RecursiveBuffer.hpp + SE3Transform.hpp Statistics.hpp StepResponseMetrics.hpp Toeplitz.hpp @@ -56,6 +57,7 @@ numerical_add_coverage_sources(numerical.math QNumber.cpp Quaternion.cpp RecursiveBuffer.cpp + SE3Transform.cpp Toeplitz.cpp TridiagonalSolver.cpp ) diff --git a/numerical/math/SE3Transform.cpp b/numerical/math/SE3Transform.cpp new file mode 100644 index 00000000..1b21118f --- /dev/null +++ b/numerical/math/SE3Transform.cpp @@ -0,0 +1,6 @@ +#include "numerical/math/SE3Transform.hpp" + +namespace math +{ + template struct SE3Transform; +} diff --git a/numerical/math/SE3Transform.hpp b/numerical/math/SE3Transform.hpp new file mode 100644 index 00000000..d76d35ec --- /dev/null +++ b/numerical/math/SE3Transform.hpp @@ -0,0 +1,233 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Geometry3D.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include +#include +#include +#include + +namespace math +{ + template + using Vector6 = Vector; + + template + using Matrix6 = SquareMatrix; + + template + struct SE3Transform + { + static_assert(std::is_floating_point_v, "SE3Transform supports floating-point types only"); + + Matrix3 rotation{ Matrix3::Identity() }; + Vector3 translation{}; + + static SE3Transform Identity(); + OPTIMIZE_FOR_SPEED SE3Transform operator*(const SE3Transform& rhs) const; + SE3Transform Inverse() const; + OPTIMIZE_FOR_SPEED Vector3 Apply(const Vector3& point) const; + Vector3 Rotate(const Vector3& direction) const; + SquareMatrix Homogeneous() const; + Matrix6 Adjoint() const; + Vector6 TransformTwist(const Vector6& twist) const; + Vector6 TransformWrench(const Vector6& wrench) const; + + OPTIMIZE_FOR_SPEED static SE3Transform Exp(const Vector6& twist, T theta); + Vector6 Log() const; + static Vector3 LogRotation(const Matrix3& r); + static Vector6 PoseError(const SE3Transform& target, const SE3Transform& current); + + private: + static constexpr T seriesThreshold{ T{ 0.1f } }; + + static Vector3 Linear(const Vector6& v); + static Vector3 Angular(const Vector6& v); + static Vector6 Stack(const Vector3& linear, const Vector3& angular); + static Vector3 AxisFromSymmetricPart(const Matrix3& r, T cosPhi, const Vector3& sinAxis); + }; + + template + SE3Transform SE3Transform::Identity() + { + return {}; + } + + template + OPTIMIZE_FOR_SPEED SE3Transform SE3Transform::operator*(const SE3Transform& rhs) const + { + return { rotation * rhs.rotation, rotation * rhs.translation + translation }; + } + + template + SE3Transform SE3Transform::Inverse() const + { + const Matrix3 transposed{ rotation.Transpose() }; + return { transposed, transposed * translation * T{ -1 } }; + } + + template + OPTIMIZE_FOR_SPEED Vector3 SE3Transform::Apply(const Vector3& point) const + { + return rotation * point + translation; + } + + template + Vector3 SE3Transform::Rotate(const Vector3& direction) const + { + return rotation * direction; + } + + template + SquareMatrix SE3Transform::Homogeneous() const + { + SquareMatrix homogeneous{}; + homogeneous.SetBlock(rotation, 0, 0); + homogeneous.SetBlock(translation, 0, 3); + homogeneous.at(3, 3) = T{ 1 }; + return homogeneous; + } + + template + Matrix6 SE3Transform::Adjoint() const + { + Matrix6 adjoint{}; + adjoint.SetBlock(rotation, 0, 0); + adjoint.SetBlock(SkewSymmetric(translation) * rotation, 0, 3); + adjoint.SetBlock(rotation, 3, 3); + return adjoint; + } + + template + Vector6 SE3Transform::TransformTwist(const Vector6& twist) const + { + const Vector3 angular{ rotation * Angular(twist) }; + return Stack(rotation * Linear(twist) + CrossProduct(translation, angular), angular); + } + + template + Vector6 SE3Transform::TransformWrench(const Vector6& wrench) const + { + const Vector3 force{ rotation * Linear(wrench) }; + return Stack(force, rotation * Angular(wrench) + CrossProduct(translation, force)); + } + + template + OPTIMIZE_FOR_SPEED SE3Transform SE3Transform::Exp(const Vector6& twist, T theta) + { + const Vector3 v{ Linear(twist) }; + const Vector3 omega{ Angular(twist) }; + const T phi{ theta * VectorNorm(omega) }; + const T phi2{ phi * phi }; + + T sinc{ T{ 1 } - phi2 / T{ 6 } + phi2 * phi2 / T{ 120 } }; + T halfVersine{ T{ 0.5f } - phi2 / T{ 24 } + phi2 * phi2 / T{ 720 } }; + T cubic{ T{ 1 } / T{ 6 } - phi2 / T{ 120 } + phi2 * phi2 / T{ 5040 } }; + if (phi >= seriesThreshold) + { + const T halfSin{ math::Sin(phi / T{ 2 }) }; + sinc = math::Sin(phi) / phi; + halfVersine = T{ 2 } * halfSin * halfSin / phi2; + cubic = (phi - math::Sin(phi)) / (phi2 * phi); + } + + const Matrix3 k{ SkewSymmetric(omega) }; + const Matrix3 k2{ k * k }; + const Vector3 kv{ k * v }; + const Vector3 k2v{ k * kv }; + + return { Matrix3::Identity() + k * (theta * sinc) + k2 * (theta * theta * halfVersine), + v * theta + kv * (theta * theta * halfVersine) + k2v * (theta * theta * theta * cubic) }; + } + + template + Vector6 SE3Transform::Log() const + { + const Vector3 omega{ LogRotation(rotation) }; + const T phi{ VectorNorm(omega) }; + const T phi2{ phi * phi }; + + T coefficient{ T{ 1 } / T{ 12 } + phi2 / T{ 720 } + phi2 * phi2 / T{ 30240 } }; + if (phi >= seriesThreshold) + { + const T half{ phi / T{ 2 } }; + coefficient = (T{ 1 } - half * math::Cos(half) / math::Sin(half)) / phi2; + } + + const Vector3 kp{ CrossProduct(omega, translation) }; + return Stack(translation - kp * T{ 0.5f } + CrossProduct(omega, kp) * coefficient, omega); + } + + template + Vector3 SE3Transform::LogRotation(const Matrix3& r) + { + const Vector3 sinAxis{ (r.at(2, 1) - r.at(1, 2)) / T{ 2 }, (r.at(0, 2) - r.at(2, 0)) / T{ 2 }, (r.at(1, 0) - r.at(0, 1)) / T{ 2 } }; + const T sinPhi{ VectorNorm(sinAxis) }; + const T cosPhi{ (r.Trace() - T{ 1 }) / T{ 2 } }; + const T phi{ math::Atan2(sinPhi, cosPhi) }; + + if (cosPhi < T{}) + return AxisFromSymmetricPart(r, cosPhi, sinAxis) * phi; + + if (sinPhi > T{}) + return sinAxis * (phi / sinPhi); + + return Vector3{}; + } + + template + Vector6 SE3Transform::PoseError(const SE3Transform& target, const SE3Transform& current) + { + return Stack(target.translation - current.translation, LogRotation(target.rotation * current.rotation.Transpose())); + } + + template + Vector3 SE3Transform::Linear(const Vector6& v) + { + return v.template GetBlock<3, 1>(0, 0); + } + + template + Vector3 SE3Transform::Angular(const Vector6& v) + { + return v.template GetBlock<3, 1>(3, 0); + } + + template + Vector6 SE3Transform::Stack(const Vector3& linear, const Vector3& angular) + { + Vector6 stacked{}; + stacked.SetBlock(linear, 0, 0); + stacked.SetBlock(angular, 3, 0); + return stacked; + } + + template + Vector3 SE3Transform::AxisFromSymmetricPart(const Matrix3& r, T cosPhi, const Vector3& sinAxis) + { + const T versine{ T{ 1 } - cosPhi }; + + std::size_t i{ 0 }; + for (std::size_t k = 1; k < 3; ++k) + if (r.at(k, k) > r.at(i, i)) + i = k; + + Vector3 axis{}; + axis[i] = math::Sqrt(std::max((r.at(i, i) - cosPhi) / versine, T{})); + for (std::size_t k = 0; k < 3; ++k) + if (k != i) + axis[k] = (r.at(i, k) + r.at(k, i)) / (T{ 2 } * versine * axis[i]); + + if (sinAxis[i] < T{}) + axis *= T{ -1 }; + + const T norm{ VectorNorm(axis) }; + return axis * (T{ 1 } / norm); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template struct SE3Transform; +#endif +} diff --git a/numerical/math/test/CMakeLists.txt b/numerical/math/test/CMakeLists.txt index d0e0e44b..e100ccf4 100644 --- a/numerical/math/test/CMakeLists.txt +++ b/numerical/math/test/CMakeLists.txt @@ -30,6 +30,7 @@ target_sources(numerical.math_test PRIVATE TestMatrix.cpp TestQuaternion.cpp TestRecursiveBuffer.cpp + TestSE3Transform.cpp TestStatistics.cpp TestStepResponseMetrics.cpp TestToeplitz.cpp diff --git a/numerical/math/test/TestSE3Transform.cpp b/numerical/math/test/TestSE3Transform.cpp new file mode 100644 index 00000000..5e19fe60 --- /dev/null +++ b/numerical/math/test/TestSE3Transform.cpp @@ -0,0 +1,139 @@ +#include "numerical/math/SE3Transform.hpp" +#include +#include +#include +#include + +namespace +{ + using Transform = math::SE3Transform; + using Vector3 = math::Vector3; + using Vector6 = math::Vector6; + + class TestSE3Transform : public ::testing::Test + { + protected: + static constexpr float pi{ std::numbers::pi_v }; + static constexpr float tolerance{ 1e-5f }; + const Vector3 xAxis{ 1.0f, 0.0f, 0.0f }; + const Vector3 yAxis{ 0.0f, 1.0f, 0.0f }; + const Vector3 zAxis{ 0.0f, 0.0f, 1.0f }; + const Transform a{ math::RotationAboutAxis(zAxis, pi / 2.0f), Vector3{ 1.0f, 0.0f, 0.0f } }; + const Transform b{ math::RotationAboutAxis(yAxis, pi / 3.0f), Vector3{ 0.0f, 2.0f, 0.0f } }; + const Vector6 twist{ 0.3f, -0.1f, 0.2f, 0.5f, 0.4f, -0.3f }; + + template + static void ExpectNear(const math::Matrix& actual, const math::Matrix& expected, float eps = tolerance) + { + for (std::size_t r = 0; r < Rows; ++r) + for (std::size_t c = 0; c < Cols; ++c) + EXPECT_NEAR(actual.at(r, c), expected.at(r, c), eps) << "(" << r << ", " << c << ")"; + } + + static void ExpectNear(const Transform& actual, const Transform& expected, float eps = tolerance) + { + ExpectNear(actual.rotation, expected.rotation, eps); + ExpectNear(actual.translation, expected.translation, eps); + } + + static Vector3 UnitAxis(float x, float y, float z) + { + const float norm{ std::sqrt(x * x + y * y + z * z) }; + return Vector3{ x / norm, y / norm, z / norm }; + } + + static float Dot(const Vector6& lhs, const Vector6& rhs) + { + float dot{ 0.0f }; + for (std::size_t i = 0; i < 6; ++i) + dot += lhs[i] * rhs[i]; + + return dot; + } + }; +} + +TEST_F(TestSE3Transform, apply_rotates_then_translates) +{ + ExpectNear(a.Apply(xAxis), Vector3{ 1.0f, 1.0f, 0.0f }); +} + +TEST_F(TestSE3Transform, composition_applies_right_operand_first) +{ + const Vector3 x{ 0.3f, -0.2f, 0.5f }; + + ExpectNear((a * b).Apply(x), a.Apply(b.Apply(x))); +} + +TEST_F(TestSE3Transform, inverse_composes_to_identity) +{ + ExpectNear(a * a.Inverse(), Transform::Identity()); + ExpectNear(a.Inverse() * a, Transform::Identity()); +} + +TEST_F(TestSE3Transform, homogeneous_matches_block_form) +{ + const auto h{ a.Homogeneous() }; + + ExpectNear(h.GetBlock<3, 3>(0, 0), a.rotation, 0.0f); + ExpectNear(h.GetBlock<3, 1>(0, 3), a.translation, 0.0f); + ExpectNear(h.GetBlock<1, 4>(3, 0), math::Matrix{ 0.0f, 0.0f, 0.0f, 1.0f }, 0.0f); +} + +TEST_F(TestSE3Transform, adjoint_maps_twists_consistently_with_composition) +{ + ExpectNear(Transform::Exp(a.TransformTwist(twist), 0.4f), a * Transform::Exp(twist, 0.4f) * a.Inverse()); + ExpectNear(a.TransformTwist(twist), Vector6{ a.Adjoint() * twist }); +} + +TEST_F(TestSE3Transform, adjoint_of_product_is_product_of_adjoints) +{ + ExpectNear((a * b).Adjoint(), a.Adjoint() * b.Adjoint()); +} + +TEST_F(TestSE3Transform, wrench_and_twist_power_is_frame_invariant) +{ + const Vector6 wrench{ 1.0f, -2.0f, 0.5f, 0.3f, 0.7f, -0.4f }; + + EXPECT_NEAR(Dot(a.TransformWrench(wrench), a.TransformTwist(twist)), Dot(wrench, twist), tolerance); +} + +TEST_F(TestSE3Transform, exp_of_pure_rotation_about_offset_axis) +{ + const Transform screw{ Transform::Exp(Vector6{ 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 1.0f }, pi / 2.0f) }; + + ExpectNear(screw.translation, Vector3{ 1.0f, -1.0f, 0.0f }); + ExpectNear(screw.rotation, math::RotationAboutAxis(zAxis, pi / 2.0f)); +} + +TEST_F(TestSE3Transform, exp_of_pure_translation) +{ + ExpectNear(Transform::Exp(Vector6{ 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }, 0.3f), Transform{ Transform::Identity().rotation, Vector3{ 0.3f, 0.0f, 0.0f } }); +} + +TEST_F(TestSE3Transform, log_inverts_exp) +{ + const Transform nearHalfTurn{ math::RotationAboutAxis(UnitAxis(1.0f, -2.0f, 0.5f), 179.9f * pi / 180.0f), Vector3{ 0.5f, -1.0f, 2.0f } }; + + ExpectNear(Transform::Exp(a.Log(), 1.0f), a); + ExpectNear(Transform::Exp(b.Log(), 1.0f), b); + ExpectNear(Transform::Exp(nearHalfTurn.Log(), 1.0f), nearHalfTurn, 1e-4f); +} + +TEST_F(TestSE3Transform, log_near_zero_and_near_pi_stays_finite) +{ + const Vector3 axis{ UnitAxis(0.2f, 0.3f, -0.9f) }; + + ExpectNear(Transform::LogRotation(math::RotationAboutAxis(axis, 1e-6f)), axis * 1e-6f, 1e-10f); + ExpectNear(Transform::LogRotation(math::RotationAboutAxis(axis, pi - 1e-4f)), axis * (pi - 1e-4f), 1e-3f); +} + +TEST_F(TestSE3Transform, pose_error_takes_the_short_way) +{ + const Transform target{ math::RotationAboutAxis(zAxis, 190.0f * pi / 180.0f) * b.rotation, b.translation }; + + const Vector6 error{ Transform::PoseError(target, b) }; + + ExpectNear(error.GetBlock<3, 1>(0, 0), Vector3{}); + ExpectNear(error.GetBlock<3, 1>(3, 0), Vector3{ 0.0f, 0.0f, -170.0f * pi / 180.0f }, 1e-4f); +} diff --git a/roadmap/README.md b/roadmap/README.md index 4fdf37ba..a6cd9782 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -20,7 +20,7 @@ All pseudocode respects the library constraints: **no heap**, bounded containers `template` interface that is validated and instantiated for **`float`** only (no `Q15`/`Q31`). The generic signature keeps a future fixed-point specialisation cheap to add. -The canonical worked example is [math/SE3Transform](math/SE3Transform/implementation.md). +For a deployed example of the artifact set a spec turns into, see [SE3Transform](../numerical/math/SE3Transform.hpp), its [test](../numerical/math/test/TestSE3Transform.cpp) and [doc page](../doc/math/SE3Transform.md). ## Deployment shape (float-only) @@ -102,7 +102,7 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). `LmsAdaptiveFilter` (21) ### `math` -`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) · `SE3Transform` (53) +`Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) ### `optimization` `LevenbergMarquardt` (64) diff --git a/roadmap/math/SE3Transform/explanation.md b/roadmap/math/SE3Transform/explanation.md deleted file mode 100644 index 5976239b..00000000 --- a/roadmap/math/SE3Transform/explanation.md +++ /dev/null @@ -1,38 +0,0 @@ -# SE(3) Transform, Twists and Wrenches — Overview - -## What it is -The algebra of rigid-body motion: a transform is a rotation plus a translation; a *twist* is a rigid -body's instantaneous velocity (linear and angular); a *wrench* is a force together with a moment. The -adjoint map moves twists and wrenches between frames, and the exponential/logarithm maps convert -between a constant twist held for some time and the finite motion it produces. - -## Why it matters (embedded) -Every pose-level algorithm — manipulator kinematics and Jacobians, pose inverse kinematics, Cartesian -interpolation, task-space control, drone attitude/position control, visual odometry — needs the same -small set of operations. Defining them once, with one ordering convention, prevents the silent sign -and ordering mismatches that otherwise appear when independently written modules exchange 6-vectors. - -## How it works (intuition) -A transform stores a 3×3 rotation and a 3-vector translation; composing two transforms rotates and -shifts the second by the first, and the inverse has the closed form `(Rᵀ, −Rᵀp)`. A twist written in one -frame is re-expressed in another with the adjoint: rotate both parts, then add the "lever-arm" effect -of the frame offset on the linear part. Wrenches transform with the dual rule so that power (force -times velocity) is the same in every frame. The exponential turns "rotate about this line while -sliding along it" into a finite motion — Rodrigues' formula for the rotation plus a matching integral -for the translation — and the logarithm recovers that motion, which is exactly what pose-error -feedback needs. - -## Key parameters -- **Ordering convention** — linear part first for twists `(v; ω)` and wrenches `(f; n)`. -- **Singular regions** — small angles and angles near 180° in the logarithm. -- **Orthonormality of `R`** — maintained on SO(3); re-orthonormalize against drift. - -## Reference -K. M. Lynch, F. C. Park, *Modern Robotics* (2017), Ch. 3; R. M. Murray, Z. Li, S. S. Sastry, -*A Mathematical Introduction to Robotic Manipulation* (1994), Ch. 2; J. Solà, J. Deray, D. Atchuthan, -"A micro Lie theory for state estimation in robotics," arXiv:1812.01537 (2018). (The first two use -`(ω; v)` ordering; this library uses `(v; ω)`, which only permutes the blocks.) - -## See also -`Quaternion` (the SO(3) rotation part), `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), -`MatrixExponential` (the general matrix analogue of `Exp`). diff --git a/roadmap/math/SE3Transform/implementation.md b/roadmap/math/SE3Transform/implementation.md deleted file mode 100644 index 589362b9..00000000 --- a/roadmap/math/SE3Transform/implementation.md +++ /dev/null @@ -1,122 +0,0 @@ -# SE(3) Transform, Twists and Wrenches — Implementation Pseudocode - -> Roadmap ref: #53 (Tier 2) · Target: `numerical/math` · Namespace `math` · Type: `float` (templated on `T`, instantiated for `float` only) - -Rigid-body transform plus the twist/wrench algebra that every pose-level consumer needs (robot -kinematics and control in robotics-toolbox-cpp, SLAM, drone navigation). It fixes the **one ordering -convention** shared by all consumers: 6-vectors are ordered **linear part first** — twists `(v; ω)`, -wrenches `(f; n)` — matching the geometric Jacobian with linear rows first and the tangent ordering of -Sophus and manif. Lynch & Park and Murray et al. use `(ω; v)`; the two only permute the blocks. - -## Data structures - -```cpp -template using Vector6 = math::Vector # twist (v; ω) or wrench (f; n) -template using Matrix6 = math::SquareMatrix - -template # static_assert(std::is_floating_point_v); instantiated for float -struct SE3Transform: - Matrix3 R # rotation, orthonormal, det = +1 - Vector3 p # translation - # equivalently the 4×4 homogeneous matrix [[R, p],[0 0 0 1]] -``` - -## Interface - -```text -static SE3Transform Identity() -SE3Transform operator*(const SE3Transform& rhs) const # composition; hot path -SE3Transform Inverse() const # closed form, no matrix solve -Vector3 Apply(const Vector3& point) const # R·x + p; hot path -Vector3 Rotate(const Vector3& direction) const # R·x -SquareMatrix Homogeneous() const -Matrix6 Adjoint() const # twist map for (v; ω) -Vector6 TransformTwist(const Vector6& twist) const # Adjoint()·twist -Vector6 TransformWrench(const Vector6& wrench) const # dual map, see below - -static SE3Transform Exp(const Vector6& twist, T theta) # e^{[ξ]θ}; hot path -Vector6 Log() const # inverse of Exp with theta = 1 -static Vector3 LogRotation(const Matrix3& R) # SO(3) log: rotation vector, |·| ≤ π -static Vector6 PoseError(const SE3Transform& target, const SE3Transform& current) -``` - -## Algorithm (pseudocode) - -```text -function operator*(rhs): return { R·rhs.R, R·rhs.p + p } -function Inverse(): return { Rᵀ, −Rᵀ·p } - -function Adjoint(): # V_A = Ad_T · V_B for V = (v; ω) - return [[ R, Skew(p)·R ], # reuse Geometry3D::SkewSymmetric - [ 0, R ]] - -function TransformWrench(w = (f; n)): # power-consistent dual of Adjoint - fA = R·f - nA = R·n + CrossProduct(p, R·f) - return (fA; nA) - -function Exp(ξ = (v; ω), θ): # OPTIMIZE_FOR_SPEED - if ‖ω‖ < ε: # pure translation - return { I, v·θ } - # general twist: fold the magnitude of ω into θ so the axis is unit - s = ‖ω‖; ω̂ = ω / s; v̂ = v / s; φ = θ·s - R = RotationAboutAxis(ω̂, φ) # reuse Geometry3D (Rodrigues) - G = I·φ + (1 − cos φ)·Skew(ω̂) + (φ − sin φ)·Skew(ω̂)² # left Jacobian · φ - return { R, G·v̂ } - -function LogRotation(R): # rotation vector ω̂·φ, φ ∈ [0, π] - φ = atan2(‖Vee(R − Rᵀ)‖/2, (trace(R) − 1)/2) # accurate near 0 and π, unlike acos - if φ < ε: # small angle: first-order - return Vee(R − Rᵀ) / 2 - if π − φ < ε: # sin φ → 0: use the diagonal form - ω̂ = axis from the largest of sqrt((R_ii + 1) / 2), signs from the off-diagonals - else: - ω̂ = Vee(R − Rᵀ) / (2 sin φ) - return ω̂·φ - -function Log(): # returns (v; ω) with ‖ω‖ = rotation angle - ω = LogRotation(R); φ = ‖ω‖ - if φ < ε: return (p; ω) - ω̂ = ω / φ - G⁻¹ = I/φ − Skew(ω̂)/2 + (1/φ − cot(φ/2)/2)·Skew(ω̂)² - return (G⁻¹·p·φ; ω) - -function PoseError(target, current): # base-frame error, matches a linear-rows-first Jacobian - eP = target.p − current.p - eR = LogRotation(target.R · current.Rᵀ) # |eR| ≤ π, short way round - return (eP; eR) -``` - -## Complexity & memory - -- Compose / inverse / apply / rotate: `O(1)` — a handful of 3×3 products. -- `Exp` / `Log`: `O(1)` — one `sin`/`cos` pair (plus `atan2` for `Log`). -- Memory: 12 scalars per transform; `Matrix6` only when an adjoint is requested. No heap. - -## Numerical / embedded notes - -- **Convention (normative):** twists `(v; ω)`, wrenches `(f; n)`; `PoseError` returns - `(position; rotation-vector)`. robotics-toolbox-cpp's geometric Jacobian, controllers and pose IK - depend on this ordering; changing it is a breaking change for that consumer. -- Use the **closed-form rigid inverse** `(Rᵀ, −Rᵀp)`; never run a general 4×4 solve. -- `Log` must be the true logarithm: `2·vec(quaternion)` equals `2 sin(φ/2)·ω̂`, which is only a - small-angle approximation of `φ·ω̂`. Rotations beyond `π` are re-expressed the short way, so `|eR| ≤ π`. -- Handle both singular regions of `Log` (`φ → 0` and `φ → π`) explicitly; derive `φ` with `atan2`, not - `acos`, which loses precision near ±1 in `float`. -- Composition drifts off SO(3) over long chains of products in `float`; re-orthonormalize (Gram–Schmidt - or via `Quaternion` normalization) when a transform is integrated over time, not after single - compositions. -- Float-only: `static_assert(std::is_floating_point_v)`; the generic `T` signature keeps a - `Q15`/`Q31` specialisation cheap to add later. - -## Deployment - -- Header: `numerical/math/SE3Transform.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on - `operator*`/`Apply`/`Exp`, and `extern template struct SE3Transform;` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/math/SE3Transform.cpp` → `template struct SE3Transform;` -- Test: `numerical/math/test/TestSE3Transform.cpp` -- Doc: `doc/math/SE3Transform.md` (per `doc/TEMPLATE.md`) + row in `doc/math/README.md` -- CMake: `.hpp` → `target_sources(numerical.math PRIVATE ...)`; `.cpp` → - `numerical_add_coverage_sources(numerical.math ...)`; `TestSE3Transform.cpp` → `numerical.math_test`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/math/SE3Transform/tests.md b/roadmap/math/SE3Transform/tests.md deleted file mode 100644 index a114eeaa..00000000 --- a/roadmap/math/SE3Transform/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# SE(3) Transform — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestSE3Transform : public ::testing::Test: - SE3Transform a{ RotationAboutAxis(ẑ, π/2), (1, 0, 0) } - SE3Transform b{ RotationAboutAxis(ŷ, π/3), (0, 2, 0) } -# each case below is a TEST_F(TestSE3Transform, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -apply_rotates_then_translates: - Assert: a.Apply((1, 0, 0)) ≈ (1, 1, 0) -composition_applies_right_operand_first: - Assert: (a * b).Apply(x) ≈ a.Apply(b.Apply(x)) for x = (0.3, −0.2, 0.5) -inverse_composes_to_identity: - Assert: a * a.Inverse() ≈ Identity and a.Inverse() * a ≈ Identity -homogeneous_matches_block_form: - Assert: a.Homogeneous() == [[R, p],[0, 0, 0, 1]] -adjoint_maps_twists_consistently_with_composition: - Arrange: twist V in the frame of a - Assert: Exp(a.TransformTwist(V), t) ≈ a * Exp(V, t) * a.Inverse() (t = 0.4) -adjoint_of_product_is_product_of_adjoints: - Assert: (a * b).Adjoint() ≈ a.Adjoint() · b.Adjoint() -wrench_and_twist_power_is_frame_invariant: - Arrange: twist V, wrench W in the same frame - Assert: a.TransformWrench(W) · a.TransformTwist(V) ≈ W · V -exp_of_pure_rotation_about_offset_axis: - Arrange: screw about ẑ through (1, 0, 0): ξ = (−ẑ × (1,0,0); ẑ) = ((0,−1,0); ẑ) - Assert: Exp(ξ, π/2).p ≈ (1, −1, 0) and R ≈ Rz(π/2) -exp_of_pure_translation: - Assert: Exp(((1,0,0); 0), 0.3) ≈ { I, (0.3, 0, 0) } -log_inverts_exp: - Assert: Exp(Log(T), 1) ≈ T for T = a, b, and a rotation of 179.9° -log_near_zero_and_near_pi_stays_finite: - Assert: LogRotation of rotations of 1e-6 rad and π − 1e-4 rad are finite with the expected angle -pose_error_takes_the_short_way: - Arrange: target = current rotated by +190° about ẑ - Assert: ‖PoseError(target, current).angular‖ ≈ 170° (2.967 rad), direction −ẑ -``` - -## Reference vectors - -- `Rz(π/2) ⊕ (1,0,0)` applied to `(1,0,0)` ⇒ `(1,1,0)`. -- Screw about `ẑ` through `(1,0,0)` by `π/2` maps the origin to `(1,−1,0)`. -- Rotation error of `+190°` about `ẑ` ⇒ rotation vector `−170°·ẑ`. - -## Edge cases - -- `Exp` with `ω = 0` (pure translation) and with non-unit `ω` (magnitude folded into the angle). -- `LogRotation` at `φ = π` exactly: either axis sign is valid; the test accepts both. -- Repeated composition in `float` keeps `RᵀR ≈ I` to `1e-5` after 1000 random products. From 019df56161ac2fd6fb044dac9f988e9c6024f625 Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:26:06 +0000 Subject: [PATCH 5/6] docs: align markdown tables Apply markdown-table-formatter's layout to the tables this tier adds or edits, as suggested by MegaLinter. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 28 +++++++++--------- doc/estimators/README.md | 14 ++++----- doc/math/CubicSpline.md | 34 +++++++++++----------- doc/math/README.md | 34 +++++++++++----------- doc/math/SE3Transform.md | 46 +++++++++++++++--------------- doc/solvers/DampedPseudoInverse.md | 18 ++++++------ doc/solvers/README.md | 2 +- 7 files changed, 88 insertions(+), 88 deletions(-) diff --git a/README.md b/README.md index 30d48027..b794fb35 100644 --- a/README.md +++ b/README.md @@ -14,21 +14,21 @@ Refer to the documentation to quickly integrate and utilize the library's signal ## Documentation -| Category | Description | -|--------------------------------------------------------------------|----------------------------------------------------------------------| -| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | -| [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, 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 | -| [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) | -| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | +| Category | Description | +|--------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC | +| [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, 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 | +| [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) | +| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile | | [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet), Damped Pseudo-Inverse + Null-Space Projector | -| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | -| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | -| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | +| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) | +| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control | +| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations | Each category page lists its algorithms with a brief description and links to the detailed documentation. diff --git a/doc/estimators/README.md b/doc/estimators/README.md index ff60692a..21faaf5b 100644 --- a/doc/estimators/README.md +++ b/doc/estimators/README.md @@ -4,13 +4,13 @@ Statistical estimation algorithms for fitting models to observed data and making ## Offline Estimators (Batch) -| Algorithm | Description | -|--------------------------------------------------------|---------------------------------------------------------------------------| -| [Linear Regression](LinearRegression.md) | Ordinary least-squares regression using the normal equation | -| [Polynomial Fitting](PolynomialFitting.md) | Degree-d polynomial fit via Vandermonde normal equations | -| [Total Least Squares](TotalLeastSquares.md) | Errors-in-variables fit (noisy regressors) via the SVD of `[A | b]` | -| [Yule-Walker](YuleWalker.md) | Autoregressive model parameter estimation via the Yule-Walker equations | -| [Expectation-Maximization](ExpectationMaximization.md) | EM algorithm for Kalman filter parameter identification (Shumway-Stoffer) | +| Algorithm | Description | +|--------------------------------------------------------------|---------------------------------------------------------------------------------------------------------------| +| [Linear Regression](LinearRegression.md) | Ordinary least-squares regression using the normal equation | +| [Polynomial Fitting](PolynomialFitting.md) | Degree-d polynomial fit via Vandermonde normal equations | +| [Total Least Squares](TotalLeastSquares.md) | Errors-in-variables fit (noisy regressors) via the SVD of `[A | b]` | +| [Yule-Walker](YuleWalker.md) | Autoregressive model parameter estimation via the Yule-Walker equations | +| [Expectation-Maximization](ExpectationMaximization.md) | EM algorithm for Kalman filter parameter identification (Shumway-Stoffer) | | [Incremental QR Least Squares](IncrementalQrLeastSquares.md) | Streaming Givens QR in fixed memory with a rank-revealing SVD solve (minimum-norm, identifiable combinations) | ## Online Estimators (Streaming) diff --git a/doc/math/CubicSpline.md b/doc/math/CubicSpline.md index 703f88b0..ef41cb6f 100644 --- a/doc/math/CubicSpline.md +++ b/doc/math/CubicSpline.md @@ -26,10 +26,10 @@ Each row couples a knot to its two neighbours only, so the system is tridiagonal ### End conditions -| Condition | First row | Last row | -|-----------|-----------|----------| +| Condition | First row | Last row | +|-------------------------------------------------|--------------------------------------------|----------------------------------------------------------------| | **Clamped** (end slopes $\sigma_0$, $\sigma_1$) | $2h_0 M_0 + h_0 M_1 = 6\,(s_0 - \sigma_0)$ | $h_{K-2} M_{K-2} + 2h_{K-2} M_{K-1} = 6\,(\sigma_1 - s_{K-2})$ | -| **Natural** | $M_0 = 0$ | $M_{K-1} = 0$ | +| **Natural** | $M_0 = 0$ | $M_{K-1} = 0$ | Clamping fixes the end slopes and leaves the end second derivatives free; natural ends do the reverse. Default slopes are zero. @@ -47,11 +47,11 @@ An abscissa exactly on an interior knot selects the interval starting there; the ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Factorization | $O(K)$ | three bands | Once per fit, shared by all channels | -| Fit | $O(D\,K)$ | $K_{\max}(2 + 2D)$ scalars | $D$ channels, each one right-hand side build and one solve of $O(K)$; no iteration | -| Evaluate | $O(\log K + D)$ | $O(1)$ | Binary search, then three Horner polynomials per channel | +| Operation | Time | Space | Notes | +|---------------|-----------------|----------------------------|------------------------------------------------------------------------------------| +| Factorization | $O(K)$ | three bands | Once per fit, shared by all channels | +| Fit | $O(D\,K)$ | $K_{\max}(2 + 2D)$ scalars | $D$ channels, each one right-hand side build and one solve of $O(K)$; no iteration | +| Evaluate | $O(\log K + D)$ | $O(1)$ | Binary search, then three Horner polynomials per channel | Storage holds the knots, their reciprocal spans, the values and the second derivatives: $K_{\max}(2 + 2D)$ scalars, where $K_{\max}$ is the compile-time capacity, plus the three bands of the solver. All of it is bounded and on the stack or inside the object; no heap. @@ -62,11 +62,11 @@ Knots $(0, 1, 2)$, values $(0, 1, 0)$, so $h_0 = h_1 = 1$ and $s_0 = 1$, $s_1 = **Clamped, zero end slopes.** The system is $$\begin{pmatrix} 2 & 1 & 0 \\ 1 & 4 & 1 \\ 0 & 1 & 2 \end{pmatrix} M = \begin{pmatrix} 6 \\ -12 \\ 6 \end{pmatrix}$$ -| $k$ | Pivot $p_k$ | $c'_k$ | Forward $y_k$ | Back $M_k$ | -|-----|-------------|--------|---------------|------------| -| 0 | $2$ | $1/2$ | $3$ | $3 + 3 = 6$ | -| 1 | $4 - 1/2 = 7/2$ | $2/7$ | $-30/7$ | $-30/7 - 12/7 = -6$ | -| 2 | $2 - 2/7 = 12/7$ | unused | $6$ | $6$ | +| $k$ | Pivot $p_k$ | $c'_k$ | Forward $y_k$ | Back $M_k$ | +|-----|------------------|--------|---------------|---------------------| +| 0 | $2$ | $1/2$ | $3$ | $3 + 3 = 6$ | +| 1 | $4 - 1/2 = 7/2$ | $2/7$ | $-30/7$ | $-30/7 - 12/7 = -6$ | +| 2 | $2 - 2/7 = 12/7$ | unused | $6$ | $6$ | So $M = (6, -6, 6)$. Interval 0 has $\beta_0 = 1 - (12 - 6)/6 = 0$ and $d_0 = -2$, giving $y(\tau) = 3\tau^2 - 2\tau^3$. Hence $y(0.5) = 0.75 - 0.25 = 0.5$, $y'(0.5) = 3 - 1.5 = 1.5$ and $y''(0.5) = 0$. The curve leaves and enters every knot flat, like a smooth step. @@ -102,11 +102,11 @@ Random fits are checked for interpolation of every knot, for continuity of first ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| [Tridiagonal Solver](TridiagonalSolver.md) | Solves the second-derivative system; its factorization is reused across channels | +| Algorithm | Relationship | +|---------------------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------| +| [Tridiagonal Solver](TridiagonalSolver.md) | Solves the second-derivative system; its factorization is reused across channels | | [Polynomial Profile](../motion_profiles/PolynomialProfile.md) | The two-knot clamped case is the same cubic Hermite polynomial; the spline generalizes it to many knots with $C^2$ joins | -| Polynomial fitting | Least-squares fit instead of interpolation | +| Polynomial fitting | Least-squares fit instead of interpolation | ## References & Further Reading diff --git a/doc/math/README.md b/doc/math/README.md index 8388129b..ca563f5e 100644 --- a/doc/math/README.md +++ b/doc/math/README.md @@ -4,20 +4,20 @@ Core mathematical primitives for numerical computation. ## Algorithms -| Algorithm | Description | -|-----------------------------|-----------------------------------------------------------------------------------------------| -| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | -| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | -| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | -| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | -| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | -| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | -| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | -| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | -| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | -| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | -| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | -| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | -| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | -| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | -| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) | +| Algorithm | Description | +|----------------------------------------------------|--------------------------------------------------------------------------------------------------------------------------| +| [CORDIC](Cordic.md) | Iterative shift-add engine for sin/cos, atan2, magnitude, and vector rotation — no multiplier | +| [Quaternion](Quaternion.md) | Unit-quaternion rotation type: Hamilton product, SLERP, rotation-matrix and Euler conversions | +| [MatrixNorms](MatrixNorms.md) | Frobenius, 1-norm, infinity-norm on matrices; vector L2 norm/normalize | +| [Householder Transform](HouseholderTransform.md) | Householder reflector for a sub-column — orthogonal, backward-stable factorization primitive | +| [Givens Rotation](GivensRotation.md) | Plane rotation zeroing one entry — streaming/sparse factorization primitive | +| [Triangular Solve](TriangularSolve.md) | Lower/upper-triangular forward and back-substitution shared by Gaussian elimination, Cholesky, and QR | +| [Cholesky Decomposition](CholeskyDecomposition.md) | SPD factorization $A=LL^T$ with `Factor` (L) and `Solve` (SPD system) — twice as fast as LU | +| [Matrix Operations](MatrixOperations.md) | Structural matrix utilities — `Symmetrize` (closest symmetric matrix) | +| [Step Response Metrics](StepResponseMetrics.md) | Rise time, settling time, percent overshoot, peak time, and steady-state error from a bounded step-response vector | +| [Matrix Exponential](MatrixExponential.md) | Scaling-and-squaring with diagonal (6,6) Padé approximant — exact ODE solution operator and discretisation engine | +| [Consistency Metrics](ConsistencyMetrics.md) | NEES/NIS estimator consistency with χ² gates — normalised estimation error squared and normalised innovation squared | +| [Angle Utilities](AngleUtilities.md) | Wrap to $(-\pi, \pi]$ / $[0, 2\pi)$, shortest signed angle difference, and unwrapping of wrapped measurements | +| [Tridiagonal Solver](TridiagonalSolver.md) | Thomas algorithm — $O(n)$ factorization reused across right-hand sides for diagonally dominant / SPD tridiagonal systems | +| [Cubic Spline](CubicSpline.md) | Vector-valued $C^2$ cubic spline (clamped / natural ends) fitted with one shared tridiagonal factorization | +| [SE(3) Transform](SE3Transform.md) | Rigid transforms with twist/wrench adjoints and an accurate exp/log (linear-first $(v;\omega)$ ordering) | diff --git a/doc/math/SE3Transform.md b/doc/math/SE3Transform.md index 8a0a83e1..93627c1f 100644 --- a/doc/math/SE3Transform.md +++ b/doc/math/SE3Transform.md @@ -89,15 +89,15 @@ the translation difference stacked on the rotation vector that carries the curre ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Compose | O(1) | O(1) | one $3\times3$ product, one matrix–vector product | -| Inverse | O(1) | O(1) | transpose and one matrix–vector product; no solve | -| Apply / rotate | O(1) | O(1) | one matrix–vector product (plus 3 additions for apply) | -| Twist / wrench transform | O(1) | O(1) | two matrix–vector products and one cross product, no $6\times6$ formed | -| Adjoint | O(1) | O(1) | $36$ scalars, one extra $3\times3$ product; built only on request | -| Exp | O(1) | O(1) | two sines of $\varphi$ and $\varphi/2$, none below $\varphi = 0.1$ | -| Log | O(1) | O(1) | one atan2; a half-angle sine/cosine pair above $\varphi = 0.1$; one square root for large angles | +| Operation | Time | Space | Notes | +|--------------------------|------|-------|--------------------------------------------------------------------------------------------------| +| Compose | O(1) | O(1) | one $3\times3$ product, one matrix–vector product | +| Inverse | O(1) | O(1) | transpose and one matrix–vector product; no solve | +| Apply / rotate | O(1) | O(1) | one matrix–vector product (plus 3 additions for apply) | +| Twist / wrench transform | O(1) | O(1) | two matrix–vector products and one cross product, no $6\times6$ formed | +| Adjoint | O(1) | O(1) | $36$ scalars, one extra $3\times3$ product; built only on request | +| Exp | O(1) | O(1) | two sines of $\varphi$ and $\varphi/2$, none below $\varphi = 0.1$ | +| Log | O(1) | O(1) | one atan2; a half-angle sine/cosine pair above $\varphi = 0.1$; one square root for large angles | A transform holds $12$ scalars; the $6\times6$ adjoint appears only when requested. All operations are stack-only: no heap, no recursion, constant work per call. @@ -107,13 +107,13 @@ A transform holds $12$ scalars; the $6\times6$ adjoint appears only when request **Exp of a screw.** Rotation about $\hat z$ through the point $(1,0,0)$: $\omega = \hat z$, $v = -\hat z \times (1,0,0) = (0,-1,0)$, $\theta = \pi/2$. -| Step | Value | -|------|-------| -| $\varphi = \theta\|\omega\|$ | $\pi/2 \ge 0.1$, direct forms | -| Coefficients | $a_1 = 2/\pi$, $a_2 = 4/\pi^2$, $a_3 = (\pi/2 - 1)/(\pi/2)^3$ | -| Vectors | $Kv = \hat z \times v = (1,0,0)$, $K^2 v = (0,1,0)$ | -| $R$ | $\theta a_1 = 1$, $\theta^2 a_2 = 1$: $R = I + K + K^2 = R_z(\pi/2)$ | -| $p$ | $\tfrac{\pi}{2}(0,-1,0) + (1,0,0) + (\tfrac{\pi}{2} - 1)(0,1,0) = (1,-1,0)$ | +| Step | Value | +|------------------------------|-----------------------------------------------------------------------------| +| $\varphi = \theta\|\omega\|$ | $\pi/2 \ge 0.1$, direct forms | +| Coefficients | $a_1 = 2/\pi$, $a_2 = 4/\pi^2$, $a_3 = (\pi/2 - 1)/(\pi/2)^3$ | +| Vectors | $Kv = \hat z \times v = (1,0,0)$, $K^2 v = (0,1,0)$ | +| $R$ | $\theta a_1 = 1$, $\theta^2 a_2 = 1$: $R = I + K + K^2 = R_z(\pi/2)$ | +| $p$ | $\tfrac{\pi}{2}(0,-1,0) + (1,0,0) + (\tfrac{\pi}{2} - 1)(0,1,0) = (1,-1,0)$ | Check: the origin sits at $(-1,0,0)$ relative to the axis; a quarter turn takes it to $(0,-1,0)$, and adding the axis point gives $(1,-1,0)$. @@ -138,13 +138,13 @@ The result $\big((0,-\tfrac\pi2,0);\,(0,0,\tfrac\pi2)\big)$ is $\theta\xi$, as e ## Variants & Generalizations -| Variant | Key difference | -|---------|----------------| -| **Angular-first ordering** $(\omega;\, v)$ | Same algebra with blocks permuted; used by Lynch & Park and Murray et al. | -| **Body vs. spatial twist** | Body twists are $T^{-1}\dot T$, spatial twists $\dot T\,T^{-1}$; related by the adjoint | -| **Quaternion rotation part** | Four scalars and cheap renormalisation; the rotation vector is the vector part scaled by $\varphi/\sin(\varphi/2)$ | -| **Dual quaternions** | Rotation and translation in one algebra; screw interpolation | -| **Similarity group** | Adds a uniform scale for monocular visual odometry | +| Variant | Key difference | +|--------------------------------------------|--------------------------------------------------------------------------------------------------------------------| +| **Angular-first ordering** $(\omega;\, v)$ | Same algebra with blocks permuted; used by Lynch & Park and Murray et al. | +| **Body vs. spatial twist** | Body twists are $T^{-1}\dot T$, spatial twists $\dot T\,T^{-1}$; related by the adjoint | +| **Quaternion rotation part** | Four scalars and cheap renormalisation; the rotation vector is the vector part scaled by $\varphi/\sin(\varphi/2)$ | +| **Dual quaternions** | Rotation and translation in one algebra; screw interpolation | +| **Similarity group** | Adds a uniform scale for monocular visual odometry | ## Applications diff --git a/doc/solvers/DampedPseudoInverse.md b/doc/solvers/DampedPseudoInverse.md index 96949a4a..cf875ef2 100644 --- a/doc/solvers/DampedPseudoInverse.md +++ b/doc/solvers/DampedPseudoInverse.md @@ -56,12 +56,12 @@ with $\sigma_{\min}$ the smallest nonzero singular value. Robustness costs accur ## Complexity Analysis -| Operation | Time | Space | Notes | -|-----------|------|-------|-------| -| Factor | $O(Mk^2)$ | $O(mk + nk)$ | One Golub–Kahan SVD; wide matrices through their transpose | -| Solve | $O((m + n)\,k)$ | $O(n)$ | $k$ inner products of length $m$, $k$ updates of length $n$; no division | -| Project | $O(n\,r)$ | $O(n)$ | $r$ inner products and updates; $P$ never formed | -| Explicit inverse, projector | $O(mnk)$, $O(n^2 r)$ | $O(mn)$, $O(n^2)$ | Diagnostics; sums of rank-one terms | +| Operation | Time | Space | Notes | +|-----------------------------|----------------------|-------------------|--------------------------------------------------------------------------| +| Factor | $O(Mk^2)$ | $O(mk + nk)$ | One Golub–Kahan SVD; wide matrices through their transpose | +| Solve | $O((m + n)\,k)$ | $O(n)$ | $k$ inner products of length $m$, $k$ updates of length $n$; no division | +| Project | $O(n\,r)$ | $O(n)$ | $r$ inner products and updates; $P$ never formed | +| Explicit inverse, projector | $O(mnk)$, $O(n^2 r)$ | $O(mn)$, $O(n^2)$ | Diagnostics; sums of rank-one terms | Here $M = \max(m, n)$. The SVD factors are the only persistent storage: $U$ is $m \times k$, $V$ is $n \times k$, plus $k$ singular values. No heap is used, and the factorization is paid once per matrix; later solves and projections are inner products only. @@ -74,9 +74,9 @@ Here $M = \max(m, n)$. The SVD factors are the only persistent storage: $U$ is $ **Gains and projections** at $\lambda = 0.1$: | $i$ | $\sigma_i$ | $u_i^\top b$ | $1/\sigma_i$ | $g(\sigma_i)$ | -|-----|-----------|--------------|--------------|---------------| -| 0 | $1.732051$ | $2.121320$ | $0.577350$ | $0.575432$ | -| 1 | $1$ | $-0.707107$ | $1$ | $0.990099$ | +|-----|------------|--------------|--------------|---------------| +| 0 | $1.732051$ | $2.121320$ | $0.577350$ | $0.575432$ | +| 1 | $1$ | $-0.707107$ | $1$ | $0.990099$ | **Solution.** Without damping, $x = A^\dagger b = (0, 1, 1)$: it satisfies $Ax = b$ and is orthogonal to the null space ($0 + 1 - 1 = 0$). With $\lambda = 0.1$, $x = (0.003289, 0.993388, 0.996678)$. The residual is $b - Ax = \lambda^2(AA^\top + \lambda^2 I)^{-1}b$, of norm $0.009934$, within the bound $0.022139$ (the looser $\lambda^2\|b\|/\sigma_{\min}^2$ gives $0.022361$). diff --git a/doc/solvers/README.md b/doc/solvers/README.md index b50e65b9..daf3395f 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -19,4 +19,4 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. | [Singular Value Decomposition](SingularValueDecomposition.md) | Golub-Kahan bidiagonalization and implicit-shift QR for SVD, pseudo-inverse, rank, and condition number | | [Jacobi Eigenvalue Solver](JacobiEigenSolver.md) | Cyclic Jacobi rotations for the full symmetric eigenvalue/eigenvector problem | | [Symplectic Integrators](SymplecticIntegrators.md) | Semi-implicit Euler and velocity Verlet for second-order systems — bounded energy error on conservative plants | -| [Damped Pseudo-Inverse](DampedPseudoInverse.md) | Damped (Tikhonov) least-squares inverse and exact null-space projector from one SVD — tall or wide matrices | +| [Damped Pseudo-Inverse](DampedPseudoInverse.md) | Damped (Tikhonov) least-squares inverse and exact null-space projector from one SVD — tall or wide matrices | From 920b43db826da5d34e133bc3714931f4bc1b7a0b Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:46:55 +0000 Subject: [PATCH 6/6] refactor(math): make SE3Transform composition a hidden friend Define operator* as an inline friend, matching Quaternion, Matrix and Complex, as flagged by SonarCloud (cpp:S2807). Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- numerical/math/SE3Transform.hpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/numerical/math/SE3Transform.hpp b/numerical/math/SE3Transform.hpp index d76d35ec..3ee813ca 100644 --- a/numerical/math/SE3Transform.hpp +++ b/numerical/math/SE3Transform.hpp @@ -26,7 +26,12 @@ namespace math Vector3 translation{}; static SE3Transform Identity(); - OPTIMIZE_FOR_SPEED SE3Transform operator*(const SE3Transform& rhs) const; + + [[nodiscard]] OPTIMIZE_FOR_SPEED friend SE3Transform operator*(const SE3Transform& lhs, const SE3Transform& rhs) + { + return { lhs.rotation * rhs.rotation, lhs.rotation * rhs.translation + lhs.translation }; + } + SE3Transform Inverse() const; OPTIMIZE_FOR_SPEED Vector3 Apply(const Vector3& point) const; Vector3 Rotate(const Vector3& direction) const; @@ -55,12 +60,6 @@ namespace math return {}; } - template - OPTIMIZE_FOR_SPEED SE3Transform SE3Transform::operator*(const SE3Transform& rhs) const - { - return { rotation * rhs.rotation, rotation * rhs.translation + translation }; - } - template SE3Transform SE3Transform::Inverse() const {