Skip to content

feat: add cubic spline, damped pseudo-inverse, incremental QR least squares and SE(3) transform #357

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 6 commits into from
Oct 23, 2026
Merged

feat: add cubic spline, damped pseudo-inverse, incremental QR least squares and SE(3) transform #357

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
046c0b0
feat(math): add vector-valued cubic spline interpolation
claude Oct 23, 2026
9d1d1c6
feat(solvers): add damped pseudo-inverse and null-space projector
claude Oct 23, 2026
3497912
feat(estimators): add incremental QR least squares
claude Oct 23, 2026
6045e8b
feat(math): add SE(3) transform with twist/wrench algebra and exp/log
claude Oct 23, 2026
019df56
docs: align markdown tables
claude Oct 23, 2026
920b43d
refactor(math): make SE3Transform composition a hidden friend
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 |
| [Filters](doc/filters/README.md) | Kalman, Extended Kalman, Unscented Kalman, Square-Root Kalman, Alpha-Beta/Alpha-Beta-Gamma, FIR, IIR, Exponential Moving Average, Moving Average, Complementary, Median Filter, CIC (Cascaded Integrator-Comb), Notch/Comb Filter, Savitzky-Golay Filter, Biquad/Second-Order-Section Cascade, IIR Filter Design (Butterworth/Chebyshev-I + Bilinear Transform), Madgwick/Mahony AHRS |
| [Optimization](doc/optimization/README.md) | Gradient Descent, SGD / Adam Step Optimisers |
| [Regularization](doc/regularization/README.md) | L1 (Lasso), L2 (Ridge) |
| [Math](doc/math/README.md) | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential, Angle Utilities, Tridiagonal (Thomas) Solver |
| [Motion Profiles](doc/motion_profiles/README.md) | Polynomial (Cubic/Quintic) Point-to-Point Profile, Trapezoidal (LSPB) Profile, S-Curve (Jerk-Limited) Profile |
| [Solvers](doc/solvers/README.md) | Gaussian Elimination, Levinson-Durbin, Durand-Kerner, Cholesky, DARE, Runge-Kutta ODE Integrators (RK4 + Dormand-Prince), Spectral Radius & Discrete Stability Margin, QR Decomposition (Householder / Givens), LU Decomposition with Partial Pivoting, Singular Value Decomposition (Golub-Kahan), Symplectic Integrators (Semi-Implicit Euler, Velocity Verlet) |
| [Nonlinear Control](doc/nonlinear_control/README.md) | Feedback Linearization, Backstepping Control, Model Reference Adaptive Control (MRAC) |
| [Robust Control](doc/robust_control/README.md) | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB), H∞ State-Feedback Control |
| [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations |
| 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 |

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

Expand Down
43 changes: 0 additions & 43 deletions ROADMAP.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,12 +26,8 @@ Difficulty legend:

| # | Component | Target module | Difficulty |
|----|-------------------------------------------------------|-------------------------|------------|
| 53 | SE(3) transform, twists, wrenches, adjoint, exp/log | `math` | ★★☆☆☆ |
| 61 | Cubic spline interpolation (vector-valued) | `math` | ★★☆☆☆ |
| 62 | Damped pseudo-inverse + null-space projector | `solvers` | ★★☆☆☆ |
| 66 | SE(2) transform, twists, exp/log, pose error | `math` | ★★☆☆☆ |
| 67 | Kabsch / Umeyama point-set alignment | `solvers` | ★★☆☆☆ |
| 63 | Incremental QR least squares (rank-revealing) | `estimators/offline` | ★★★☆☆ |
| 64 | Levenberg–Marquardt (retraction, pluggable damping) | `optimization` | ★★★☆☆ |
| 68 | Lie-group Jacobians and ⊕/⊖ for SO(3), SE(3), SE(2) | `math` | ★★★☆☆ |
| 69 | Control allocation (redistributed pseudo-inverse, QP) | `controllers` | ★★★☆☆ |
Expand All @@ -53,36 +49,6 @@ the library does not yet expose. Detailed below under

## Tier 2 — Easy ★★☆☆☆

### 53. SE(3) transform, twists and wrenches
- **What:** Rigid transform (rotation + translation) with closed-form inverse, adjoint, twist/wrench
transforms, exponential/logarithm (SO(3) and SE(3)) and a log-map pose error. 6-vectors are ordered
linear part first: twists `(v; ω)`, wrenches `(f; n)`.
- **Embedded value:** The algebra every pose-level consumer shares — robot kinematics, Jacobians and
task-space control in robotics-toolbox-cpp, drone attitude/position control, visual odometry. One
type and one ordering convention prevent silent block-order mismatches between modules.
- **Algorithm / paper:** K. Lynch, F. Park, *Modern Robotics* (2017), Ch. 3; R. Murray, Z. Li,
S. Sastry (1994), Ch. 2; J. Solà et al., "A micro Lie theory for state estimation in robotics" (2018).
- **Reuses:** `Geometry3D` (`RotationAboutAxis`, `SkewSymmetric`), `Quaternion`.
- **Spec:** [roadmap/math/SE3Transform](roadmap/math/SE3Transform/implementation.md).

### 61. Cubic spline interpolation
- **What:** C² vector-valued spline through `K` knots, clamped or natural ends, shared factorization across
channels.
- **Embedded value:** Smooth interpolation of waypoint lists and calibration tables in linear time on fixed
storage.
- **Algorithm / paper:** C. de Boor, *A Practical Guide to Splines* (2001), Ch. IV.
- **Reuses:** Item 59.
- **Spec:** [roadmap/math/CubicSpline](roadmap/math/CubicSpline/implementation.md).

### 62. Damped pseudo-inverse and null-space projector
- **What:** `A⁺_λ·b` and the exact projector `I − A⁺A` for any `Rows × Cols` matrix from one SVD.
- **Embedded value:** Singularity-robust inversion for redundant robots, over-actuated control allocation
and regularized calibration; the projector exposes spare actuation to a secondary objective.
- **Algorithm / paper:** C. Wampler, *IEEE Trans. SMC*, 16(1), 1986; A. Liégeois, *IEEE Trans. SMC*,
7(12), 1977.
- **Reuses:** `SingularValueDecomposition` (tall and wide).
- **Spec:** [roadmap/solvers/DampedPseudoInverse](roadmap/solvers/DampedPseudoInverse/implementation.md).

### 66. SE(2) transform
- **What:** Planar rigid transform `(R(θ), p)` with composition, closed-form inverse, adjoint, exp/log of
planar twists `(vx, vy, ω)` and a pose error, ordered linear part first like item 53.
Expand All @@ -104,15 +70,6 @@ the library does not yet expose. Detailed below under

## Tier 3 — Moderate ★★★☆☆

### 63. Incremental QR least squares
- **What:** Streaming Givens-QR accumulation of an unbounded number of rows into a `P×P` factor, with a
rank-revealing SVD solve (minimum-norm solution, identifiable directions, residual RMS).
- **Embedded value:** Parameter identification from long logs in fixed memory without squaring the
condition number.
- **Algorithm / paper:** G. Golub, C. Van Loan, §5.1.8 and §5.5; Å. Björck (1996), Ch. 2.
- **Reuses:** `GivensRotation`, `SingularValueDecomposition`.
- **Spec:** [roadmap/estimators/offline/IncrementalQrLeastSquares](roadmap/estimators/offline/IncrementalQrLeastSquares/implementation.md).

### 64. Levenberg–Marquardt
- **What:** Damped Gauss–Newton for small nonlinear least squares with a problem-supplied retraction
(vectors, bounds, rotations) and a pluggable damping policy.
Expand Down
Loading
Loading