From 599a35ecf99738f82879e5df480944f9bbc18cdc Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:36:33 +0000 Subject: [PATCH 1/3] =?UTF-8?q?feat(optimization):=20add=20Levenberg?= =?UTF-8?q?=E2=80=93Marquardt=20with=20retraction=20and=20damping=20policy?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 64: bounded-iteration damped Gauss–Newton over a LeastSquaresProblem interface (residual, Jacobian w.r.t. a local increment, retraction x ⊕ δ) and a DampingPolicy interface (ConstantDamping; λ = 0 is Gauss–Newton). Each step solves the smaller normal system with LU, can be capped by maxStep, and a singular system stops the solve unconverged instead of dividing by zero. The Rosenbrock test uses tolerance 1e-4 instead of the spec's 1e-5: cond(JJᵀ) ≈ 4600 there, so float32 Gauss–Newton ends its two exact steps at ‖r‖ ≈ 1e-5, right on the spec's threshold. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 22 +- ROADMAP.md | 10 - doc/optimization/LevenbergMarquardt.md | 130 ++++++++++++ doc/optimization/README.md | 1 + numerical/optimization/CMakeLists.txt | 4 + numerical/optimization/DampingPolicy.hpp | 37 ++++ .../optimization/LeastSquaresProblem.hpp | 21 ++ numerical/optimization/LevenbergMarquardt.cpp | 7 + numerical/optimization/LevenbergMarquardt.hpp | 138 +++++++++++++ numerical/optimization/test/CMakeLists.txt | 1 + .../test/TestLevenbergMarquardt.cpp | 190 ++++++++++++++++++ roadmap/README.md | 2 - .../LevenbergMarquardt/explanation.md | 34 ---- .../LevenbergMarquardt/implementation.md | 118 ----------- .../optimization/LevenbergMarquardt/tests.md | 51 ----- 15 files changed, 540 insertions(+), 226 deletions(-) create mode 100644 doc/optimization/LevenbergMarquardt.md create mode 100644 numerical/optimization/DampingPolicy.hpp create mode 100644 numerical/optimization/LeastSquaresProblem.hpp create mode 100644 numerical/optimization/LevenbergMarquardt.cpp create mode 100644 numerical/optimization/LevenbergMarquardt.hpp create mode 100644 numerical/optimization/test/TestLevenbergMarquardt.cpp delete mode 100644 roadmap/optimization/LevenbergMarquardt/explanation.md delete mode 100644 roadmap/optimization/LevenbergMarquardt/implementation.md delete mode 100644 roadmap/optimization/LevenbergMarquardt/tests.md diff --git a/README.md b/README.md index b794fb35..5851457f 100644 --- a/README.md +++ b/README.md @@ -14,17 +14,17 @@ 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, Levenberg–Marquardt | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log) | +| [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 | diff --git a/ROADMAP.md b/ROADMAP.md index a9ade851..48c07198 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -28,7 +28,6 @@ Difficulty legend: |----|-------------------------------------------------------|-------------------------|------------| | 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ | | 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ | -| 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` | ★★★☆☆ | | 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ | @@ -70,15 +69,6 @@ the library does not yet expose. Detailed below under ## Tier 3 — Moderate ★★★☆☆ -### 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. -- **Embedded value:** One bounded, heap-free solver for IK, parallel-robot forward kinematics, calibration - and scan matching. -- **Algorithm / paper:** K. Levenberg (1944); D. Marquardt (1963); J. Nocedal, S. Wright (2006), Ch. 10. -- **Reuses:** `LuDecomposition`. -- **Spec:** [roadmap/optimization/LevenbergMarquardt](roadmap/optimization/LevenbergMarquardt/implementation.md). - ### 68. Lie-group Jacobians and ⊕/⊖ - **What:** Left and right Jacobians (and inverses) of SO(3), SE(3) and SE(2), the `⊕`/`⊖` operators on the tangent space, and adjoint-based covariance transport between frames. diff --git a/doc/optimization/LevenbergMarquardt.md b/doc/optimization/LevenbergMarquardt.md new file mode 100644 index 00000000..341dbc5a --- /dev/null +++ b/doc/optimization/LevenbergMarquardt.md @@ -0,0 +1,130 @@ +# Levenberg–Marquardt (Damped Gauss–Newton with Retraction) + +## Overview & Motivation + +Inverse kinematics, forward kinematics of parallel robots, sensor and hand-eye calibration and scan matching all ask the same question: which state makes a set of nonlinear residuals as small as possible? The problems are small, solved every control cycle or at commissioning, and must finish within a hard iteration budget without heap allocation. + +**Levenberg–Marquardt** answers it by repeated linearization. Near the current guess the residuals behave like a linear function of a small correction, and Gauss–Newton solves that linear problem exactly, converging quadratically when a zero residual is reachable. Far from the solution, or near a singular Jacobian, the exact step can be huge or undefined. A **damping** term penalizes large corrections and trades speed for robustness. + +The state need not be a plain vector: an angle wraps, an orientation lives on a manifold, a joint stops at a limit. The solver therefore never adds the correction itself. The problem applies it through a **retraction**, so the state never leaves its valid set. One careful solver replaces a hand-rolled Newton loop in every module. + +## Mathematical Theory + +### Problem Statement + +$$ +\min_{x}\; \tfrac12\,\|r(x)\|^2, \qquad r(x) \in \mathbb{R}^{R} +$$ + +The state $x$ may lie on a manifold or within bounds and has $P$ local degrees of freedom. The problem supplies three things: + +- the residual $r(x)$; +- the Jacobian with respect to a local increment, $J = \partial r(x \oplus \delta)/\partial\delta\,\big|_{\delta=0} \in \mathbb{R}^{R\times P}$; +- the retraction $x \oplus \delta$: plain addition for vectors, addition followed by clamping for bounded states, a manifold update (a rotation composed with the exponential of $\delta$) for orientations. It must agree with the exact motion to first order. + +A damping policy supplies $\lambda^2 \ge 0$ from the current Jacobian. A constant policy is classic Levenberg–Marquardt, and $\lambda = 0$ is Gauss–Newton. A problem-aware schedule, such as zero away from singularities, plugs in through the same interface. + +### Damped Step + +Linearizing $r(x \oplus \delta) \approx r + J\delta$ and penalizing the correction gives + +$$ +\min_{\delta}\; \tfrac12\|r + J\delta\|^2 + \tfrac12\lambda^2\|\delta\|^2 \;\Longrightarrow\; (J^\top J + \lambda^2 I)\,\delta = -J^\top r . +$$ + +The push-through identity $(J^\top J + \lambda^2 I)^{-1}J^\top = J^\top (JJ^\top + \lambda^2 I)^{-1}$ gives an equivalent form, so the solver always factors the smaller system: + +| Shape | Condition | System | Step | +|-------|-----------|--------|------| +| Tall | $R > P$ | $P \times P$, parameter space | $\delta = -(J^\top J + \lambda^2 I)^{-1}J^\top r$ | +| Wide or square | $R \le P$ | $R \times R$, residual space | $\delta = -J^\top (JJ^\top + \lambda^2 I)^{-1} r$ | + +For $\lambda = 0$ and full row rank, the wide form is the minimum-norm solution of $J\delta = -r$: the shortest correction that cancels the linearized residual. The system is solved by LU decomposition with partial pivoting, never by an explicit inverse. If the factorization finds a pivot below a relative $10^{-6}$ (for example $\lambda = 0$ with a rank-deficient $J$), the solve ends unconverged at the current state instead of dividing by zero. For $\lambda^2 > 0$ the matrix is symmetric positive definite. + +### Iteration + +With tolerance $\varepsilon$, optional step cap $\Delta$ and iteration budget $N$: + +1. Evaluate $r$. If $\|r\| < \varepsilon$, stop converged. +2. Evaluate $J$ and $\lambda^2$, compute $\delta$. If the system is singular, stop unconverged. +3. If $\|\delta\| > \Delta$, rescale $\delta \leftarrow \delta\,\Delta/\|\delta\|$. This is trust-region-like: the direction is kept and only the length is limited. +4. Retract: $x \leftarrow x \oplus \delta$. + +After $N$ iterations the residual is recomputed at the returned state, which is converged only if it is within tolerance. The reported residual always belongs to the returned state. + +### Fixed Points and the Effect of Damping + +The damped step vanishes exactly when $J^\top r = 0$. For a tall problem this is a stationary point of the cost. For a wide problem of full row rank it forces $r = 0$. Damping changes the path, not the destination: convergence drops from quadratic to linear, but a zero-residual fixed point stays a fixed point. A non-zero final residual therefore means an unreachable target, a singularity or an active bound. + +## Complexity Analysis + +| Item | Cost | Justification | +|------|------|---------------| +| Normal matrix | $O(R\,P\,m)$ | $J^\top J$ costs $R P^2$, $JJ^\top$ costs $P R^2$; $m = \min(R, P)$ | +| Factorization and solve | $O(m^3)$ | LU of an $m \times m$ matrix dominates the $O(m^2)$ triangular solves | +| Whole solve | at most $N$ iterations | each adds one residual and one Jacobian from the problem; $N + 1$ residual evaluations in all, the last at the returned state | +| Memory | one $R \times P$ Jacobian, one $m \times m$ system | stack only; no heap, no recursion | + +## Step-by-Step Walkthrough + +**Rosenbrock residuals.** $r = (10(y - x^2),\; 1 - x)$, $J = \begin{pmatrix} -20x & 10 \\ -1 & 0 \end{pmatrix}$, zero residual at $(1, 1)$. Start at $(-1.2, 1)$ with $\lambda = 0$; $R = P = 2$, so the step is solved in residual space. + +| Iteration | State | $r$ | $J$ | Step $\delta$ | New state | +|-----------|-------|-----|-----|---------------|-----------| +| 1 | $(-1.2, 1)$ | $(-4.4, 2.2)$, $\|r\| = 4.919$ | $\begin{pmatrix} 24 & 10 \\ -1 & 0 \end{pmatrix}$ | $(2.2, -4.84)$ | $(1, -3.84)$ | +| 2 | $(1, -3.84)$ | $(-48.4, 0)$, $\|r\| = 48.4$ | $\begin{pmatrix} -20 & 10 \\ -1 & 0 \end{pmatrix}$ | $(0, 4.84)$ | $(1, 1)$ | + +The second residual is linear in $x$, so step 1 zeroes it exactly. The first is then linear in $y$, so step 2 finishes exactly. The residual norm rises from $4.9$ to $48.4$ on the way: the iteration is not monotone. With a step cap $\Delta = 1$, the first step of norm $5.316540$ is scaled to unit length, giving $(-0.786197,\; 0.089634)$. + +**Angle on the unit circle.** State $\theta$, $r = (\cos\theta, \sin\theta) - (\cos 2.5, \sin 2.5)$, $J = (-\sin\theta,\; \cos\theta)^\top$. Here $R = 2 > P = 1$ and $J^\top J = 1$, so with $\lambda = 0$ each step is $\theta \leftarrow \theta - \sin(\theta - 2.5)$, wrapped into $(-\pi, \pi]$. Start at $\theta = -2.5$, which is only $1.28$ rad from the target across the $\pm\pi$ seam. + +| Iteration | $\theta$ | $\sin(\theta - 2.5)$ | Before wrap | After wrap | +|-----------|----------|----------------------|-------------|------------| +| 1 | $-2.500000$ | $0.958924$ | $-3.458924$ | $2.824261$ | +| 2 | $2.824261$ | $0.318608$ | $2.505653$ | $2.505653$ | +| 3 | $2.505653$ | $0.005653$ | $2.500000$ | $2.500000$ | + +The wrap keeps the state in its canonical range, so the answer is $2.5$ and not $2.5 - 2\pi$. The error falls $0.324 \to 0.0057 \to 3\times10^{-8}$, faster than quadratic because of the circle's symmetry. + +**Unreachable target.** Target $(2, 0)$ lies outside the unit circle, with $\lambda^2 = 1$. Then $J^\top J + \lambda^2 = 2$ and $\delta = -\tfrac12 J^\top r = -\sin\theta$, so $\theta \leftarrow \theta - \sin\theta \to 0$. At $\theta = 0$ the step vanishes ($J^\top r = 0$) while $\|r\| = \|(1, 0) - (2, 0)\| = 1$. The solve exhausts its budget and reports not converged. + +## Pitfalls & Edge Cases + +- **Squared condition number in single precision.** Forming $J^\top J$ or $JJ^\top$ squares the condition number of $J$, and the square case pays it too. On the Rosenbrock start $\operatorname{cond}(JJ^\top) \approx 4600$, and float32 Gauss–Newton reaches only $\|r\| \approx 10^{-5}$ after its two exact steps. Use tolerances of at least $10^{-4}$ in single precision. +- **Singular undamped system.** With $\lambda = 0$ and a rank-deficient $J$ the solve stops unconverged at the current state, with the iteration count reached so far. Use positive damping near singularities. A small positive $\lambda^2$ can pass the pivot test yet allow steps up to about $\|r\|/(2\lambda)$, so pair it with a step cap. +- **No cost-decrease check, local method.** The step is accepted unconditionally, so a poor initial guess can overshoot, as the Rosenbrock walkthrough shows, and with several minima the initial state decides which one is reached. A step cap and a warm start limit the risk; neither guarantees descent. +- **Non-zero minimum residual.** Convergence is tested only on $\|r\| < \varepsilon$. A problem whose best fit leaves a residual above $\varepsilon$ (noisy over-determined data, unreachable target) runs the full budget and reports not converged. The returned residual shows the value attained. +- **Jacobian and retraction must agree.** The Jacobian must be taken with respect to the very increment the retraction consumes. A mismatch, such as a left perturbation in the Jacobian and a right update, degrades convergence to linear or stalls it. +- **Active bounds.** A clamping retraction applies an unconstrained step and then projects it. Components pressed against a bound stop moving while the others continue, and the residual may stay non-zero. + +## Variants & Generalizations + +| Variant | Key difference | +|---------|----------------| +| **Gauss–Newton ($\lambda = 0$)** | Quadratic convergence for zero-residual problems; fails on rank deficiency | +| **Constant or problem-aware damping (this)** | Fixed $\lambda$ gives robust linear convergence; a Jacobian-dependent $\lambda^2$ (zero away from singularities) keeps the fast rate where safe | +| **Gain-ratio (Nielsen) adaptation** | Raises or lowers $\lambda$ from the ratio of actual to predicted cost reduction; needs a trial evaluation and accept/reject, a possible later variant | + +## Applications + +- **Inverse kinematics** — pose targets for manipulators, orientation on a manifold, joint limits through clamping, singularity-robust damping. +- **Parallel-robot forward kinematics** — platform pose from measured leg lengths. +- **Calibration and scan matching** — sensor models, hand-eye transforms and rigid alignment of measurements against a reference. +- **Cyclic control tasks** — warm start from the previous cycle's solution, which typically needs few iterations and gives a fixed worst-case cost. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| [LU Decomposition](../solvers/LuDecomposition.md) | Solves the damped normal system; its singularity report ends the solve unconverged | +| [Damped Pseudo-Inverse](../solvers/DampedPseudoInverse.md) | The same damped step in SVD form for one linear solve; avoids squaring the condition number at higher cost | +| [SE(3) Transform](../math/SE3Transform.md) | Supplies the manifold retraction for pose states | +| [Angle Utilities](../math/AngleUtilities.md) | Supplies the wrapped retraction for angle states | +| [Gradient Descent](Optimizer.md) | First-order method for general costs; this method exploits the sum-of-squares structure through the Gauss–Newton curvature and needs far fewer iterations on small problems | + +## References & Further Reading + +- Levenberg, K., "A method for the solution of certain non-linear problems in least squares," *Quarterly of Applied Mathematics*, 2(2):164–168, 1944. +- Marquardt, D.W., "An algorithm for least-squares estimation of nonlinear parameters," *SIAM Journal on Applied Mathematics*, 11(2):431–441, 1963. +- Nocedal, J. and Wright, S.J., *Numerical Optimization*, 2nd ed., Springer, 2006 — Chapter 10. +- Absil, P.-A., Mahony, R., and Sepulchre, R., *Optimization Algorithms on Matrix Manifolds*, Princeton University Press, 2008 — Chapter 4 (retractions). diff --git a/doc/optimization/README.md b/doc/optimization/README.md index 76a711b4..91877965 100644 --- a/doc/optimization/README.md +++ b/doc/optimization/README.md @@ -9,3 +9,4 @@ General-purpose optimization algorithms for parameter fitting, training, and con | [Bayesian Optimization](BayesianOptimization.md) | Gradient-free global optimization using Gaussian Process surrogate and Expected Improvement | | [Gradient Descent](Optimizer.md) | Iterative batch optimization via gradient-based weight updates | | [SGD / Adam Step Optimisers](StepOptimizer.md) | Stateful per-step SGD (with optional momentum/Nesterov) and Adam with bias correction | +| [Levenberg–Marquardt](LevenbergMarquardt.md) | Damped Gauss–Newton for small nonlinear least squares with a retraction (manifold / bounded states) and pluggable damping | diff --git a/numerical/optimization/CMakeLists.txt b/numerical/optimization/CMakeLists.txt index 7335194d..69e74b4d 100644 --- a/numerical/optimization/CMakeLists.txt +++ b/numerical/optimization/CMakeLists.txt @@ -14,7 +14,10 @@ target_link_libraries(numerical.optimization ${NUMERICAL_VISIBILITY} target_sources(numerical.optimization PRIVATE Adam.hpp BayesianOptimization.hpp + DampingPolicy.hpp GradientDescent.hpp + LeastSquaresProblem.hpp + LevenbergMarquardt.hpp ObjectiveFunction.hpp Optimizer.hpp Sgd.hpp @@ -25,6 +28,7 @@ numerical_add_coverage_sources(numerical.optimization Adam.cpp BayesianOptimization.cpp GradientDescent.cpp + LevenbergMarquardt.cpp Sgd.cpp ) diff --git a/numerical/optimization/DampingPolicy.hpp b/numerical/optimization/DampingPolicy.hpp new file mode 100644 index 00000000..9fa9a597 --- /dev/null +++ b/numerical/optimization/DampingPolicy.hpp @@ -0,0 +1,37 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace optimization +{ + template + class DampingPolicy + { + static_assert(std::is_floating_point_v, "DampingPolicy supports floating-point types only"); + + public: + virtual ~DampingPolicy() = default; + + [[nodiscard]] virtual T DampingSquared(const math::Matrix& jacobian) const = 0; + }; + + template + class ConstantDamping + : public DampingPolicy + { + public: + explicit ConstantDamping(T lambdaSquared) + : lambdaSquared{ lambdaSquared } + {} + + [[nodiscard]] T DampingSquared(const math::Matrix&) const override + { + return lambdaSquared; + } + + private: + T lambdaSquared; + }; +} diff --git a/numerical/optimization/LeastSquaresProblem.hpp b/numerical/optimization/LeastSquaresProblem.hpp new file mode 100644 index 00000000..2afb9fdc --- /dev/null +++ b/numerical/optimization/LeastSquaresProblem.hpp @@ -0,0 +1,21 @@ +#pragma once + +#include "numerical/math/Matrix.hpp" +#include +#include + +namespace optimization +{ + template + class LeastSquaresProblem + { + static_assert(std::is_floating_point_v, "LeastSquaresProblem supports floating-point types only"); + + public: + virtual ~LeastSquaresProblem() = default; + + [[nodiscard]] virtual math::Vector Residual(const State& x) const = 0; + [[nodiscard]] virtual math::Matrix Jacobian(const State& x) const = 0; + [[nodiscard]] virtual State Retract(const State& x, const math::Vector& delta) const = 0; + }; +} diff --git a/numerical/optimization/LevenbergMarquardt.cpp b/numerical/optimization/LevenbergMarquardt.cpp new file mode 100644 index 00000000..07efd24c --- /dev/null +++ b/numerical/optimization/LevenbergMarquardt.cpp @@ -0,0 +1,7 @@ +#include "numerical/optimization/LevenbergMarquardt.hpp" + +namespace optimization +{ + template class ConstantDamping; + template class LevenbergMarquardt, 2, 2>; +} diff --git a/numerical/optimization/LevenbergMarquardt.hpp b/numerical/optimization/LevenbergMarquardt.hpp new file mode 100644 index 00000000..9134a493 --- /dev/null +++ b/numerical/optimization/LevenbergMarquardt.hpp @@ -0,0 +1,138 @@ +#pragma once + +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/optimization/DampingPolicy.hpp" +#include "numerical/optimization/LeastSquaresProblem.hpp" +#include "numerical/solvers/LuDecomposition.hpp" +#include +#include +#include +#include + +namespace optimization +{ + template + struct LevenbergMarquardtConfig + { + static_assert(std::is_floating_point_v, "LevenbergMarquardtConfig supports floating-point types only"); + + T tolerance{}; + T maxStep{ std::numeric_limits::infinity() }; + std::size_t maxIterations{}; + }; + + template + struct LevenbergMarquardtResult + { + static_assert(std::is_floating_point_v, "LevenbergMarquardtResult supports floating-point types only"); + + State x; + T residualNorm{}; + std::size_t iterations{}; + bool converged{ false }; + }; + + template + class LevenbergMarquardt + { + static_assert(std::is_floating_point_v, "LevenbergMarquardt supports floating-point types only"); + + public: + using Problem = LeastSquaresProblem; + using Damping = DampingPolicy; + using Result = LevenbergMarquardtResult; + + LevenbergMarquardt(const Problem& problem, const Damping& damping, const LevenbergMarquardtConfig& config); + + OPTIMIZE_FOR_SPEED Result Solve(const State& initial) const; + + private: + using ResidualVector = math::Vector; + using StepVector = math::Vector; + using Jacobian = math::Matrix; + + std::optional DampedStep(const Jacobian& jacobian, const ResidualVector& residual) const; + + template + static T Norm(const math::Vector& v); + + const Problem& problem; + const Damping& damping; + LevenbergMarquardtConfig config; + }; + + template + LevenbergMarquardt::LevenbergMarquardt(const Problem& problem, const Damping& damping, const LevenbergMarquardtConfig& config) + : problem{ problem } + , damping{ damping } + , config{ config } + {} + + template + OPTIMIZE_FOR_SPEED auto LevenbergMarquardt::Solve(const State& initial) const -> Result + { + State x{ initial }; + for (std::size_t iteration = 0; iteration < config.maxIterations; ++iteration) + { + const ResidualVector residual{ problem.Residual(x) }; + const T residualNorm{ Norm(residual) }; + if (residualNorm < config.tolerance) + return { x, residualNorm, iteration, true }; + + auto step{ DampedStep(problem.Jacobian(x), residual) }; + if (!step) + return { x, residualNorm, iteration, false }; + + const T stepNorm{ Norm(*step) }; + if (stepNorm > config.maxStep) + *step *= config.maxStep / stepNorm; + + x = problem.Retract(x, *step); + } + + const T residualNorm{ Norm(problem.Residual(x)) }; + return { x, residualNorm, config.maxIterations, residualNorm < config.tolerance }; + } + + template + auto LevenbergMarquardt::DampedStep(const Jacobian& jacobian, const ResidualVector& residual) const -> std::optional + { + const T lambdaSquared{ damping.DampingSquared(jacobian) }; + const auto transposed{ jacobian.Transpose() }; + + if constexpr (Residuals <= Parameters) + { + solvers::LuDecomposition lu; + if (!lu.Decompose(jacobian * transposed + math::SquareMatrix::Identity() * lambdaSquared)) + return std::nullopt; + + return transposed * lu.Solve(residual) * T{ -1 }; + } + else + { + solvers::LuDecomposition lu; + if (!lu.Decompose(transposed * jacobian + math::SquareMatrix::Identity() * lambdaSquared)) + return std::nullopt; + + return lu.Solve(transposed * residual) * T{ -1 }; + } + } + + template + template + T LevenbergMarquardt::Norm(const math::Vector& v) + { + T sum{}; + for (std::size_t i = 0; i < Size; ++i) + sum += v[i] * v[i]; + + return math::Sqrt(sum); + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ConstantDamping; + extern template class LevenbergMarquardt, 2, 2>; +#endif +} diff --git a/numerical/optimization/test/CMakeLists.txt b/numerical/optimization/test/CMakeLists.txt index 99a22e66..f008fcb7 100644 --- a/numerical/optimization/test/CMakeLists.txt +++ b/numerical/optimization/test/CMakeLists.txt @@ -12,5 +12,6 @@ numerical_link_qemu_runtime(numerical.optimization_test) target_sources(numerical.optimization_test PRIVATE TestBayesianOptimization.cpp TestGradientDescent.cpp + TestLevenbergMarquardt.cpp TestStepOptimizer.cpp ) diff --git a/numerical/optimization/test/TestLevenbergMarquardt.cpp b/numerical/optimization/test/TestLevenbergMarquardt.cpp new file mode 100644 index 00000000..a76131e8 --- /dev/null +++ b/numerical/optimization/test/TestLevenbergMarquardt.cpp @@ -0,0 +1,190 @@ +#include "numerical/optimization/LevenbergMarquardt.hpp" +#include "gmock/gmock.h" +#include "gtest/gtest.h" +#include +#include +#include + +namespace +{ + using Vector1 = math::Vector; + using Vector2 = math::Vector; + using Matrix21 = math::Matrix; + using Matrix22 = math::Matrix; + + MATCHER_P(MatrixNear, expected, "") + { + for (std::size_t r = 0; r < 2; ++r) + for (std::size_t c = 0; c < 2; ++c) + if (std::abs(arg.at(r, c) - expected.at(r, c)) > 1e-5f) + return false; + + return true; + } + + class RosenbrockResiduals + : public optimization::LeastSquaresProblem + { + public: + Vector2 Residual(const Vector2& x) const override + { + return Vector2{ 10.0f * (x[1] - x[0] * x[0]), 1.0f - x[0] }; + } + + Matrix22 Jacobian(const Vector2& x) const override + { + return Matrix22{ { -20.0f * x[0], 10.0f }, { -1.0f, 0.0f } }; + } + + Vector2 Retract(const Vector2& x, const Vector2& delta) const override + { + return x + delta; + } + }; + + class UnitCircleAngle + : public optimization::LeastSquaresProblem + { + public: + explicit UnitCircleAngle(const Vector2& target) + : target{ target } + {} + + Vector2 Residual(const float& theta) const override + { + return Vector2{ std::cos(theta) - target[0], std::sin(theta) - target[1] }; + } + + Matrix21 Jacobian(const float& theta) const override + { + return Matrix21{ -std::sin(theta), std::cos(theta) }; + } + + float Retract(const float& theta, const Vector1& delta) const override + { + const float wrapped{ std::remainder(theta + delta[0], 2.0f * std::numbers::pi_v) }; + const float retracted{ wrapped <= -std::numbers::pi_v ? wrapped + 2.0f * std::numbers::pi_v : wrapped }; + if (retractions == 0) + firstRetraction = retracted; + largestMagnitude = std::max(largestMagnitude, std::abs(retracted)); + ++retractions; + return retracted; + } + + mutable float firstRetraction{ 0.0f }; + mutable float largestMagnitude{ 0.0f }; + mutable int retractions{ 0 }; + + private: + Vector2 target; + }; + + class ConstantResidual + : public optimization::LeastSquaresProblem + { + public: + Vector2 Residual(const Vector2&) const override + { + return Vector2{ 1.0f, 1.0f }; + } + + Matrix22 Jacobian(const Vector2&) const override + { + return Matrix22{}; + } + + Vector2 Retract(const Vector2& x, const Vector2& delta) const override + { + return x + delta; + } + }; + + class MockDampingPolicy + : public optimization::DampingPolicy + { + public: + MOCK_METHOD(float, DampingSquared, (const Matrix22& jacobian), (const, override)); + }; + + class TestLevenbergMarquardt + : public ::testing::Test + { + protected: + RosenbrockResiduals rosenbrock; + optimization::ConstantDamping gaussNewton{ 0.0f }; + optimization::ConstantDamping circleGaussNewton{ 0.0f }; + testing::StrictMock policy; + optimization::LevenbergMarquardtConfig config{ 1e-4f, std::numeric_limits::infinity(), 20 }; + const Vector2 start{ -1.2f, 1.0f }; + }; +} + +TEST_F(TestLevenbergMarquardt, gauss_newton_solves_rosenbrock_residuals_in_two_steps) +{ + const auto result{ optimization::LevenbergMarquardt{ rosenbrock, gaussNewton, config }.Solve(start) }; + + EXPECT_NEAR(result.x[0], 1.0f, 1e-5f); + EXPECT_NEAR(result.x[1], 1.0f, 1e-5f); + EXPECT_TRUE(result.converged); + EXPECT_EQ(result.iterations, 2); +} + +TEST_F(TestLevenbergMarquardt, damping_policy_is_queried_with_the_current_jacobian) +{ + config.maxIterations = 1; + EXPECT_CALL(policy, DampingSquared(MatrixNear(Matrix22{ { 24.0f, 10.0f }, { -1.0f, 0.0f } }))).WillOnce(testing::Return(0.0f)); + + const auto result{ optimization::LevenbergMarquardt{ rosenbrock, policy, config }.Solve(start) }; + + EXPECT_NEAR(result.x[0], 1.0f, 1e-5f); + EXPECT_NEAR(result.x[1], -3.84f, 1e-4f); + EXPECT_FALSE(result.converged); +} + +TEST_F(TestLevenbergMarquardt, step_is_clamped_to_max_step) +{ + config.maxIterations = 1; + config.maxStep = 1.0f; + + const auto result{ optimization::LevenbergMarquardt{ rosenbrock, gaussNewton, config }.Solve(start) }; + + EXPECT_NEAR(result.x[0], -0.786197f, 1e-5f); + EXPECT_NEAR(result.x[1], 0.089634f, 1e-5f); +} + +TEST_F(TestLevenbergMarquardt, retraction_keeps_a_wrapped_state) +{ + const UnitCircleAngle circle{ Vector2{ std::cos(2.5f), std::sin(2.5f) } }; + + const auto result{ optimization::LevenbergMarquardt{ circle, circleGaussNewton, config }.Solve(-2.5f) }; + + EXPECT_NEAR(result.x, 2.5f, 1e-4f); + EXPECT_TRUE(result.converged); + EXPECT_NEAR(circle.firstRetraction, 2.824261f, 1e-5f); + EXPECT_LE(circle.largestMagnitude, std::numbers::pi_v); +} + +TEST_F(TestLevenbergMarquardt, unreachable_target_reports_non_convergence) +{ + const UnitCircleAngle circle{ Vector2{ 2.0f, 0.0f } }; + const optimization::ConstantDamping damped{ 1.0f }; + + const auto result{ optimization::LevenbergMarquardt{ circle, damped, config }.Solve(0.3f) }; + + EXPECT_FALSE(result.converged); + EXPECT_EQ(result.iterations, config.maxIterations); + EXPECT_NEAR(result.x, 0.0f, 1e-5f); + EXPECT_NEAR(result.residualNorm, 1.0f, 1e-5f); +} + +TEST_F(TestLevenbergMarquardt, singular_undamped_system_stops_without_dividing_by_zero) +{ + const ConstantResidual constant; + + const auto result{ optimization::LevenbergMarquardt{ constant, gaussNewton, config }.Solve(start) }; + + EXPECT_FALSE(result.converged); + EXPECT_EQ(result.iterations, 0); + EXPECT_EQ(result.x[0], start[0]); + EXPECT_EQ(result.x[1], start[1]); +} diff --git a/roadmap/README.md b/roadmap/README.md index a6cd9782..00c8fec7 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -104,8 +104,6 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `math` `Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) -### `optimization` -`LevenbergMarquardt` (64) ### `control_analysis` `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) diff --git a/roadmap/optimization/LevenbergMarquardt/explanation.md b/roadmap/optimization/LevenbergMarquardt/explanation.md deleted file mode 100644 index 418fd882..00000000 --- a/roadmap/optimization/LevenbergMarquardt/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Levenberg–Marquardt — Overview - -## What it is -An iterative solver for "find the state that makes these residuals as small as possible" when the -residuals depend nonlinearly on the state. Each iteration linearizes the residuals, solves a small damped -linear least-squares problem for a correction, and applies it through a problem-defined update. - -## Why it matters (embedded) -Inverse kinematics, the forward kinematics of parallel robots, sensor and hand-eye calibration, and scan -matching are all small nonlinear least-squares problems solved every control cycle or at commissioning. -Writing one careful solver — bounded iterations, no heap, singularity-safe damping, a pluggable update for -angles, rotations and joint limits — replaces a hand-rolled Newton loop in every module. - -## How it works (intuition) -Near the current guess the residual behaves like a linear function of a small correction; Gauss–Newton -solves that linear problem exactly. When the linearization is poor or nearly singular, a damping term -penalizes large corrections, trading speed for robustness — the Levenberg–Marquardt idea. The correction -is applied by a "retraction" supplied by the problem: ordinary addition for plain vectors, addition plus -clamping for bounded states, or a rotation update for orientations, so the state never leaves its valid -set. - -## Key parameters -- **Damping policy** — constant `λ`, or a problem-aware schedule (e.g. zero away from singularities). -- **Tolerance** and **iteration budget** — stopping rule and worst-case cost. -- **Step cap** — maximum correction per iteration. - -## Reference -K. Levenberg, *Quarterly of Applied Mathematics*, 2(2), 1944; D. Marquardt, *SIAM J. Appl. Math.*, 11(2), -1963; J. Nocedal, S. Wright, *Numerical Optimization*, 2nd ed. (2006), Ch. 10; P.-A. Absil, R. Mahony, -R. Sepulchre, *Optimization Algorithms on Matrix Manifolds* (2008), Ch. 4 (retractions). - -## See also -`GradientDescent` / `Adam` (first-order optimizers for general costs), `LuDecomposition` (the step solve), -`DampedPseudoInverse` (#62, the same damping for one linear solve), `SE3Transform` (#53, pose retractions). diff --git a/roadmap/optimization/LevenbergMarquardt/implementation.md b/roadmap/optimization/LevenbergMarquardt/implementation.md deleted file mode 100644 index 756bc099..00000000 --- a/roadmap/optimization/LevenbergMarquardt/implementation.md +++ /dev/null @@ -1,118 +0,0 @@ -# Levenberg–Marquardt (damped Gauss–Newton with retraction) — Implementation Pseudocode - -> Roadmap ref: #64 (Tier 3) · Target: `numerical/optimization` · Namespace `optimization` · Type: `float` (templated on `T`, instantiated for `float` only) - -Bounded-iteration solver for small nonlinear least-squares problems `min ½‖r(x)‖²` whose state may live -on a manifold (an angle, a rotation, a pose) or inside bounds (joint limits). The problem supplies the -residual, its Jacobian with respect to a local increment `δ`, and a **retraction** `x ⊕ δ`; the solver -supplies the damped Gauss–Newton step. Consumers: pose inverse kinematics and parallel-robot forward -kinematics (robotics-toolbox-cpp), hand-eye and sensor calibration, scan matching. - -## Data structures - -```cpp -template -class LeastSquaresProblem: # pure interface; static_assert(std::is_floating_point_v) - virtual ~LeastSquaresProblem() = default - virtual math::Vector Residual(const State& x) const = 0 - virtual math::Matrix Jacobian(const State& x) const = 0 # ∂r(x ⊕ δ)/∂δ at δ = 0 - virtual State Retract(const State& x, const math::Vector& delta) const = 0 - # x ⊕ δ: plain addition, addition + clamping to bounds, or a manifold update (e.g. R ← Exp(δ)·R) - -template -class DampingPolicy: # pure interface - virtual ~DampingPolicy() = default - virtual T DampingSquared(const math::Matrix& jacobian) const = 0 # λ² ≥ 0 - -template -class ConstantDamping : public DampingPolicy: - T lambdaSquared # returns it unconditionally - -template -struct LevenbergMarquardtConfig: - T tolerance # on ‖r‖ - T maxStep # cap on ‖δ‖; +∞ disables - std::size_t maxIterations - -template -struct LevenbergMarquardtResult: - State x - T residualNorm # ‖r(x)‖ at the returned x - std::size_t iterations - bool converged - -template # instantiated for float -class LevenbergMarquardt: - const LeastSquaresProblem& problem # injected - const DampingPolicy& damping # injected - LevenbergMarquardtConfig config -``` - -## Interface - -```cpp -LevenbergMarquardt(const LeastSquaresProblem<...>& problem, const DampingPolicy<...>& damping, - const LevenbergMarquardtConfig& config) -LevenbergMarquardtResult Solve(const State& initial) const # hot path -``` - -## Algorithm (pseudocode) - -```text -function Solve(x0): # OPTIMIZE_FOR_SPEED - x = x0 - for iter in 0..maxIterations−1: - r = problem.Residual(x) - if ‖r‖ < tolerance: return { x, ‖r‖, iter, true } - J = problem.Jacobian(x) - λ² = damping.DampingSquared(J) - if Residuals ≤ Parameters: # wide or square: solve in residual space - lu = LuDecomposition(J·Jᵀ + λ²·I) - if !lu: return { x, ‖r‖, iter, false } # singular undamped system - δ = −Jᵀ·lu.Solve(r) # = −Jᵀ(JJᵀ + λ²I)⁻¹r, minimum-norm step - else: # tall: solve in parameter space - lu = LuDecomposition(Jᵀ·J + λ²·I) - if !lu: return { x, ‖r‖, iter, false } - δ = −lu.Solve(Jᵀ·r) - if ‖δ‖ > maxStep: δ = δ·maxStep/‖δ‖ - x = problem.Retract(x, δ) - rFinal = ‖problem.Residual(x)‖ # recomputed at the returned x - return { x, rFinal, maxIterations, rFinal < tolerance } -``` - -## Complexity & memory - -- Per iteration: one `Residual` + one `Jacobian` (problem cost), `O(Residuals·Parameters·m)` for the - normal matrix and an `O(m³)` LU, `m = min(Residuals, Parameters)`. -- Memory: one Jacobian, one `m×m` system; stack only, no heap. Deterministic upper bound: - `maxIterations + 1` residual evaluations. - -## Numerical / embedded notes - -- **Damping does not bias the answer:** at a fixed point with `J` of full rank the step is zero only when - `r = 0` (wide) or `Jᵀr = 0` (tall); damping slows convergence (linear instead of quadratic) but does not - move the solution. A non-zero final residual means an unreachable target, a singularity or an active - bound in `Retract`. -- **Policies:** `ConstantDamping` is plain Levenberg–Marquardt with fixed `λ` (`λ = 0`: Gauss–Newton). - A problem-aware policy — e.g. robotics-toolbox-cpp's manipulability-based IK damping, zero away from - singularities — plugs in through the same interface. Gain-ratio (Nielsen) adaptation, which needs a - trial evaluation and accept/reject, is a possible later variant. -- Choosing the smaller normal system keeps the solve `min(R, P)`-sized; use `LuDecomposition`, never an - explicit inverse. Upstream LU reports singular below a relative pivot of `1e-6`; with `λ = 0` a - singular `J` therefore ends the solve with `converged = false` instead of dividing by zero. -- `maxStep` bounds the change per iteration (trust-region-like safety for poor initial guesses). -- Warm-start from the previous control cycle for few-iteration convergence. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Headers: `numerical/optimization/LevenbergMarquardt.hpp` (+ `LeastSquaresProblem.hpp`, `DampingPolicy.hpp`, - pure interfaces) — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class LevenbergMarquardt, 2, 2>;` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/optimization/LevenbergMarquardt.cpp` → the same instantiation (and `ConstantDamping`). -- Test: `numerical/optimization/test/TestLevenbergMarquardt.cpp` -- Doc: `doc/optimization/LevenbergMarquardt.md` (per `doc/TEMPLATE.md`) + row in `doc/optimization/README.md` -- CMake: `.hpp` → `target_sources(numerical.optimization PRIVATE ...)` (link `numerical.solver` for LU); - `.cpp` → `numerical_add_coverage_sources(numerical.optimization ...)`; test → the optimization `_test` target. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/optimization/LevenbergMarquardt/tests.md b/roadmap/optimization/LevenbergMarquardt/tests.md deleted file mode 100644 index cc405e46..00000000 --- a/roadmap/optimization/LevenbergMarquardt/tests.md +++ /dev/null @@ -1,51 +0,0 @@ -# Levenberg–Marquardt — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestLevenbergMarquardt : public ::testing::Test: - RosenbrockResiduals rosenbrock # r = (10(y − x²), 1 − x), J = [[−20x, 10], [−1, 0]], Retract = add - UnitCircleAngle circle{ target } # State = angle θ; r = (cos θ, sin θ) − target; J = (−sin θ, cos θ)ᵀ; - # Retract = θ + δ wrapped to (−π, π] - ConstantDamping gaussNewton{ 0 } - StrictMock> policy - LevenbergMarquardtConfig config{ .tolerance = 1e-5, .maxStep = +∞, .maxIterations = 20 } -# each case below is a TEST_F(TestLevenbergMarquardt, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -gauss_newton_solves_rosenbrock_residuals_in_two_steps: - Act: LevenbergMarquardt{ rosenbrock, gaussNewton, config }.Solve((−1.2, 1)) - Assert: x ≈ (1, 1), converged, iterations == 2 -damping_policy_is_queried_with_the_current_jacobian: - Arrange: config.maxIterations = 1; EXPECT_CALL(policy, DampingSquared(J(−1.2, 1) = [[24, 10], [−1, 0]])) → 0 - Assert: x ≈ (1, −3.84), !converged -step_is_clamped_to_max_step: - Arrange: config.maxIterations = 1, config.maxStep = 1 - Assert: x ≈ (−0.786197, 0.089634) (undamped step (2.2, −4.84) scaled to unit length) -retraction_keeps_a_wrapped_state: - Arrange: circle target (cos 2.5, sin 2.5), θ₀ = −2.5 (1.28 rad away across ±π) - Assert: θ ≈ 2.5, converged, θ ∈ (−π, π] after every step (first step lands at 2.824261 via the wrap) -unreachable_target_reports_non_convergence: - Arrange: circle target (2, 0), ConstantDamping{ 1 }, θ₀ = 0.3 - Assert: !converged, iterations == maxIterations, θ ≈ 0, residualNorm ≈ 1 -singular_undamped_system_stops_without_dividing_by_zero: - Arrange: constant residual (1, 1), J ≡ 0, ConstantDamping{ 0 } - Assert: !converged, iterations == 0, x == x₀ -``` - -## Reference vectors - -- Rosenbrock from `(−1.2, 1)`, Gauss–Newton: `(1, −3.84)` after one step, `(1, 1)` after two (exact). -- First undamped step `(2.2, −4.84)`, norm `5.316540`. -- Circle, `θ₀ = −2.5`, target angle `2.5`: `θ = 2.824261 → 2.505653 → 2.500000` (python3). -- Unreachable target, `λ² = 1`: `θ ← θ − sin θ` ⇒ `θ → 0`, residual `‖(1, 0) − (2, 0)‖ = 1`. - -## Edge cases - -- `maxIterations = 0` ⇒ returns `x₀` with its residual, `converged` iff already within tolerance. -- Tall problem (`Residuals > Parameters`) and wide/square problem exercise the two solve branches. From af4b57f152e6263f88db3bd09096d03daea1c5ad Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 15:37:08 +0000 Subject: [PATCH 2/3] =?UTF-8?q?feat(solvers):=20add=20Goldfarb=E2=80=93Idn?= =?UTF-8?q?ani=20dual=20active-set=20QP?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Deploy roadmap item 65: dense strictly convex QP with equality and inequality constraints, using the Goldfarb–Idnani dual method in Powell's J = L⁻ᵀQ / R form with Givens add/drop updates. It needs no feasible start, adds equalities first, then the most violated inequality, and reports Optimal / Infeasible / NotPositiveDefinite / IterationLimit. QuadraticProgram is specialised for zero equalities or zero inequalities, since math::Matrix has no zero-size form. Change from the spec: a constraint counts as linearly dependent on the active set when ‖d₂‖ ≤ 1e-5·‖d‖ (relative), not when ‖z‖ ≤ ε. In float32 the absolute test reported 73 of 3000 infeasible random problems as optimal after a wild step. The relative test agrees with a brute-force KKT enumeration on all of them. The C++ solver was cross-checked on 3000 random problems of shapes (2,1,2), (2,0,4) and (4,2,8), including duplicated, negated, dependent and badly scaled rows: optimal x matches the oracle to ≈1e-4 relative, scaled KKT residual ≤ 4.4e-6, no negative inequality multipliers. Co-Authored-By: Claude Opus 5.5 Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5 --- README.md | 8 +- ROADMAP.md | 18 +- doc/solvers/ActiveSetQp.md | 148 +++++++ doc/solvers/README.md | 3 +- numerical/solvers/ActiveSetQp.cpp | 8 + numerical/solvers/ActiveSetQp.hpp | 406 ++++++++++++++++++ numerical/solvers/CMakeLists.txt | 2 + numerical/solvers/test/CMakeLists.txt | 1 + numerical/solvers/test/TestActiveSetQp.cpp | 115 +++++ roadmap/README.md | 4 +- roadmap/solvers/ActiveSetQp/explanation.md | 34 -- roadmap/solvers/ActiveSetQp/implementation.md | 127 ------ roadmap/solvers/ActiveSetQp/tests.md | 57 --- 13 files changed, 690 insertions(+), 241 deletions(-) create mode 100644 doc/solvers/ActiveSetQp.md create mode 100644 numerical/solvers/ActiveSetQp.cpp create mode 100644 numerical/solvers/ActiveSetQp.hpp create mode 100644 numerical/solvers/test/TestActiveSetQp.cpp delete mode 100644 roadmap/solvers/ActiveSetQp/explanation.md delete mode 100644 roadmap/solvers/ActiveSetQp/implementation.md delete mode 100644 roadmap/solvers/ActiveSetQp/tests.md diff --git a/README.md b/README.md index 5851457f..aa2d9922 100644 --- a/README.md +++ b/README.md @@ -25,10 +25,10 @@ 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, 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 | +| [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, Dense Convex QP (Goldfarb–Idnani) | +| [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 48c07198..644cd117 100644 --- a/ROADMAP.md +++ b/ROADMAP.md @@ -31,15 +31,13 @@ Difficulty legend: | 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ | | 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ | | 70 | Particle filter (bounded, systematic resampling) | `filters/active` | ★★★☆☆ | -| 65 | Dense convex QP (Goldfarb–Idnani dual active set) | `solvers` | ★★★★☆ | | 71 | Error-state (multiplicative) EKF on Lie groups | `filters/active` | ★★★★☆ | | 72 | Minimum-derivative (min-snap) piecewise polynomials | `motion_profiles` (new) | ★★★★☆ | | 73 | Online jerk-limited trajectory from arbitrary state | `motion_profiles` (new) | ★★★★★ | -Items 53–73 are the generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as -NT53–NT73); each also serves non-robotics consumers (motor control, navigation, calibration). All are -*(float-first)*. Items 66–73 serve its mobile, aerial, legged, marine and localization families and -have no spec yet. +Items 66–73 are generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as +NT66–NT73; NT53–NT65 are deployed). They serve its mobile, aerial, legged, marine and localization +families, also have non-robotics consumers, and have no spec yet. All are *(float-first)*. Items 48–52 are the **evaluation & metrics primitives** — reusable quantities the per-family unit-test reference [`TESTING.md`](TESTING.md) depends on but which @@ -99,16 +97,6 @@ the library does not yet expose. Detailed below under ## Tier 4 — Advanced ★★★★☆ -### 65. Dense convex QP -- **What:** Goldfarb–Idnani dual active-set solver for strictly convex QPs with equality and inequality - constraints; finite, bounded iterations. -- **Embedded value:** Exact constrained allocation (cable tensions, thrusters), task-priority control and - constrained MPC without heuristics. -- **Algorithm / paper:** D. Goldfarb, A. Idnani, *Mathematical Programming*, 27(1), 1983; M. J. D. Powell - (1985). -- **Reuses:** `CholeskyDecomposition`, `TriangularSolve`, `GivensRotation`. -- **Spec:** [roadmap/solvers/ActiveSetQp](roadmap/solvers/ActiveSetQp/implementation.md). - ### 71. Error-state Kalman filter on Lie groups - **What:** Multiplicative EKF whose nominal state lives on a group (quaternion/SO(3), SE(3)) and whose error state lives in the tangent space; predict, update, reset with the right Jacobians. diff --git a/doc/solvers/ActiveSetQp.md b/doc/solvers/ActiveSetQp.md new file mode 100644 index 00000000..01530405 --- /dev/null +++ b/doc/solvers/ActiveSetQp.md @@ -0,0 +1,148 @@ +# Active-Set Quadratic Programming (Goldfarb–Idnani) + +## Overview & Motivation + +Many embedded decisions are quadratic programs: distribute a wrench over cables that can only pull, allocate thrust to motors that saturate, track prioritised tasks under joint limits, keep model-predictive inputs inside bounds. Clipping the unconstrained answer, or running a fixed number of projection sweeps, is cheap but can be wrong at the edges, where several limits interact. + +This solver finds the exact minimiser of a strictly convex quadratic cost under linear equality and inequality constraints, in a bounded number of steps and without dynamic memory. It is a **dual active-set** method: it starts at the unconstrained minimum, which violates some constraints, and repeatedly pulls the point towards the most violated one along the direction that raises the cost least. Every intermediate point is optimal for the constraints chosen so far, so no feasible starting point is needed, and a running orthogonal factorisation keeps each step at quadratic cost. + +## Mathematical Theory + +### Problem Statement + +$$ +\min_{x \in \mathbb{R}^n} \; \tfrac12 x^\top H x + g^\top x \quad \text{s.t.} \quad E x = e, \;\; C x \ge c, +$$ + +with $H$ symmetric positive definite. Strict convexity makes the minimiser unique whenever the feasible set is non-empty. Write the rows of $E$ followed by the rows of $C$ as normals $n_i$ with bounds $b_i$, $m = $ (number of equalities) $+$ (number of inequalities). A bound $\ell_k \le x_k \le h_k$ is two inequality rows: $x_k \ge \ell_k$ and $-x_k \ge -h_k$. + +### Optimality Conditions + +A point $x$ is optimal exactly when there are multipliers with + +$$ +H x + g = E^\top \lambda + C^\top \mu, \qquad \mu \ge 0, \qquad \mu_i\,(n_i^\top x - b_i) = 0, +$$ + +and $x$ is feasible. Equality multipliers $\lambda$ are free in sign; inequality multipliers $\mu$ are non-negative and zero for inactive rows. The algorithm maintains stationarity and $\mu \ge 0$ at every iteration and reaches feasibility only at the end: the cost of the iterate rises monotonically towards the optimum. + +### Factored Form (Powell) + +Factor $H = L L^\top$ and let $N$ collect the active normals. The change of variables $y = L^\top x$ turns the cost into $\tfrac12\|y\|^2 + (L^{-1}g)^\top y$ and each normal into $L^{-1} n_i$. A QR factorisation of these transformed normals gives the stable pair + +$$ +J = L^{-\top} Q, \qquad J^\top N = \begin{bmatrix} R \\ 0 \end{bmatrix}, \qquad J^\top H J = I, +$$ + +with $Q$ orthogonal and $R$ upper triangular, $q \times q$ for $q$ active constraints. Split $J = [J_1\; J_2]$ at column $q$. The columns of $J_2$ satisfy $N^\top J_2 = 0$: moving along them keeps every active constraint satisfied. Since $J J^\top = H^{-1}$, products with $J$ replace any explicit inverse of $H$, which is what keeps the update numerically stable. + +### Adding a Constraint + +Let $p$ be the constraint to add, $n^+$ its normal oriented so that the current residual $s = n^{+\top} x - b^+$ is negative. Define + +$$ +d = J^\top n^+ = \begin{bmatrix} d_1 \\ d_2 \end{bmatrix}, \qquad z = J_2 d_2, \qquad r = R^{-1} d_1 . +$$ + +Then $n^+ = N r + H z$ and $n^{+\top} z = \|d_2\|^2$. Moving $x \leftarrow x + t z$, $u \leftarrow u - t r$ on the active multipliers and $u_p \leftarrow u_p + t$ preserves stationarity, leaves the active constraints satisfied, and changes the residual of $p$ by $t\,\|d_2\|^2$. Two events can stop the step: + +- **Full step** — the residual reaches zero: $t_2 = -s / (z^\top n^+)$, defined unless $n^+$ is linearly dependent on the active normals. +- **Partial step** — an active inequality multiplier reaches zero: $t_1 = \min\{u_j / r_j : r_j > 0\}$ over active inequalities. + +With $t = \min(t_1, t_2)$ the multipliers and, if $t_2$ is finite, $x$ are updated. If $t = t_2$, constraint $p$ becomes active. If $t = t_1$, the blocking constraint is dropped and the step repeats for the same $p$ with a smaller active set. If $t = \infty$, nothing bounds the multiplier of $p$ and the constraints are contradictory: the problem is infeasible. + +### Updating the Factors + +- **Add:** Givens rotations on adjacent columns of $J_2$ zero all but the first entry of $d_2$; the rotated $d$ becomes the new last column of $R$, which grows by one. +- **Drop:** deleting column $k$ of $R$ leaves it upper Hessenberg; Givens rotations on adjacent rows restore triangularity and are applied to the same columns of $J$ to keep $J^\top N = [R;0]$. + +### Linear Dependence Test + +In exact arithmetic $n^+$ depends on the active normals iff $d_2 = 0$. In single precision $d_2$ is a tiny nonzero vector, and an absolute test on $\|z\|$ lets the step $-s/(z^\top n^+)$ divide by a rounding artefact. The shipped solver therefore uses a relative test: + +$$ +\|d_2\| \le 10^{-5}\,\|d\| . +$$ + +Since $J$ transforms by an orthogonal $Q$, the ratio $\|d_2\|/\|d\|$ is the sine of the angle between the transformed new normal and the span of the active ones. A dependent constraint takes no full step; only a dual step with $t_1$ remains, which is how degenerate active sets resolve. + +### Termination + +Equalities are added first, in order; an equality whose residual is positive is added with its normal and bound negated, and its multiplier is reported with the original sign. Equalities are never dropped. Then, while some inactive inequality has residual below $-\tau\,\|n_i\|$ (relative tolerance, default $\tau = 10^{-6}$), the one with the most negative raw residual is added. Each completed addition raises the cost, so in exact arithmetic no active set repeats; an outer cap (default $2m + n$) and a bound on drops per addition guard against rounding-induced cycling. Hitting either returns an iteration-limit status with the current finite $x$, which is dual feasible but may violate constraints. + +## Complexity Analysis + +With $n$ variables, $m$ constraints, $q \le n$ active, and $K$ the iteration cap: + +| Case | Time | Space | Notes | +|------|------|-------|-------| +| Setup | $O(n^3)$ | $O(n^2)$ | Cholesky, triangular inverse $J = L^{-\top}$, start point $x = -J J^\top g$ | +| Best | $O(n^3 + mn)$ | $O(n^2 + m)$ | Unconstrained minimum already feasible: one residual scan | +| Average | $O(n^3 + k\,(n^2 + mn))$ | $O(n^2 + m)$ | $k$ additions, a few drops; per addition $d$, $z$, $r$ and rotations cost $O(n^2)$, the scan $O(mn)$ | +| Worst | $O(n^3 + K\,(n + m)\,n^2)$ | $O(n^2 + m)$ | Cap reached; loose bound, each addition allows at most $n + m + 1$ add/drop passes | + +Storage is $J$ and $R$ ($n \times n$ each), multipliers, signs and an active-set list of length at most $n$; all on the stack, no heap. + +## Step-by-Step Walkthrough + +Minimise $\tfrac12\|x\|^2$ ($H = I$, $g = 0$) subject to $x_2 \ge 1$ and $0.1\,x_1 + 0.5\,x_2 \ge 0.9$. Start at $x = 0$ with $J = I$; residuals are $-1$ and $-0.9$, so the first row is the most violated. + +| Step | Action | $t_1$ | $t_2$ | Result | +|------|--------|-------|-------|--------| +| 1 | Add row 1: $z = (0,1)$, no active set | $\infty$ | $1/1 = 1$ | $x = (0, 1)$, $u_1 = 1$, row 1 active | +| 2 | Add row 2: $n^+ = (0.1, 0.5)$; component along row 1 is $d_1 = 0.5$, orthogonal part $z = (0.1, 0)$, $r = 0.5$, residual $-0.4$ | $1/0.5 = 2$ | $0.4/0.01 = 40$ | Partial step $t = 2$: $x = (0.2, 1)$, $u_1 = 0$, $u_2 = 2$; row 1 dropped | +| 3 | Repeat row 2 with empty active set: $z = n^+$, residual $-0.38$ | $\infty$ | $0.38/0.26 = 1.4615$ | $x = (0.346154, 1.730769)$, $u_2 = 3.461538$ | + +No inactive row is violated ($x_2 = 1.73 \ge 1$), so the solver stops: $x = 0.9\,(0.1, 0.5)/0.26$, the projection onto the second row, with multiplier $0.9/0.26 = 3.461538$ and row 1 inactive. + +Other reference points: + +- $\min \tfrac12\|x - (1,1)\|^2$ s.t. $x_1 + x_2 \le 1$ (row $-x_1 - x_2 \ge -1$): residual $-1$, $t_2 = 1/2$, so $x = (0.5, 0.5)$ and $\mu = 0.5$. +- $\min \tfrac12\|x\|^2$ s.t. $x_1 + x_2 = 1$: residual $-1$, $t_2 = 1/2$, so $x = (0.5, 0.5)$ and $\lambda = 0.5$. +- $\min \tfrac12\|x - (2,-1)\|^2$ over the box $[0,1]^2$: the violated bounds have orthogonal normals, so both are added with full steps and no drop, giving $x = (1, 0)$ with multipliers $1$ and $1$. + +## Pitfalls & Edge Cases + +- **Strict convexity is required.** A semidefinite or indefinite $H$ makes the Cholesky factorisation fail and the solver returns a not-positive-definite status. For an LP-like problem add $\varepsilon I$; since $J$ scales as $1/\sqrt{\varepsilon}$, pick $\varepsilon$ large enough to keep the conditioning benign in single precision. +- **Linear-dependence threshold (single precision).** The textbook absolute test on $\|z\|$ failed: a dependent constraint leaves a tiny nonzero $z$ and the solver takes a wild step. In a 3000-problem random test it reported 73 infeasible problems as optimal. The relative test $\|d_2\| \le 10^{-5}\|d\|$ matched a brute-force enumeration of KKT active sets on all 3000; it still does at $10^{-4}$ and fails at $10^{-3}$. +- **Selection by raw residual.** The constraint added is the one with the largest raw violation; only the acceptance test is relative to the row norm. Scale rows to comparable norms, otherwise a large-norm row hides the others. +- **Dependent equalities.** A consistent but linearly dependent equality has no blocking multiplier and is reported infeasible. Remove redundant equalities beforehand. +- **Contradictory constraints.** Detected when the step is unbounded; the returned point is not a solution. +- **Early stop.** After an iteration limit the point is dual feasible, so its cost is a lower bound on the optimum, but it may violate constraints. +- **Degenerate vertices.** More than $n$ rows binding at one point is handled by the drop branch; no more than $n$ constraints are ever active. +- **Feasibility tolerance.** Acceptance tolerates a residual down to $-\tau\|n_i\|$, so the returned point can violate a row by about that amount. +- **Measured accuracy.** On 3000 random problems of shapes (2 variables, 1 equality, 2 inequalities), (2, 0, 4) and (4, 2, 8) with duplicated, negated, dependent and badly scaled rows, the optimal point matched the oracle to about $10^{-4}$ relative, the scaled KKT residual was at most $4.4\times10^{-6}$, primal infeasibility at most $1.2\times10^{-5}$, and no inequality multiplier was negative. + +## Variants & Generalizations + +- **Primal active set:** keeps every iterate feasible and needs a Phase I to find a feasible start; Goldfarb–Idnani needs none but requires strictly convex $H$. +- **Warm start:** reuse the previous active set and factors when consecutive problems differ slightly, as in a receding-horizon loop; a possible extension, not provided. +- **Interior-point and first-order (splitting) methods:** they accept semidefinite $H$, but return approximate solutions and no exact active set. +- **Specialised bounds:** handling $\ell_k \le x_k \le h_k$ implicitly avoids stating each bound as two dense rows. +- **Projected coordinate descent for boxes:** cheaper per iteration, but a fixed sweep count gives only an approximate answer. + +## Applications + +- **Control allocation:** distribute a demanded force and torque over saturating actuators, minimising effort exactly. +- **Cable-tension distribution:** find tensions that realise a wrench while every cable only pulls. +- **Task-priority and whole-body control:** tracking objectives with joint and torque limits. +- **Model predictive control:** input bounds, and box or rate constraints on a condensed problem. +- **Constrained trajectory generation:** minimum-snap segments with position and velocity limits. +- **Least-distance projection:** nearest point of a polytope, as in the box example above. + +## Connections to Other Algorithms + +| Algorithm | Relationship | +|-----------|--------------| +| Cholesky decomposition | Supplies $L$ with $H = L L^\top$, and is the test for strict convexity | +| Triangular solve | Builds $J = L^{-\top}$ once and back-substitutes $R^{-1} d_1$ every pass | +| Givens rotation | Restores the triangular form of $R$ when a constraint is added or dropped | +| QR decomposition | $J$ and $R$ are an incrementally updated QR of the active normals in the $H^{-1}$ metric | +| Damped pseudo-inverse | The unconstrained counterpart: with no active constraint the answer is $-H^{-1} g$; this solver adds exact limits | +| Model predictive control | Its input bounds are a direct use of the bound rows; the current fixed-count projected descent could be replaced by this solver | + +## References & Further Reading + +- D. Goldfarb and A. Idnani, "A numerically stable dual method for solving strictly convex quadratic programs," *Mathematical Programming*, 27(1), 1983. +- M. J. D. Powell, "On the quadratic programming algorithm of Goldfarb and Idnani," *Mathematical Programming Study*, 25, 1985. +- J. Nocedal and S. J. Wright, *Numerical Optimization*, 2nd ed., Springer, 2006, Ch. 16. diff --git a/doc/solvers/README.md b/doc/solvers/README.md index daf3395f..9df6fafc 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -19,4 +19,5 @@ 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 | +| [Active-Set QP (Goldfarb–Idnani)](ActiveSetQp.md) | Dense strictly convex QP with equality and inequality constraints via the dual active-set method — no feasible start needed | diff --git a/numerical/solvers/ActiveSetQp.cpp b/numerical/solvers/ActiveSetQp.cpp new file mode 100644 index 00000000..33dc04a2 --- /dev/null +++ b/numerical/solvers/ActiveSetQp.cpp @@ -0,0 +1,8 @@ +#include "numerical/solvers/ActiveSetQp.hpp" + +namespace solvers +{ + template class ActiveSetQp; + template class ActiveSetQp; + template class ActiveSetQp; +} diff --git a/numerical/solvers/ActiveSetQp.hpp b/numerical/solvers/ActiveSetQp.hpp new file mode 100644 index 00000000..8f4c59c8 --- /dev/null +++ b/numerical/solvers/ActiveSetQp.hpp @@ -0,0 +1,406 @@ +#pragma once + +#include "numerical/math/CholeskyDecomposition.hpp" +#include "numerical/math/CompilerOptimizations.hpp" +#include "numerical/math/GivensRotation.hpp" +#include "numerical/math/Math.hpp" +#include "numerical/math/Matrix.hpp" +#include "numerical/math/TriangularSolve.hpp" +#include +#include +#include +#include +#include +#include + +namespace solvers +{ + template + struct QuadraticProgram + { + static_assert(std::is_floating_point_v, "QuadraticProgram supports floating-point types only"); + + math::SquareMatrix hessian{}; + math::Vector gradient{}; + math::Matrix equalityMatrix{}; + math::Vector equalityVector{}; + math::Matrix inequalityMatrix{}; + math::Vector inequalityVector{}; + }; + + template + struct QuadraticProgram + { + static_assert(std::is_floating_point_v, "QuadraticProgram supports floating-point types only"); + + math::SquareMatrix hessian{}; + math::Vector gradient{}; + math::Matrix inequalityMatrix{}; + math::Vector inequalityVector{}; + }; + + template + struct QuadraticProgram + { + static_assert(std::is_floating_point_v, "QuadraticProgram supports floating-point types only"); + + math::SquareMatrix hessian{}; + math::Vector gradient{}; + math::Matrix equalityMatrix{}; + math::Vector equalityVector{}; + }; + + enum class QpStatus : std::uint8_t + { + Optimal, + Infeasible, + NotPositiveDefinite, + IterationLimit + }; + + template + struct QpResult + { + static_assert(std::is_floating_point_v, "QpResult supports floating-point types only"); + + math::Vector x{}; + math::Vector multipliers{}; + std::array active{}; + std::size_t iterations{ 0 }; + QpStatus status{ QpStatus::IterationLimit }; + }; + + template + class ActiveSetQp + { + static_assert(std::is_floating_point_v, "ActiveSetQp supports floating-point types only"); + static_assert(Equalities + Inequalities > 0, "ActiveSetQp requires at least one constraint"); + static_assert(Equalities <= Variables, "ActiveSetQp requires Equalities <= Variables"); + + static constexpr std::size_t Constraints{ Equalities + Inequalities }; + + public: + using Program = QuadraticProgram; + using Result = QpResult; + + explicit ActiveSetQp(std::size_t maxIterations = 2 * Constraints + Variables, T feasibilityTolerance = T{ 1e-6f }); + + OPTIMIZE_FOR_SPEED Result Solve(const Program& qp) const; + + private: + using VariableVector = math::Vector; + using Basis = math::SquareMatrix; + + enum class AddOutcome : std::uint8_t + { + Added, + Infeasible, + Stalled + }; + + struct State + { + Basis j{}; + Basis r{}; + VariableVector x{}; + std::array u{}; + std::array sign{}; + std::array activeSet{}; + std::size_t activeCount{ 0 }; + }; + + struct Direction + { + VariableVector d{}; + VariableVector z{}; + VariableVector r{}; + bool dependent{ false }; + }; + + struct Blocking + { + T step{ std::numeric_limits::infinity() }; + std::size_t position{ 0 }; + }; + + static constexpr T dependencyTolerance{ T{ 1e-5f } }; + static constexpr T infinity{ std::numeric_limits::infinity() }; + + static VariableVector Normal(const Program& qp, std::size_t i); + static T Bound(const Program& qp, std::size_t i); + static bool Initialize(const Program& qp, State& state); + bool SelectConstraint(const Program& qp, const State& state, std::size_t iteration, std::size_t& p) const; + static AddOutcome AddConstraint(const Program& qp, State& state, std::size_t p); + static Direction ComputeDirection(const State& state, const VariableVector& normal); + static Blocking FindBlocking(const State& state, const Direction& direction); + static void ActivateConstraint(State& state, VariableVector d, std::size_t p); + static void DropConstraint(State& state, std::size_t position); + static Result Finish(const State& state, std::size_t iterations, QpStatus status); + + std::size_t maxIterations; + T feasibilityTolerance; + }; + + template + ActiveSetQp::ActiveSetQp(std::size_t maxIterations, T feasibilityTolerance) + : maxIterations{ maxIterations } + , feasibilityTolerance{ feasibilityTolerance } + {} + + template + OPTIMIZE_FOR_SPEED auto ActiveSetQp::Solve(const Program& qp) const -> Result + { + State state{}; + if (!Initialize(qp, state)) + return Finish(state, 0, QpStatus::NotPositiveDefinite); + + for (std::size_t iteration = 0; iteration < maxIterations; ++iteration) + { + std::size_t p{ 0 }; + if (!SelectConstraint(qp, state, iteration, p)) + return Finish(state, iteration, QpStatus::Optimal); + + const AddOutcome outcome{ AddConstraint(qp, state, p) }; + if (outcome == AddOutcome::Infeasible) + return Finish(state, iteration, QpStatus::Infeasible); + if (outcome == AddOutcome::Stalled) + return Finish(state, iteration, QpStatus::IterationLimit); + } + + return Finish(state, maxIterations, QpStatus::IterationLimit); + } + + template + auto ActiveSetQp::Normal(const Program& qp, std::size_t i) -> VariableVector + { + if constexpr (Inequalities == 0) + return qp.equalityMatrix.template GetBlock<1, Variables>(i, 0).Transpose(); + else if constexpr (Equalities == 0) + return qp.inequalityMatrix.template GetBlock<1, Variables>(i, 0).Transpose(); + else if (i < Equalities) + return qp.equalityMatrix.template GetBlock<1, Variables>(i, 0).Transpose(); + else + return qp.inequalityMatrix.template GetBlock<1, Variables>(i - Equalities, 0).Transpose(); + } + + template + T ActiveSetQp::Bound(const Program& qp, std::size_t i) + { + if constexpr (Inequalities == 0) + return qp.equalityVector[i]; + else if constexpr (Equalities == 0) + return qp.inequalityVector[i]; + else if (i < Equalities) + return qp.equalityVector[i]; + else + return qp.inequalityVector[i - Equalities]; + } + + template + bool ActiveSetQp::Initialize(const Program& qp, State& state) + { + const auto l{ math::CholeskyDecomposition::TryFactor(qp.hessian) }; + if (!l) + return false; + + for (std::size_t k = 0; k < Variables; ++k) + { + VariableVector unit{}; + unit[k] = T{ 1 }; + const VariableVector column{ math::SolveLowerTriangular(*l, unit) }; + for (std::size_t i = 0; i < Variables; ++i) + state.j.at(k, i) = column[i]; + } + + state.x = state.j * (state.j.Transpose() * qp.gradient) * T{ -1 }; + state.sign.fill(T{ 1 }); + return true; + } + + template + bool ActiveSetQp::SelectConstraint(const Program& qp, const State& state, std::size_t iteration, std::size_t& p) const + { + if (iteration < Equalities) + { + p = iteration; + return true; + } + + T worst{ infinity }; + for (std::size_t i = Equalities; i < Constraints; ++i) + { + bool isActive{ false }; + for (std::size_t l = 0; l < state.activeCount; ++l) + isActive = isActive || state.activeSet[l] == i; + + const VariableVector normal{ Normal(qp, i) }; + const T slack{ (normal.Transpose() * state.x)[0] - Bound(qp, i) }; + const T scale{ math::Sqrt((normal.Transpose() * normal)[0]) }; + if (!isActive && slack < -feasibilityTolerance * scale && slack < worst) + { + worst = slack; + p = i; + } + } + + return worst < infinity; + } + + template + auto ActiveSetQp::AddConstraint(const Program& qp, State& state, std::size_t p) -> AddOutcome + { + if (p < Equalities && (Normal(qp, p).Transpose() * state.x)[0] > Bound(qp, p)) + state.sign[p] = T{ -1 }; + + const VariableVector normal{ Normal(qp, p) * state.sign[p] }; + const T bound{ Bound(qp, p) * state.sign[p] }; + T multiplier{}; + + for (std::size_t pass = 0; pass <= Variables + Constraints; ++pass) + { + const Direction direction{ ComputeDirection(state, normal) }; + + const Blocking blocking{ FindBlocking(state, direction) }; + const T slack{ (normal.Transpose() * state.x)[0] - bound }; + const T curvature{ (direction.z.Transpose() * normal)[0] }; + const T fullStep{ !direction.dependent && curvature > T{} ? -slack / curvature : infinity }; + const T step{ std::min(blocking.step, fullStep) }; + if (step == infinity) + return AddOutcome::Infeasible; + + for (std::size_t l = 0; l < state.activeCount; ++l) + state.u[state.activeSet[l]] -= step * direction.r[l]; + multiplier += step; + if (fullStep < infinity) + state.x += direction.z * step; + + if (step == fullStep) + { + state.u[p] = multiplier; + ActivateConstraint(state, direction.d, p); + return AddOutcome::Added; + } + + DropConstraint(state, blocking.position); + } + + return AddOutcome::Stalled; + } + + template + auto ActiveSetQp::ComputeDirection(const State& state, const VariableVector& normal) -> Direction + { + Direction direction{}; + direction.d = state.j.Transpose() * normal; + + const std::size_t q{ state.activeCount }; + T free{}; + T total{}; + for (std::size_t i = 0; i < Variables; ++i) + { + total += direction.d[i] * direction.d[i]; + if (i < q) + continue; + + free += direction.d[i] * direction.d[i]; + for (std::size_t row = 0; row < Variables; ++row) + direction.z[row] += state.j.at(row, i) * direction.d[i]; + } + direction.dependent = free <= dependencyTolerance * dependencyTolerance * total; + + for (std::size_t n = q; n > 0; --n) + { + const std::size_t i{ n - 1 }; + T sum{ direction.d[i] }; + for (std::size_t k = i + 1; k < q; ++k) + sum -= state.r.at(i, k) * direction.r[k]; + direction.r[i] = sum / state.r.at(i, i); + } + + return direction; + } + + template + auto ActiveSetQp::FindBlocking(const State& state, const Direction& direction) -> Blocking + { + Blocking blocking{}; + for (std::size_t l = 0; l < state.activeCount; ++l) + { + const std::size_t index{ state.activeSet[l] }; + if (index >= Equalities && direction.r[l] > T{} && state.u[index] / direction.r[l] < blocking.step) + blocking = { state.u[index] / direction.r[l], l }; + } + + return blocking; + } + + template + void ActiveSetQp::ActivateConstraint(State& state, VariableVector d, std::size_t p) + { + const std::size_t q{ state.activeCount }; + for (std::size_t i = Variables - 1; i > q; --i) + { + const auto rotation{ math::ComputeGivens(d[i - 1], d[i]) }; + math::ApplyGivens(rotation, d[i - 1], d[i]); + for (std::size_t row = 0; row < Variables; ++row) + math::ApplyGivens(rotation, state.j.at(row, i - 1), state.j.at(row, i)); + } + + for (std::size_t i = 0; i <= q; ++i) + state.r.at(i, q) = d[i]; + + state.activeSet[q] = p; + ++state.activeCount; + } + + template + void ActiveSetQp::DropConstraint(State& state, std::size_t position) + { + const std::size_t q{ state.activeCount }; + state.u[state.activeSet[position]] = T{}; + + for (std::size_t c = position; c + 1 < q; ++c) + { + state.activeSet[c] = state.activeSet[c + 1]; + for (std::size_t row = 0; row < Variables; ++row) + state.r.at(row, c) = state.r.at(row, c + 1); + } + for (std::size_t row = 0; row < Variables; ++row) + state.r.at(row, q - 1) = T{}; + + for (std::size_t i = position; i + 1 < q; ++i) + { + const auto rotation{ math::ComputeGivens(state.r.at(i, i), state.r.at(i + 1, i)) }; + for (std::size_t c = i; c < Variables; ++c) + math::ApplyGivens(rotation, state.r.at(i, c), state.r.at(i + 1, c)); + for (std::size_t row = 0; row < Variables; ++row) + math::ApplyGivens(rotation, state.j.at(row, i), state.j.at(row, i + 1)); + } + + --state.activeCount; + } + + template + auto ActiveSetQp::Finish(const State& state, std::size_t iterations, QpStatus status) -> Result + { + Result result{}; + result.x = state.x; + result.iterations = iterations; + result.status = status; + + for (std::size_t l = 0; l < state.activeCount; ++l) + { + const std::size_t index{ state.activeSet[l] }; + result.active[index] = true; + result.multipliers[index] = state.u[index] * state.sign[index]; + } + + return result; + } + +#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD + extern template class ActiveSetQp; + extern template class ActiveSetQp; + extern template class ActiveSetQp; +#endif +} diff --git a/numerical/solvers/CMakeLists.txt b/numerical/solvers/CMakeLists.txt index ae391dca..e97e39ad 100644 --- a/numerical/solvers/CMakeLists.txt +++ b/numerical/solvers/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(numerical.solver ${NUMERICAL_VISIBILITY} ) target_sources(numerical.solver PRIVATE + ActiveSetQp.hpp ConditionNumber.hpp DampedPseudoInverse.hpp DiscreteAlgebraicRiccatiEquation.hpp @@ -32,6 +33,7 @@ target_sources(numerical.solver PRIVATE ) numerical_add_coverage_sources(numerical.solver + ActiveSetQp.cpp DampedPseudoInverse.cpp DiscreteAlgebraicRiccatiEquation.cpp DurandKerner.cpp diff --git a/numerical/solvers/test/CMakeLists.txt b/numerical/solvers/test/CMakeLists.txt index 818a9777..cad72eb3 100644 --- a/numerical/solvers/test/CMakeLists.txt +++ b/numerical/solvers/test/CMakeLists.txt @@ -10,6 +10,7 @@ target_link_libraries(numerical.solvers_test PUBLIC numerical_link_qemu_runtime(numerical.solvers_test) target_sources(numerical.solvers_test PRIVATE + TestActiveSetQp.cpp TestConditionNumber.cpp TestDampedPseudoInverse.cpp TestDiscreteAlgebraicRiccatiEquation.cpp diff --git a/numerical/solvers/test/TestActiveSetQp.cpp b/numerical/solvers/test/TestActiveSetQp.cpp new file mode 100644 index 00000000..64aca9fe --- /dev/null +++ b/numerical/solvers/test/TestActiveSetQp.cpp @@ -0,0 +1,115 @@ +#include "numerical/solvers/ActiveSetQp.hpp" +#include +#include + +namespace +{ + using WithEquality = solvers::ActiveSetQp; + using BoxOnly = solvers::ActiveSetQp; + using Vector2 = math::Vector; + using Vector4 = math::Vector; + using Rows4 = math::Matrix; + + class TestActiveSetQp : public ::testing::Test + { + protected: + static constexpr float tolerance{ 1e-5f }; + WithEquality withEquality; + BoxOnly boxOnly; + + static BoxOnly::Program LeastDistance(const Vector2& reference, const Rows4& rows, const Vector4& bounds) + { + return { math::SquareMatrix::Identity(), reference * -1.0f, rows, bounds }; + } + + static void ExpectNear(const Vector2& actual, float x0, float x1) + { + EXPECT_NEAR(actual[0], x0, tolerance); + EXPECT_NEAR(actual[1], x1, tolerance); + } + }; +} + +TEST_F(TestActiveSetQp, inactive_constraints_return_the_unconstrained_minimum) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{ 1.0f, 1.0f }, Rows4{ { 1.0f, 0.0f }, { -1.0f, 0.0f }, { 0.0f, 1.0f }, { 0.0f, -1.0f } }, Vector4{ -5.0f, -5.0f, -5.0f, -5.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 1.0f, 1.0f); + for (std::size_t i = 0; i < 4; ++i) + { + EXPECT_EQ(result.multipliers[i], 0.0f); + EXPECT_FALSE(result.active[i]); + } +} + +TEST_F(TestActiveSetQp, equality_constraint_is_met_with_its_multiplier) +{ + const WithEquality::Program qp{ math::SquareMatrix::Identity(), Vector2{}, math::Matrix{ 1.0f, 1.0f }, math::Vector{ 1.0f }, + math::Matrix{ { 1.0f, 0.0f }, { 0.0f, 1.0f } }, Vector2{ -5.0f, -5.0f } }; + + const auto result{ withEquality.Solve(qp) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 0.5f, 0.5f); + EXPECT_NEAR(result.multipliers[0], 0.5f, tolerance); + EXPECT_TRUE(result.active[0]); +} + +TEST_F(TestActiveSetQp, violated_inequality_becomes_active) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{ 1.0f, 1.0f }, Rows4{ { -1.0f, -1.0f }, { 1.0f, 0.0f }, { 0.0f, 1.0f }, { -1.0f, 0.0f } }, Vector4{ -1.0f, -5.0f, -5.0f, -5.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 0.5f, 0.5f); + EXPECT_NEAR(result.multipliers[0], 0.5f, tolerance); + EXPECT_TRUE(result.active[0]); +} + +TEST_F(TestActiveSetQp, box_bounds_clip_to_the_nearest_corner) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{ 2.0f, -1.0f }, Rows4{ { 1.0f, 0.0f }, { -1.0f, 0.0f }, { 0.0f, 1.0f }, { 0.0f, -1.0f } }, Vector4{ 0.0f, -1.0f, 0.0f, -1.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 1.0f, 0.0f); + EXPECT_NEAR(result.multipliers[0], 0.0f, tolerance); + EXPECT_NEAR(result.multipliers[1], 1.0f, tolerance); + EXPECT_NEAR(result.multipliers[2], 1.0f, tolerance); + EXPECT_NEAR(result.multipliers[3], 0.0f, tolerance); +} + +TEST_F(TestActiveSetQp, blocking_constraint_is_dropped_on_the_way) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{}, Rows4{ { 0.0f, 1.0f }, { 0.1f, 0.5f }, { 1.0f, 0.0f }, { 0.0f, 1.0f } }, Vector4{ 1.0f, 0.9f, -5.0f, -5.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 0.346154f, 1.730769f); + EXPECT_FALSE(result.active[0]); + EXPECT_TRUE(result.active[1]); + EXPECT_NEAR(result.multipliers[0], 0.0f, tolerance); + EXPECT_NEAR(result.multipliers[1], 3.461538f, 1e-4f); +} + +TEST_F(TestActiveSetQp, contradictory_constraints_are_infeasible) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{}, Rows4{ { 1.0f, 0.0f }, { -1.0f, 0.0f }, { 0.0f, 1.0f }, { 0.0f, -1.0f } }, Vector4{ 1.0f, 0.0f, -5.0f, -5.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Infeasible); +} + +TEST_F(TestActiveSetQp, indefinite_hessian_is_rejected) +{ + auto qp{ LeastDistance(Vector2{}, Rows4{ { 1.0f, 0.0f }, { -1.0f, 0.0f }, { 0.0f, 1.0f }, { 0.0f, -1.0f } }, Vector4{ -5.0f, -5.0f, -5.0f, -5.0f }) }; + qp.hessian.at(1, 1) = -1.0f; + + EXPECT_EQ(boxOnly.Solve(qp).status, solvers::QpStatus::NotPositiveDefinite); +} + +TEST_F(TestActiveSetQp, redundant_constraints_do_not_break_the_solve) +{ + const auto result{ boxOnly.Solve(LeastDistance(Vector2{ 1.0f, 1.0f }, Rows4{ { -1.0f, -1.0f }, { -1.0f, -1.0f }, { 1.0f, 0.0f }, { 0.0f, 1.0f } }, Vector4{ -1.0f, -1.0f, -5.0f, -5.0f })) }; + + EXPECT_EQ(result.status, solvers::QpStatus::Optimal); + ExpectNear(result.x, 0.5f, 0.5f); + EXPECT_NEAR(result.multipliers[0] + result.multipliers[1], 0.5f, tolerance); +} diff --git a/roadmap/README.md b/roadmap/README.md index 00c8fec7..d37a9605 100644 --- a/roadmap/README.md +++ b/roadmap/README.md @@ -85,7 +85,6 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `controllers` `SaturationRateLimiter` (3) · `BangBangHysteresis` (4) · `Feedforward2Dof` (7) · `GainScheduledController` (10) · `LeadLagCompensator` (17) · `LuenbergerObserver` (19) · `IntegralStateFeedbackLqi` (20) - ### `robust_control` `SlidingModeControl` (34) · `DisturbanceObserver` (35) · `ActiveDisturbanceRejection` (36) · `HInfinityStateFeedback` (46) @@ -104,9 +103,8 @@ with `EXPECT_NEAR` and `math::Tolerance()` (or an explicit tolerance). ### `math` `Quaternion` (18) · `Cordic` (23) · `MatrixExponential` (29) · `ContinuousToDiscrete` (30) - ### `control_analysis` `ControllabilityObservability` (26) · `TransferFunctionStateSpace` (32) ### `solvers` -`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) · `ActiveSetQp` (65) +`RungeKuttaIntegrators` (24) · `QrDecomposition` (27) · `LuDecomposition` (28) · `LyapunovSylvester` (31) · `JacobiEigenSolver` (42) · `SingularValueDecomposition` (43) diff --git a/roadmap/solvers/ActiveSetQp/explanation.md b/roadmap/solvers/ActiveSetQp/explanation.md deleted file mode 100644 index bbee8465..00000000 --- a/roadmap/solvers/ActiveSetQp/explanation.md +++ /dev/null @@ -1,34 +0,0 @@ -# Dense Convex QP (Goldfarb–Idnani) — Overview - -## What it is -A solver for problems of the form "minimize a convex quadratic cost subject to linear equalities and -inequalities" — the workhorse of constrained control and allocation. It handles small, dense problems -exactly, in a bounded number of steps, without dynamic memory. - -## Why it matters (embedded) -Many embedded decisions are QPs: distribute a wrench over cables that can only pull, allocate thrust to -motors that saturate, track several tasks with priorities and joint limits, keep MPC inputs in bounds. -Heuristics such as clipping or a fixed number of projection sweeps are cheap but can be wrong at the -edges; an exact, finite active-set method gives the true optimum with a predictable worst case. - -## How it works (intuition) -Start from the unconstrained minimum, which is cheap thanks to a Cholesky factor of the cost. If it -violates a constraint, pull the solution towards that constraint along the direction that hurts the cost -least, raising its multiplier. If, on the way, an already active constraint's multiplier would turn -negative (it is pushing the wrong way), release it first. Repeat until nothing is violated. Because every -intermediate point is optimal for the constraints chosen so far, the method never needs a feasible start, -and a running orthogonal factorization keeps each step at quadratic cost. - -## Key parameters -- **Hessian `H`** — must be positive definite (strictly convex cost). -- **Constraint rows** — equalities are always enforced; inequalities only when they bind. -- **Iteration cap** and **feasibility tolerance** — worst-case cost and acceptance threshold. - -## Reference -D. Goldfarb, A. Idnani, "A numerically stable dual method for solving strictly convex quadratic -programs," *Mathematical Programming*, 27(1), 1983; M. J. D. Powell, "On the quadratic programming -algorithm of Goldfarb and Idnani," *Mathematical Programming Study*, 25, 1985. - -## See also -`CholeskyDecomposition`, `GivensRotation`, `Mpc` (box-constrained inputs), `DampedPseudoInverse` (#62, -the unconstrained counterpart); robotics-toolbox-cpp's cable-tension distribution (exact path). diff --git a/roadmap/solvers/ActiveSetQp/implementation.md b/roadmap/solvers/ActiveSetQp/implementation.md deleted file mode 100644 index c658e4b3..00000000 --- a/roadmap/solvers/ActiveSetQp/implementation.md +++ /dev/null @@ -1,127 +0,0 @@ -# Dense Convex QP (Goldfarb–Idnani dual active set) — Implementation Pseudocode - -> Roadmap ref: #65 (Tier 4) · Target: `numerical/solvers` · Namespace `solvers` · Type: `float` (templated on `T`, instantiated for `float` only) - -Exact solver for small, dense, strictly convex quadratic programs - -```text -minimize ½·xᵀHx + gᵀx subject to E·x = e, C·x ≥ c (H symmetric positive definite) -``` - -in a bounded number of iterations, with no heap. The dual active-set method starts from the -unconstrained minimum and adds the most violated constraint each iteration, so it needs no feasible -starting point and terminates finitely. Consumers: exact cable-tension distribution and task-priority / -whole-body control (robotics-toolbox-cpp), control allocation with actuator limits, `Mpc` with input -constraints (whose current box handling is a fixed-count projected coordinate descent), minimum-snap -trajectories. - -## Data structures - -```cpp -template -struct QuadraticProgram: # static_assert(std::is_floating_point_v) - math::SquareMatrix hessian # H, SPD - math::Vector gradient # g - math::Matrix equalityMatrix # E - math::Vector equalityVector # e - math::Matrix inequalityMatrix # C (rows: C_i·x ≥ c_i) - math::Vector inequalityVector # c - # bounds lo ≤ x ≤ hi are two inequality rows per bounded variable (+e_i ≥ lo_i, −e_i ≥ −hi_i) - -enum class QpStatus : uint8_t { Optimal, Infeasible, NotPositiveDefinite, IterationLimit } - -template # Constraints = Equalities + Inequalities -struct QpResult: - math::Vector x - math::Vector multipliers # equalities first; inequality multipliers ≥ 0, zero if inactive - std::array active - std::size_t iterations - QpStatus status - -template # instantiated for float -class ActiveSetQp: - std::size_t maxIterations # default 2·(Equalities + Inequalities) + Variables - T feasibilityTolerance # relative to the constraint-row scale -``` - -`math::Matrix` has no zero-size form, so `Equalities = 0` is a partial specialization that omits the -equality block (and `Inequalities = 0` likewise); all other code paths are shared. - -## Interface - -```cpp -explicit ActiveSetQp(std::size_t maxIterations = default, T feasibilityTolerance = 1e-6) -QpResult Solve(const QuadraticProgram<...>& qp) const # hot path -``` - -## Algorithm (pseudocode) - -Goldfarb & Idnani (1983) in the numerically stable form of Powell (1985): the active constraint normals -`N` are tracked through `J = L⁻ᵀ·Q` (`Q` orthogonal) and an upper-triangular `R` with `Jᵀ·N = [R; 0]`; -adding or dropping a constraint updates `J` and `R` with Givens rotations. - -```text -function Solve(qp): # OPTIMIZE_FOR_SPEED - L = Cholesky(H); if fails: return { status = NotPositiveDefinite } - J = L⁻ᵀ # triangular inverse, O(n³) once - x = −H⁻¹·g; q = 0 (active count); u = 0; active = none - pending = all equalities # equalities are added first, in order - for iter in 0..maxIterations−1: - # Step 1 — choose the constraint to add - if pending not empty: p = next equality; s = E_p·x − e_p - else: - s, p = min over inactive inequalities i of (C_i·x − c_i) # most violated (raw residual) - if s ≥ −feasibilityTolerance·‖C_p‖: return { x, u, active, iter, Optimal } - n⁺ = constraint normal of p (sign-flipped for an equality with s > 0, so s ≤ 0) - loop: # Step 2 — may drop several constraints before adding p - d = Jᵀ·n⁺ - z = J₂·d₂ # primal direction (columns q..n−1 of J) - r = R⁻¹·d₁ # dual direction for the active set (back substitution) - t₁ = min over active INEQUALITIES j with r_j > 0 of u_j / r_j (∞ if none; k = argmin) - t₂ = ‖z‖ > ε ? −s / (zᵀ·n⁺) : ∞ - t = min(t₁, t₂) - if t == ∞: return { x, u, active, iter, Infeasible } # p cannot be satisfied - u_active = u_active − t·r; u_p = u_p + t - if t₂ < ∞: x = x + t·z; s = s + t·(zᵀ·n⁺) - if t == t₂: # full step: p becomes active - add p: rotate d₂ into its first entry with Givens on J₂'s columns; append column to R; q += 1 - break - drop k: delete column k of R, restore upper-triangular with Givens (applied to J); q −= 1 - if p was the last pending equality: pending = empty - return { x, u, active, maxIterations, IterationLimit } -``` - -## Complexity & memory - -- Setup: one Cholesky and one triangular inverse, `O(n³)` with `n = Variables`. -- Per iteration: `O(n²)` for `d`, `z`, `r` and the Givens updates; at most `maxIterations` iterations - (finite termination is guaranteed in exact arithmetic; the cap guards rounding-induced cycling). -- Memory: `J` (`n×n`), `R` (`n×n`), multipliers and flags; stack only, no heap. - -## Numerical / embedded notes - -- **Strict convexity is required** (`H` SPD): a pure LP or a semidefinite `H` fails in Cholesky - (`NotPositiveDefinite`); add a small `ε·I` regularization when an LP-like problem must be solved this way. - robotics-toolbox-cpp's TOPP-RA keeps its own two-variable LP for that reason. -- Dual feasibility (multipliers of active inequalities ≥ 0) holds at every iteration; primal feasibility is - reached at termination — an early stop returns a super-optimal, slightly infeasible `x`. -- Scale constraint rows to comparable norms: the constraint to add is the largest *raw* violation, and - only the acceptance test is relative to `‖C_p‖`. -- Degenerate (linearly dependent) active constraints show up as `‖z‖ ≈ 0` with a blocking `t₁` — the - drop branch handles them; truly contradictory constraints give `t = ∞` ⇒ `Infeasible`. -- Warm start (re-using the previous active set) is a possible later extension for MPC loops. -- Float-only: `static_assert(std::is_floating_point_v)`. - -## Deployment - -- Header: `numerical/solvers/ActiveSetQp.hpp` — `#pragma once`, `OPTIMIZE_FOR_SPEED` on `Solve`, and - `extern template class ActiveSetQp;` / `` / `` under - `#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD`. -- Coverage: `numerical/solvers/ActiveSetQp.cpp` → the same instantiations. -- Test: `numerical/solvers/test/TestActiveSetQp.cpp` -- Doc: `doc/solvers/ActiveSetQp.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 ...)`; `TestActiveSetQp.cpp` → `numerical.solvers_test`. -- Follow-up: `Mpc` input constraints can switch from projected coordinate descent to this solver. -- Depends on: `math::CholeskyDecomposition`, `math::TriangularSolve`, `math::GivensRotation`. -- Generic pattern: see `roadmap/DEPLOYMENT.md`. diff --git a/roadmap/solvers/ActiveSetQp/tests.md b/roadmap/solvers/ActiveSetQp/tests.md deleted file mode 100644 index 18cb4427..00000000 --- a/roadmap/solvers/ActiveSetQp/tests.md +++ /dev/null @@ -1,57 +0,0 @@ -# Dense Convex QP (Goldfarb–Idnani) — Unit Test Plan (Pseudocode) - -> GoogleTest · `TEST_F` (`float`) · `StrictMock` only · no heap. - -## Fixture - -```cpp -class TestActiveSetQp : public ::testing::Test: - # least-distance problems: H = I, g = −x_ref ⇒ minimize ½‖x − x_ref‖² - ActiveSetQp withEquality - ActiveSetQp boxOnly -# each case below is a TEST_F(TestActiveSetQp, ) -``` - -## Test cases (Arrange / Act / Assert) - -```cpp -inactive_constraints_return_the_unconstrained_minimum: - Arrange: x_ref = (1, 1); box −5 ≤ x ≤ 5 - Assert: x ≈ (1, 1), status Optimal, all multipliers 0, no constraint active -equality_constraint_is_met_with_its_multiplier: - Arrange: x_ref = (0, 0); x₁ + x₂ = 1; inequalities x ≥ −5 (inactive) - Assert: x ≈ (0.5, 0.5), equality multiplier ≈ 0.5 -violated_inequality_becomes_active: - Arrange: boxOnly-style rows: −x₁ − x₂ ≥ −1 (x₁ + x₂ ≤ 1), x_ref = (1, 1) - Assert: x ≈ (0.5, 0.5), that multiplier ≈ 0.5, active -box_bounds_clip_to_the_nearest_corner: - Arrange: x_ref = (2, −1); 0 ≤ x ≤ 1 (four rows) - Assert: x ≈ (1, 0), multipliers of x₁ ≤ 1 and x₂ ≥ 0 ≈ 1, others 0 -blocking_constraint_is_dropped_on_the_way: - Arrange: x_ref = (0, 0); rows x₂ ≥ 1 (raw violation 1) and 0.1·x₁ + 0.5·x₂ ≥ 0.9 (raw violation 0.9) - Assert: x ≈ (0.346154, 1.730769), only the second row active, its multiplier ≈ 3.461538 - (x₂ ≥ 1 is added first, its multiplier turns negative when the second row joins, and it is dropped) -contradictory_constraints_are_infeasible: - Arrange: x₁ ≥ 1 and x₁ ≤ 0 - Assert: status Infeasible -indefinite_hessian_is_rejected: - Arrange: H = diag(1, −1) - Assert: status NotPositiveDefinite -redundant_constraints_do_not_break_the_solve: - Arrange: x₁ + x₂ ≤ 1 stated twice, x_ref = (1, 1) - Assert: x ≈ (0.5, 0.5), Optimal; the two multipliers sum to 0.5 -``` - -## Reference vectors - -- `min ½‖x − (1,1)‖²`, `x₁ + x₂ ≤ 1` ⇒ `x = (0.5, 0.5)`, multiplier `0.5` (KKT: `x − x_ref = −μ·(1, 1)`). -- `min ½‖x‖²`, `x₂ ≥ 1`, `0.1x₁ + 0.5x₂ ≥ 0.9` ⇒ projection onto the second row, - `x = 0.9·(0.1, 0.5)/0.26 = (0.346154, 1.730769)`, multiplier `0.9/0.26 = 3.461538`; `x₂ ≥ 1` holds there - (inactive). Projecting onto `x₂ = 1` first gives `(0, 1)`, which violates the second row (`0.5 < 0.9`). -- `min ½‖x − (2,−1)‖²`, `0 ≤ x ≤ 1` ⇒ `(1, 0)`, multipliers `1` and `1`. - -## Edge cases - -- `Equalities = Variables` (fully determined) ⇒ `x = E⁻¹e` regardless of `g`. -- Iteration cap reached on a pathological cycle ⇒ `IterationLimit`, finite `x`. -- Large constraint-row scale differences ⇒ tolerance is relative, still `Optimal`. From f02d2042fa90a39dd221dc1d0a84c02688f8e6dd Mon Sep 17 00:00:00 2001 From: Claude Date: Fri, 23 Oct 2026 16:26:07 +0000 Subject: [PATCH 3/3] 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/optimization/LevenbergMarquardt.md | 62 +++++++++++++------------- doc/optimization/README.md | 10 ++--- doc/solvers/ActiveSetQp.md | 36 +++++++-------- doc/solvers/README.md | 34 +++++++------- 5 files changed, 85 insertions(+), 85 deletions(-) diff --git a/README.md b/README.md index aa2d9922..fcaa2c06 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, Levenberg–Marquardt | -| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | -| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log) | -| [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, Levenberg–Marquardt | +| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) | +| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log) | +| [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, Dense Convex QP (Goldfarb–Idnani) | -| [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/optimization/LevenbergMarquardt.md b/doc/optimization/LevenbergMarquardt.md index 341dbc5a..e3729a22 100644 --- a/doc/optimization/LevenbergMarquardt.md +++ b/doc/optimization/LevenbergMarquardt.md @@ -34,10 +34,10 @@ $$ The push-through identity $(J^\top J + \lambda^2 I)^{-1}J^\top = J^\top (JJ^\top + \lambda^2 I)^{-1}$ gives an equivalent form, so the solver always factors the smaller system: -| Shape | Condition | System | Step | -|-------|-----------|--------|------| -| Tall | $R > P$ | $P \times P$, parameter space | $\delta = -(J^\top J + \lambda^2 I)^{-1}J^\top r$ | -| Wide or square | $R \le P$ | $R \times R$, residual space | $\delta = -J^\top (JJ^\top + \lambda^2 I)^{-1} r$ | +| Shape | Condition | System | Step | +|----------------|-----------|-------------------------------|---------------------------------------------------| +| Tall | $R > P$ | $P \times P$, parameter space | $\delta = -(J^\top J + \lambda^2 I)^{-1}J^\top r$ | +| Wide or square | $R \le P$ | $R \times R$, residual space | $\delta = -J^\top (JJ^\top + \lambda^2 I)^{-1} r$ | For $\lambda = 0$ and full row rank, the wide form is the minimum-norm solution of $J\delta = -r$: the shortest correction that cancels the linearized residual. The system is solved by LU decomposition with partial pivoting, never by an explicit inverse. If the factorization finds a pivot below a relative $10^{-6}$ (for example $\lambda = 0$ with a rank-deficient $J$), the solve ends unconverged at the current state instead of dividing by zero. For $\lambda^2 > 0$ the matrix is symmetric positive definite. @@ -58,31 +58,31 @@ The damped step vanishes exactly when $J^\top r = 0$. For a tall problem this is ## Complexity Analysis -| Item | Cost | Justification | -|------|------|---------------| -| Normal matrix | $O(R\,P\,m)$ | $J^\top J$ costs $R P^2$, $JJ^\top$ costs $P R^2$; $m = \min(R, P)$ | -| Factorization and solve | $O(m^3)$ | LU of an $m \times m$ matrix dominates the $O(m^2)$ triangular solves | -| Whole solve | at most $N$ iterations | each adds one residual and one Jacobian from the problem; $N + 1$ residual evaluations in all, the last at the returned state | -| Memory | one $R \times P$ Jacobian, one $m \times m$ system | stack only; no heap, no recursion | +| Item | Cost | Justification | +|-------------------------|----------------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------| +| Normal matrix | $O(R\,P\,m)$ | $J^\top J$ costs $R P^2$, $JJ^\top$ costs $P R^2$; $m = \min(R, P)$ | +| Factorization and solve | $O(m^3)$ | LU of an $m \times m$ matrix dominates the $O(m^2)$ triangular solves | +| Whole solve | at most $N$ iterations | each adds one residual and one Jacobian from the problem; $N + 1$ residual evaluations in all, the last at the returned state | +| Memory | one $R \times P$ Jacobian, one $m \times m$ system | stack only; no heap, no recursion | ## Step-by-Step Walkthrough **Rosenbrock residuals.** $r = (10(y - x^2),\; 1 - x)$, $J = \begin{pmatrix} -20x & 10 \\ -1 & 0 \end{pmatrix}$, zero residual at $(1, 1)$. Start at $(-1.2, 1)$ with $\lambda = 0$; $R = P = 2$, so the step is solved in residual space. -| Iteration | State | $r$ | $J$ | Step $\delta$ | New state | -|-----------|-------|-----|-----|---------------|-----------| -| 1 | $(-1.2, 1)$ | $(-4.4, 2.2)$, $\|r\| = 4.919$ | $\begin{pmatrix} 24 & 10 \\ -1 & 0 \end{pmatrix}$ | $(2.2, -4.84)$ | $(1, -3.84)$ | -| 2 | $(1, -3.84)$ | $(-48.4, 0)$, $\|r\| = 48.4$ | $\begin{pmatrix} -20 & 10 \\ -1 & 0 \end{pmatrix}$ | $(0, 4.84)$ | $(1, 1)$ | +| Iteration | State | $r$ | $J$ | Step $\delta$ | New state | +|-----------|--------------|--------------------------------|----------------------------------------------------|----------------|--------------| +| 1 | $(-1.2, 1)$ | $(-4.4, 2.2)$, $\|r\| = 4.919$ | $\begin{pmatrix} 24 & 10 \\ -1 & 0 \end{pmatrix}$ | $(2.2, -4.84)$ | $(1, -3.84)$ | +| 2 | $(1, -3.84)$ | $(-48.4, 0)$, $\|r\| = 48.4$ | $\begin{pmatrix} -20 & 10 \\ -1 & 0 \end{pmatrix}$ | $(0, 4.84)$ | $(1, 1)$ | The second residual is linear in $x$, so step 1 zeroes it exactly. The first is then linear in $y$, so step 2 finishes exactly. The residual norm rises from $4.9$ to $48.4$ on the way: the iteration is not monotone. With a step cap $\Delta = 1$, the first step of norm $5.316540$ is scaled to unit length, giving $(-0.786197,\; 0.089634)$. **Angle on the unit circle.** State $\theta$, $r = (\cos\theta, \sin\theta) - (\cos 2.5, \sin 2.5)$, $J = (-\sin\theta,\; \cos\theta)^\top$. Here $R = 2 > P = 1$ and $J^\top J = 1$, so with $\lambda = 0$ each step is $\theta \leftarrow \theta - \sin(\theta - 2.5)$, wrapped into $(-\pi, \pi]$. Start at $\theta = -2.5$, which is only $1.28$ rad from the target across the $\pm\pi$ seam. -| Iteration | $\theta$ | $\sin(\theta - 2.5)$ | Before wrap | After wrap | -|-----------|----------|----------------------|-------------|------------| -| 1 | $-2.500000$ | $0.958924$ | $-3.458924$ | $2.824261$ | -| 2 | $2.824261$ | $0.318608$ | $2.505653$ | $2.505653$ | -| 3 | $2.505653$ | $0.005653$ | $2.500000$ | $2.500000$ | +| Iteration | $\theta$ | $\sin(\theta - 2.5)$ | Before wrap | After wrap | +|-----------|-------------|----------------------|-------------|------------| +| 1 | $-2.500000$ | $0.958924$ | $-3.458924$ | $2.824261$ | +| 2 | $2.824261$ | $0.318608$ | $2.505653$ | $2.505653$ | +| 3 | $2.505653$ | $0.005653$ | $2.500000$ | $2.500000$ | The wrap keeps the state in its canonical range, so the answer is $2.5$ and not $2.5 - 2\pi$. The error falls $0.324 \to 0.0057 \to 3\times10^{-8}$, faster than quadratic because of the circle's symmetry. @@ -99,11 +99,11 @@ The wrap keeps the state in its canonical range, so the answer is $2.5$ and not ## Variants & Generalizations -| Variant | Key difference | -|---------|----------------| -| **Gauss–Newton ($\lambda = 0$)** | Quadratic convergence for zero-residual problems; fails on rank deficiency | -| **Constant or problem-aware damping (this)** | Fixed $\lambda$ gives robust linear convergence; a Jacobian-dependent $\lambda^2$ (zero away from singularities) keeps the fast rate where safe | -| **Gain-ratio (Nielsen) adaptation** | Raises or lowers $\lambda$ from the ratio of actual to predicted cost reduction; needs a trial evaluation and accept/reject, a possible later variant | +| Variant | Key difference | +|----------------------------------------------|-------------------------------------------------------------------------------------------------------------------------------------------------------| +| **Gauss–Newton ($\lambda = 0$)** | Quadratic convergence for zero-residual problems; fails on rank deficiency | +| **Constant or problem-aware damping (this)** | Fixed $\lambda$ gives robust linear convergence; a Jacobian-dependent $\lambda^2$ (zero away from singularities) keeps the fast rate where safe | +| **Gain-ratio (Nielsen) adaptation** | Raises or lowers $\lambda$ from the ratio of actual to predicted cost reduction; needs a trial evaluation and accept/reject, a possible later variant | ## Applications @@ -114,13 +114,13 @@ The wrap keeps the state in its canonical range, so the answer is $2.5$ and not ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| [LU Decomposition](../solvers/LuDecomposition.md) | Solves the damped normal system; its singularity report ends the solve unconverged | -| [Damped Pseudo-Inverse](../solvers/DampedPseudoInverse.md) | The same damped step in SVD form for one linear solve; avoids squaring the condition number at higher cost | -| [SE(3) Transform](../math/SE3Transform.md) | Supplies the manifold retraction for pose states | -| [Angle Utilities](../math/AngleUtilities.md) | Supplies the wrapped retraction for angle states | -| [Gradient Descent](Optimizer.md) | First-order method for general costs; this method exploits the sum-of-squares structure through the Gauss–Newton curvature and needs far fewer iterations on small problems | +| Algorithm | Relationship | +|------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------| +| [LU Decomposition](../solvers/LuDecomposition.md) | Solves the damped normal system; its singularity report ends the solve unconverged | +| [Damped Pseudo-Inverse](../solvers/DampedPseudoInverse.md) | The same damped step in SVD form for one linear solve; avoids squaring the condition number at higher cost | +| [SE(3) Transform](../math/SE3Transform.md) | Supplies the manifold retraction for pose states | +| [Angle Utilities](../math/AngleUtilities.md) | Supplies the wrapped retraction for angle states | +| [Gradient Descent](Optimizer.md) | First-order method for general costs; this method exploits the sum-of-squares structure through the Gauss–Newton curvature and needs far fewer iterations on small problems | ## References & Further Reading diff --git a/doc/optimization/README.md b/doc/optimization/README.md index 91877965..dc7e2c2c 100644 --- a/doc/optimization/README.md +++ b/doc/optimization/README.md @@ -4,9 +4,9 @@ General-purpose optimization algorithms for parameter fitting, training, and con ## Algorithms -| Algorithm | Description | -|--------------------------------------------------|---------------------------------------------------------------------------------------------| -| [Bayesian Optimization](BayesianOptimization.md) | Gradient-free global optimization using Gaussian Process surrogate and Expected Improvement | -| [Gradient Descent](Optimizer.md) | Iterative batch optimization via gradient-based weight updates | -| [SGD / Adam Step Optimisers](StepOptimizer.md) | Stateful per-step SGD (with optional momentum/Nesterov) and Adam with bias correction | +| Algorithm | Description | +|--------------------------------------------------|---------------------------------------------------------------------------------------------------------------------------| +| [Bayesian Optimization](BayesianOptimization.md) | Gradient-free global optimization using Gaussian Process surrogate and Expected Improvement | +| [Gradient Descent](Optimizer.md) | Iterative batch optimization via gradient-based weight updates | +| [SGD / Adam Step Optimisers](StepOptimizer.md) | Stateful per-step SGD (with optional momentum/Nesterov) and Adam with bias correction | | [Levenberg–Marquardt](LevenbergMarquardt.md) | Damped Gauss–Newton for small nonlinear least squares with a retraction (manifold / bounded states) and pluggable damping | diff --git a/doc/solvers/ActiveSetQp.md b/doc/solvers/ActiveSetQp.md index 01530405..64e348d0 100644 --- a/doc/solvers/ActiveSetQp.md +++ b/doc/solvers/ActiveSetQp.md @@ -74,12 +74,12 @@ Equalities are added first, in order; an equality whose residual is positive is With $n$ variables, $m$ constraints, $q \le n$ active, and $K$ the iteration cap: -| Case | Time | Space | Notes | -|------|------|-------|-------| -| Setup | $O(n^3)$ | $O(n^2)$ | Cholesky, triangular inverse $J = L^{-\top}$, start point $x = -J J^\top g$ | -| Best | $O(n^3 + mn)$ | $O(n^2 + m)$ | Unconstrained minimum already feasible: one residual scan | -| Average | $O(n^3 + k\,(n^2 + mn))$ | $O(n^2 + m)$ | $k$ additions, a few drops; per addition $d$, $z$, $r$ and rotations cost $O(n^2)$, the scan $O(mn)$ | -| Worst | $O(n^3 + K\,(n + m)\,n^2)$ | $O(n^2 + m)$ | Cap reached; loose bound, each addition allows at most $n + m + 1$ add/drop passes | +| Case | Time | Space | Notes | +|---------|----------------------------|--------------|------------------------------------------------------------------------------------------------------| +| Setup | $O(n^3)$ | $O(n^2)$ | Cholesky, triangular inverse $J = L^{-\top}$, start point $x = -J J^\top g$ | +| Best | $O(n^3 + mn)$ | $O(n^2 + m)$ | Unconstrained minimum already feasible: one residual scan | +| Average | $O(n^3 + k\,(n^2 + mn))$ | $O(n^2 + m)$ | $k$ additions, a few drops; per addition $d$, $z$, $r$ and rotations cost $O(n^2)$, the scan $O(mn)$ | +| Worst | $O(n^3 + K\,(n + m)\,n^2)$ | $O(n^2 + m)$ | Cap reached; loose bound, each addition allows at most $n + m + 1$ add/drop passes | Storage is $J$ and $R$ ($n \times n$ each), multipliers, signs and an active-set list of length at most $n$; all on the stack, no heap. @@ -87,11 +87,11 @@ Storage is $J$ and $R$ ($n \times n$ each), multipliers, signs and an active-set Minimise $\tfrac12\|x\|^2$ ($H = I$, $g = 0$) subject to $x_2 \ge 1$ and $0.1\,x_1 + 0.5\,x_2 \ge 0.9$. Start at $x = 0$ with $J = I$; residuals are $-1$ and $-0.9$, so the first row is the most violated. -| Step | Action | $t_1$ | $t_2$ | Result | -|------|--------|-------|-------|--------| -| 1 | Add row 1: $z = (0,1)$, no active set | $\infty$ | $1/1 = 1$ | $x = (0, 1)$, $u_1 = 1$, row 1 active | -| 2 | Add row 2: $n^+ = (0.1, 0.5)$; component along row 1 is $d_1 = 0.5$, orthogonal part $z = (0.1, 0)$, $r = 0.5$, residual $-0.4$ | $1/0.5 = 2$ | $0.4/0.01 = 40$ | Partial step $t = 2$: $x = (0.2, 1)$, $u_1 = 0$, $u_2 = 2$; row 1 dropped | -| 3 | Repeat row 2 with empty active set: $z = n^+$, residual $-0.38$ | $\infty$ | $0.38/0.26 = 1.4615$ | $x = (0.346154, 1.730769)$, $u_2 = 3.461538$ | +| Step | Action | $t_1$ | $t_2$ | Result | +|------|---------------------------------------------------------------------------------------------------------------------------------|-------------|----------------------|---------------------------------------------------------------------------| +| 1 | Add row 1: $z = (0,1)$, no active set | $\infty$ | $1/1 = 1$ | $x = (0, 1)$, $u_1 = 1$, row 1 active | +| 2 | Add row 2: $n^+ = (0.1, 0.5)$; component along row 1 is $d_1 = 0.5$, orthogonal part $z = (0.1, 0)$, $r = 0.5$, residual $-0.4$ | $1/0.5 = 2$ | $0.4/0.01 = 40$ | Partial step $t = 2$: $x = (0.2, 1)$, $u_1 = 0$, $u_2 = 2$; row 1 dropped | +| 3 | Repeat row 2 with empty active set: $z = n^+$, residual $-0.38$ | $\infty$ | $0.38/0.26 = 1.4615$ | $x = (0.346154, 1.730769)$, $u_2 = 3.461538$ | No inactive row is violated ($x_2 = 1.73 \ge 1$), so the solver stops: $x = 0.9\,(0.1, 0.5)/0.26$, the projection onto the second row, with multiplier $0.9/0.26 = 3.461538$ and row 1 inactive. @@ -132,13 +132,13 @@ Other reference points: ## Connections to Other Algorithms -| Algorithm | Relationship | -|-----------|--------------| -| Cholesky decomposition | Supplies $L$ with $H = L L^\top$, and is the test for strict convexity | -| Triangular solve | Builds $J = L^{-\top}$ once and back-substitutes $R^{-1} d_1$ every pass | -| Givens rotation | Restores the triangular form of $R$ when a constraint is added or dropped | -| QR decomposition | $J$ and $R$ are an incrementally updated QR of the active normals in the $H^{-1}$ metric | -| Damped pseudo-inverse | The unconstrained counterpart: with no active constraint the answer is $-H^{-1} g$; this solver adds exact limits | +| Algorithm | Relationship | +|--------------------------|---------------------------------------------------------------------------------------------------------------------------------| +| Cholesky decomposition | Supplies $L$ with $H = L L^\top$, and is the test for strict convexity | +| Triangular solve | Builds $J = L^{-\top}$ once and back-substitutes $R^{-1} d_1$ every pass | +| Givens rotation | Restores the triangular form of $R$ when a constraint is added or dropped | +| QR decomposition | $J$ and $R$ are an incrementally updated QR of the active normals in the $H^{-1}$ metric | +| Damped pseudo-inverse | The unconstrained counterpart: with no active constraint the answer is $-H^{-1} g$; this solver adds exact limits | | Model predictive control | Its input bounds are a direct use of the bound rows; the current fixed-count projected descent could be replaced by this solver | ## References & Further Reading diff --git a/doc/solvers/README.md b/doc/solvers/README.md index 9df6fafc..d62fc344 100644 --- a/doc/solvers/README.md +++ b/doc/solvers/README.md @@ -4,20 +4,20 @@ Numerical solvers for linear systems, polynomial roots, and matrix equations. ## Algorithms -| Algorithm | Description | -|----------------------------------------------------------------------------|----------------------------------------------------------------------------------------------------------------| -| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | -| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | -| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | -| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | -| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | -| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | -| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | -| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | -| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | -| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | -| [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 | -| [Active-Set QP (Goldfarb–Idnani)](ActiveSetQp.md) | Dense strictly convex QP with equality and inequality constraints via the dual active-set method — no feasible start needed | +| Algorithm | Description | +|----------------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------| +| [Gaussian Elimination](GaussianElimination.md) | Direct solver for dense linear systems using partial pivoting | +| [Levinson-Durbin](LevinsonDurbin.md) | Fast solver for Toeplitz linear systems exploiting structural symmetry | +| [Durand-Kerner](DurandKerner.md) | Simultaneous iterative root-finder for polynomials | +| [Discrete Algebraic Riccati Equation](DiscreteAlgebraicRiccatiEquation.md) | Iterative solver for the DARE arising in LQR and Kalman filter design | +| [Runge-Kutta ODE Integrators](RungeKuttaIntegrators.md) | Fixed-step RK4 and adaptive Dormand-Prince RK45 for ODE integration | +| [Spectral Radius](SpectralRadius.md) | Dominant eigenvalue magnitude and discrete-time Schur stability margin | +| [Condition Number](ConditionNumber.md) | 1-norm condition number estimate via column-wise inverse solve | +| [QR Decomposition](QrDecomposition.md) | Householder factorization and Givens streaming row update for least-squares solves | +| [LU Decomposition](LuDecomposition.md) | PA = LU factorization with partial pivoting for general dense linear systems | +| [Lyapunov / Sylvester Solvers](LyapunovSylvester.md) | Sylvester AX+XB=C and continuous/discrete Lyapunov solvers via Kronecker vectorisation | +| [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 | +| [Active-Set QP (Goldfarb–Idnani)](ActiveSetQp.md) | Dense strictly convex QP with equality and inequality constraints via the dual active-set method — no feasible start needed |