feat: add feedback linearization

README.md

@@ -26,6 +26,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 | [Regularization](doc/regularization/README.md)                     | L1 (Lasso), L2 (Ridge)                                              |
 | [Math](doc/math/README.md)                                         | CORDIC, Quaternion, MatrixNorms, Step Response Metrics, MatrixExponential                             |
 | [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 |
+| [Nonlinear Control](doc/nonlinear_control/README.md)               | Feedback Linearization                                                                                    |
 | [Robust Control](doc/robust_control/README.md)                     | Active Disturbance Rejection Control (ADRC + ESO), Sliding Mode Control (SMC), Disturbance Observer (DOB) |
 | [Performance Optimization](doc/performance-optimization/README.md) | Compiler optimizations, SIMD                                         |
 

ROADMAP.md

@@ -227,12 +227,6 @@ the library does not yet expose. Detailed below under
 - **Algorithm / paper:** P. Kaminski, A. Bryson, S. Schmidt, "Discrete Square Root Filtering: A Survey of Current Techniques," *IEEE Trans. AC*, 16(6), 1971.
 - **Reuses:** [KalmanFilterBase.hpp](numerical/filters/active/KalmanFilterBase.hpp), [Cholesky](numerical/solvers/CholeskyDecomposition.hpp), item 27.
 
-### 40. Feedback linearization  *(float-first)*
-- **What:** Cancel a control-affine system's known nonlinear dynamics via a coordinate transform + inner control law, leaving an equivalent linear system that an outer loop (PD/LQR) can drive.
-- **Embedded value:** One linear gain set works across the whole operating envelope of any structurally-known nonlinear plant (robot arms, quadrotors, electromechanical drives) — no gain scheduling, no lookup tables.
-- **Algorithm / paper:** A. Isidori, *Nonlinear Control Systems* (1995); Slotine & Li, *Applied Nonlinear Control*.
-- **Reuses:** an injected control-affine plant model, `math::Matrix`, new `nonlinear_control/` module. (The manipulator computed-torque instance lives in robotics-toolbox-cpp.)
-
 ### 41. Backstepping controller  *(float-first)*
 - **What:** Recursive Lyapunov-based design for strict-feedback systems, stabilizing one integrator stage at a time.
 - **Embedded value:** Systematic, provably-stable control for cascaded nonlinear plants (electromechanical, flight).

doc/nonlinear_control/FeedbackLinearization.md

@@ -0,0 +1,106 @@
+# Feedback Linearization
+
+## Overview & Motivation
+
+Nonlinear plants such as robot arms, quadrotors, and electromechanical drives are only well-controlled by a fixed linear gain over a narrow operating range. Feedback linearization resolves this by exploiting a known model of the plant's nonlinearity to cancel it exactly in the closed loop, leaving an equivalent linear system — decoupled integrator chains — that a single outer-loop gain set can drive correctly across the full operating envelope. No gain scheduling, no lookup tables, no re-tuning when the operating point changes.
+
+## Mathematical Theory
+
+### Control-Affine Plant
+
+The technique applies to plants whose output $y \in \mathbb{R}^m$ satisfies, after $r$ differentiations,
+
+$$y^{(r)} = a(x) + B(x)\, u$$
+
+where $x \in \mathbb{R}^n$ is the state, $u \in \mathbb{R}^m$ is the input, $a(x) \in \mathbb{R}^m$ is the **drift term** (known nonlinear dynamics), and $B(x) \in \mathbb{R}^{m \times m}$ is the **decoupling matrix** (state-dependent input gain). The integer $r$ is the relative degree. For mechanical systems ($r = 2$), $B(x) = M(q)$ is the inertia matrix and $a(x) = C(q, \dot{q})\dot{q} + g(q)$ is the Coriolis-plus-gravity term.
+
+### Inner Control Law (Cancellation)
+
+The inner law selects $u$ so that the term $a(x)$ is cancelled and the decoupling matrix is factored out:
+
+$$u = B(x)\, v + a(x)$$
+
+Substituting into the plant equation yields
+
+$$y^{(r)} = a(x) + B(x)\bigl(B(x)\,v + a(x)\bigr) - a(x) = v$$
+
+leaving pure integrator chains $y^{(r)} = v$, provided $B(x)$ is nonsingular.
+
+### Outer Control Law (Linear Outer Loop)
+
+With the plant reduced to integrators, a PD outer loop commands the virtual input:
+
+$$v = y_d^{(r)} + K_d\,\dot{e} + K_p\, e, \quad e = y_d - y, \quad \dot{e} = \dot{y}_d - \dot{y}$$
+
+The closed-loop error satisfies the linear ODE
+
+$$e^{(r)} + K_d\,\dot{e} + K_p\, e = 0$$
+
+whose eigenvalues are set by choosing $K_p, K_d$. Critical damping per channel requires $K_d = 2\sqrt{K_p}$.
+
+### Combined Law
+
+Expanding yields the single expression evaluated on the hot path:
+
+$$u = B(x)\bigl(y_d^{(r)} + K_d\,\dot{e} + K_p\, e\bigr) + a(x)$$
+
+No matrix inversion appears on the hot path: the law multiplies by $B(x)$, not by $B(x)^{-1}$.
+
+## Complexity Analysis
+
+| Operation    | Time               | Space        | Notes                                  |
+|--------------|--------------------|--------------|----------------------------------------|
+| Construction | $O(m^2)$           | $O(m^2)$     | Copy two gain matrices                 |
+| ComputeInput | $O(m^2)$           | $O(m)$ extra | Two matrix-vector products dominate    |
+| Model query  | $O(m^2)$–$O(nm^2)$ | $O(m^2)$     | Implementation-defined; injected model |
+
+All storage is in fixed-size stack arrays; the law itself performs no heap allocation.
+
+## Step-by-Step Walkthrough
+
+Consider a 2-DOF planar arm with $m = 2$, $K_p = 100 I$, $K_d = 20 I$, and at one instant:
+
+- State $x = [0.1, 0.2]^\top$, $\dot{x} = [0, 0]^\top$.
+- Reference $y_d = [0.5, 0.5]^\top$, $\dot{y}_d = [0, 0]^\top$, $\ddot{y}_d = [0, 0]^\top$.
+- Model returns $B(x) = I$ and $a(x) = [0.3, 0.1]^\top$.
+
+1. Compute error: $e = [0.4, 0.3]^\top$, $\dot{e} = [0, 0]^\top$.
+2. Compute virtual input: $v = 0 + 20 \cdot 0 + 100 \cdot [0.4, 0.3]^\top = [40, 30]^\top$.
+3. Inner law: $u = I \cdot [40, 30]^\top + [0.3, 0.1]^\top = [40.3, 30.1]^\top$.
+
+The gravity-like drift $a(x)$ is added directly; the outer PD term drives position error to zero.
+
+## Pitfalls & Edge Cases
+
+- **Singular decoupling matrix**: if $B(x)$ is rank-deficient the inner law is undefined. The condition $\det B(x) \neq 0$ must hold throughout the operating region.
+- **Model mismatch**: cancellation is only as exact as the model. Unmodelled dynamics or parameter error leaves a residual nonlinearity; pair with a robust or adaptive outer term to bound the error.
+- **Zero dynamics**: exact linearisation of the output may leave internal states unobservable. These zero dynamics can be unstable even when the output tracks perfectly. Verify stability of the internal dynamics before deployment.
+- **Actuator limits**: the inner law can command arbitrarily large $u$ near the start of a transient. Saturation on $u$ breaks the exact cancellation argument; scale $K_p$, $K_d$ or add a reference pre-filter to keep the command within actuator bounds.
+- **Float precision**: for large $m$, matrix products accumulate rounding error proportional to $m \cdot \epsilon_\text{float}$. Verify the gain matrices are well-conditioned.
+
+## Variants & Generalizations
+
+- **Input-output linearization (SISO)**: for scalar output with relative degree $r > 1$, the cancellation uses Lie derivatives $L_f^r h(x)$ and $L_g L_f^{r-1} h(x)$, and the input is $u = (v - L_f^r h(x)) / L_g L_f^{r-1} h(x)$. The singularity condition $L_g L_f^{r-1} h \neq 0$ replaces $\det B \neq 0$.
+- **Computed-torque control**: the mechanical specialisation with $B = M(q)$ and $a = C(q,\dot{q})\dot{q} + g(q)$. The canonical instantiation lives in robotics-toolbox-cpp.
+- **Partial feedback linearization**: linearizes only the input-output channels, leaving the rest of the state dynamics (zero dynamics) uncontrolled by the outer loop.
+- **Adaptive feedback linearization / MRAC**: replaces the fixed model with an online-adapted estimate, enabling cancellation under parametric uncertainty.
+
+## Applications
+
+- Robot manipulators: decoupled Cartesian impedance or position control across the full joint-space workspace.
+- Quadrotor UAVs: attitude and altitude decoupling for independent channel control.
+- Electromechanical drives: cancellation of back-EMF and friction in torque-controlled axes.
+- Chemical process control: inversion of Hammerstein-type nonlinear input maps.
+
+## Connections to Other Algorithms
+
+- **Backstepping**: recursive alternative for strict-feedback systems; tolerates drift terms that cannot be directly cancelled.
+- **Model Reference Adaptive Control (MRAC)**: adapts the model online; complements feedback linearization when the plant parameters are unknown.
+- **LQR**: natural choice for the outer linear loop once the plant has been linearized.
+- **Sliding Mode Control**: robustifies the outer loop against residual model mismatch by adding a discontinuous reaching term.
+
+## References & Further Reading
+
+- A. Isidori, *Nonlinear Control Systems*, 3rd ed., Springer, 1995.
+- J.-J. Slotine, W. Li, *Applied Nonlinear Control*, Prentice-Hall, 1991, Chapter 6.
+- H. K. Khalil, *Nonlinear Systems*, 3rd ed., Prentice-Hall, 2002, Chapter 13.

doc/nonlinear_control/README.md

@@ -0,0 +1,9 @@
+# Nonlinear Control
+
+Algorithms for nonlinear control design: controllers that exploit a known plant model to cancel or structurally transform nonlinear dynamics.
+
+## Algorithms
+
+| Algorithm                                          | Description                                                                                                                                                    |
+|----------------------------------------------------|----------------------------------------------------------------------------------------------------------------------------------------------------------------|
+| [Feedback Linearization](FeedbackLinearization.md) | Cancels a control-affine plant's known nonlinear dynamics via an inner control law, leaving decoupled integrator chains that a simple outer PD/LQR loop drives |

numerical/CMakeLists.txt

@@ -7,5 +7,6 @@ add_subdirectory(math)
 add_subdirectory(neural_network)
 add_subdirectory(optimization)
 add_subdirectory(regularization)
+add_subdirectory(nonlinear_control)
 add_subdirectory(robust_control)
 add_subdirectory(solvers)

numerical/nonlinear_control/CMakeLists.txt

@@ -0,0 +1,21 @@
+numerical_add_header_library(numerical.nonlinear_control STATIC)
+
+target_include_directories(numerical.nonlinear_control ${NUMERICAL_VISIBILITY}
+    "$<BUILD_INTERFACE:${CMAKE_CURRENT_LIST_DIR}/../../>"
+    "$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>"
+)
+
+target_link_libraries(numerical.nonlinear_control ${NUMERICAL_VISIBILITY}
+    numerical.math
+    infra.util
+)
+
+target_sources(numerical.nonlinear_control PRIVATE
+    FeedbackLinearization.hpp
+)
+
+numerical_add_coverage_sources(numerical.nonlinear_control
+    FeedbackLinearization.cpp
+)
+
+add_subdirectory(test)

numerical/nonlinear_control/FeedbackLinearization.cpp

@@ -0,0 +1,9 @@
+// Copyright (c) 2024 Numerical Toolbox Contributors
+// SPDX-License-Identifier: MIT
+
+#include "numerical/nonlinear_control/FeedbackLinearization.hpp"
+
+namespace nonlinear_control
+{
+    template class FeedbackLinearization<float, 2>;
+}

numerical/nonlinear_control/FeedbackLinearization.hpp

@@ -0,0 +1,75 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/Matrix.hpp"
+#include <cstddef>
+#include <type_traits>
+
+namespace nonlinear_control
+{
+    template<typename T, std::size_t Dim>
+    class ControlAffineModel
+    {
+        static_assert(std::is_floating_point_v<T>, "ControlAffineModel supports floating-point types");
+        static_assert(Dim > 0, "ControlAffineModel requires Dim > 0");
+
+    public:
+        using StateVector = math::Vector<T, Dim>;
+        using DecouplingMatrix = math::SquareMatrix<T, Dim>;
+
+        virtual ~ControlAffineModel() = default;
+
+        [[nodiscard]] virtual DecouplingMatrix DecouplingMatrixAt(const StateVector& x) const = 0;
+        [[nodiscard]] virtual StateVector DriftTerm(const StateVector& x) const = 0;
+    };
+
+    template<typename T, std::size_t Dim>
+    class FeedbackLinearization
+    {
+        static_assert(std::is_floating_point_v<T>, "FeedbackLinearization supports floating-point types");
+        static_assert(Dim > 0, "FeedbackLinearization requires Dim > 0");
+
+    public:
+        using StateVector = math::Vector<T, Dim>;
+        using GainMatrix = math::SquareMatrix<T, Dim>;
+
+        FeedbackLinearization(const ControlAffineModel<T, Dim>& model, const GainMatrix& kp, const GainMatrix& kd);
+
+        OPTIMIZE_FOR_SPEED StateVector ComputeInput(const StateVector& x, const StateVector& xDot,
+            const StateVector& yd, const StateVector& ydDot, const StateVector& ydDdot);
+
+    private:
+        const ControlAffineModel<T, Dim>& model;
+        GainMatrix kp;
+        GainMatrix kd;
+    };
+
+    template<typename T, std::size_t Dim>
+    FeedbackLinearization<T, Dim>::FeedbackLinearization(
+        const ControlAffineModel<T, Dim>& model, const GainMatrix& kp, const GainMatrix& kd)
+        : model{ model }
+        , kp{ kp }
+        , kd{ kd }
+    {}
+
+    template<typename T, std::size_t Dim>
+    OPTIMIZE_FOR_SPEED typename FeedbackLinearization<T, Dim>::StateVector
+    FeedbackLinearization<T, Dim>::ComputeInput(const StateVector& x, const StateVector& xDot,
+        const StateVector& yd, const StateVector& ydDot, const StateVector& ydDdot)
+    {
+        const StateVector e{ yd - x };
+        const StateVector eDot{ ydDot - xDot };
+        const StateVector v{ ydDdot + kd * eDot + kp * e };
+        const auto B{ model.DecouplingMatrixAt(x) };
+        const StateVector a{ model.DriftTerm(x) };
+        return B * v + a;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class FeedbackLinearization<float, 2>;
+#endif
+}

numerical/nonlinear_control/test/CMakeLists.txt

@@ -0,0 +1,12 @@
+add_executable(numerical.nonlinear_control_test)
+emil_build_for(numerical.nonlinear_control_test BOOL NUMERICAL_TOOLBOX_BUILD_TESTS)
+emil_add_test(numerical.nonlinear_control_test)
+
+target_link_libraries(numerical.nonlinear_control_test PUBLIC
+    gmock_main
+    numerical.nonlinear_control
+)
+
+target_sources(numerical.nonlinear_control_test PRIVATE
+    TestFeedbackLinearization.cpp
+)