Skip to content

Commit 716050e

Browse files
claudeclaude
committed
feat(optimization): add Levenberg–Marquardt with retraction and damping policy
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 <noreply@anthropic.com> Claude-Session: https://claude.ai/code/session_01Q53m7MZDmHwocmKbhhd2o5
1 parent c3a69f1 commit 716050e

15 files changed

Lines changed: 543 additions & 233 deletions

File tree

‎README.md‎

Lines changed: 11 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -14,17 +14,17 @@ Refer to the documentation to quickly integrate and utilize the library's signal
1414

1515
## Documentation
1616

17-
| Category | Description |
18-
|--------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
19-
| [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 |
20-
| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space |
21-
| [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) |
22-
| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) |
23-
| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, 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 |
24-
| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers |
25-
| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) |
26-
| [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) |
27-
| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile |
17+
| Category | Description |
18+
|--------------------------------------------------------------------|----------------------------------------------------------------------|
19+
| [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 |
20+
| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space |
21+
| [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) |
22+
| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) |
23+
| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, 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 |
24+
| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers, Levenberg–Marquardt |
25+
| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) |
26+
| [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) |
27+
| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile |
2828
| [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 |
2929
| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) |
3030
| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control |

‎ROADMAP.md‎

Lines changed: 3 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -24,10 +24,9 @@ Difficulty legend:
2424

2525
## Master list (by priority)
2626

27-
| # | Component | Target module | Difficulty |
28-
|----|-----------------------------------------------------|----------------|------------|
29-
| 64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | ★★★☆☆ |
30-
| 65 | Dense convex QP (Goldfarb–Idnani dual active set) | `solvers` | ★★★★☆ |
27+
| # | Component | Target module | Difficulty |
28+
|----|------------------------------------------------------|-------------------------|------------|
29+
| 65 | Dense convex QP (Goldfarb–Idnani dual active set) | `solvers` | ★★★★☆ |
3130

3231
Items 53–65 are the generic building blocks of robotics-toolbox-cpp's roadmap (referenced there as
3332
NT53–NT65); each also serves non-robotics consumers (motor control, navigation, calibration). All are
@@ -38,19 +37,6 @@ unit-test reference [`TESTING.md`](TESTING.md) depends on but which
3837
the library does not yet expose. Detailed below under
3938
[Evaluation & metrics primitives](#evaluation--metrics-primitives).
4039

41-
## Tier 3 — Moderate ★★★☆☆
42-
43-
### 64. Levenberg–Marquardt
44-
- **What:** Damped Gauss–Newton for small nonlinear least squares with a problem-supplied retraction
45-
(vectors, bounds, rotations) and a pluggable damping policy.
46-
- **Embedded value:** One bounded, heap-free solver for IK, parallel-robot forward kinematics, calibration
47-
and scan matching.
48-
- **Algorithm / paper:** K. Levenberg (1944); D. Marquardt (1963); J. Nocedal, S. Wright (2006), Ch. 10.
49-
- **Reuses:** `LuDecomposition`.
50-
- **Spec:** [roadmap/optimization/LevenbergMarquardt](roadmap/optimization/LevenbergMarquardt/implementation.md).
51-
52-
---
53-
5440
## Tier 4 — Advanced ★★★★☆
5541

5642
### 65. Dense convex QP

0 commit comments

Comments
 (0)