Skip to content

feat: add Levenberg–Marquardt and Goldfarb–Idnani active-set QP #358

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
gabrielfrasantos merged 3 commits into from
Oct 23, 2026
Merged

feat: add Levenberg–Marquardt and Goldfarb–Idnani active-set QP #358

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
599a35e
feat(optimization): add Levenberg–Marquardt with retraction and dampi…
claude Oct 23, 2026
af4b57f
feat(solvers): add Goldfarb–Idnani dual active-set QP
claude Oct 23, 2026
f02d204
docs: align markdown tables
claude Oct 23, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
30 changes: 15 additions & 15 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,21 +14,21 @@ Refer to the documentation to quickly integrate and utilize the library's signal

## Documentation

| Category | Description |
|--------------------------------------------------------------------|-----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
| [Analysis](doc/analysis/README.md) | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Discrete Wavelet Transform (Haar/Daubechies), Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm, Decibels, Hilbert Transform / Analytic Signal, Mel Filterbank / MFCC |
| [Control Analysis](doc/control_analysis/README.md) | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians, Continuous-to-Discrete, Transfer Function ↔ State Space |
| [Controllers](doc/controllers/README.md) | Bang-Bang/Hysteresis, Deadbeat Control, PID, LQR, LQI (Integral/Servo State Feedback), MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer, Friction Compensation (Coulomb + Viscous + Stribeck) |
| [Estimators](doc/estimators/README.md) | Linear Regression, Polynomial Fitting, Total Least Squares, Yule-Walker (offline), Recursive Least Squares, Recursive Instrumental Variables, LMS / NLMS Adaptive Filter (online), Consistency Metrics / NEES / NIS, Incremental QR Least Squares (rank-revealing) |
| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS |
| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers |
| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) |
| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver, Cubic Spline Interpolation, SE(3) Transform (Twists, Wrenches, Exp/Log) |
| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile |
| [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 |
| 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 |

Each category page lists its algorithms with a brief description and links to the detailed documentation.

Expand Down
28 changes: 3 additions & 25 deletions ROADMAP.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,19 +28,16 @@ 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` | ★★★☆☆ |
| 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
Expand Down Expand Up @@ -70,15 +67,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.
Expand Down Expand Up @@ -109,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.
Expand Down
Loading
Loading