Merge branch 'main' into feature/add-biquad-filter

Author: gabrielfrasantos

README.md

@@ -18,7 +18,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 |--------------------------------------------------------------------|----------------------------------------------------------------------|
 | [Analysis](doc/analysis/README.md)                                 | FFT, Real-Input FFT (RFFT), Power Spectral Density, DCT, Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm |
 | [Control Analysis](doc/control_analysis/README.md)                 | Frequency Response, Root Locus, Controllability/Observability Matrices & Gramians |
-| [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, PID, LQR, MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Lead-Lag Compensator, Luenberger Observer |
+| [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, 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 |
 | [Dynamics](doc/dynamics/README.md)                                 | Euler-Lagrange, Newton-Euler, Recursive Newton-Euler, ABA            |
 | [Estimators](doc/estimators/README.md)                             | Linear Regression, Polynomial Fitting, Yule-Walker (offline), Recursive Least Squares, LMS / NLMS Adaptive Filter (online) |
 | [Filters](doc/filters/README.md)                                   | Kalman, Extended Kalman, Unscented 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 |

ROADMAP.md

@@ -27,7 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 20 | Integral / servo state feedback (LQI)                | `controllers`             | ★★★☆☆      |
 | 27 | QR decomposition (Householder / Givens)              | `solvers`                 | ★★★★☆      |
 | 28 | LU decomposition with partial pivoting               | `solvers`                 | ★★★★☆      |
 | 29 | Matrix exponential (scaling & squaring + Padé)       | `math`                    | ★★★★☆      |

doc/controllers/IntegralStateFeedbackLqi.md

@@ -0,0 +1,116 @@
+# Integral State Feedback (LQI / Servo)
+
+## Overview & Motivation
+
+Plain LQR drives the system state toward the origin but leaves a persistent steady-state offset when a constant reference or disturbance is present. The integral state feedback controller
+(LQI, or servo LQR) removes this offset by augmenting the plant with integrators on the
+tracking error. A single LQR solve on the augmented system produces two gains: one that
+acts on the physical states and one that closes the integral loop, guaranteeing zero
+steady-state error to constant references with no manual trim.
+
+## Mathematical Theory
+
+### Plant Model
+
+The discrete-time plant is
+
+$$x_{k+1} = A x_k + B u_k, \quad y_k = C x_k$$
+
+with state $x \in \mathbb{R}^n$, input $u \in \mathbb{R}^m$, and tracked output $y \in \mathbb{R}^p$.
+
+### Augmented Plant
+
+Define the integral-of-error state $x_i \in \mathbb{R}^p$:
+
+$$x_{i,k+1} = x_{i,k} + (r_k - y_k) T_s$$
+
+Stacking $\xi = [x^\top \; x_i^\top]^\top$ gives the augmented system
+
+$$\xi_{k+1} = A_a \xi_k + B_a u_k + E_a r_k$$
+
+$$A_a = \begin{bmatrix} A & 0 \\ -C T_s & I \end{bmatrix}, \quad B_a = \begin{bmatrix} B \\ 0 \end{bmatrix}, \quad E_a = \begin{bmatrix} 0 \\ T_s I \end{bmatrix}$$
+
+### LQR Design on the Augmented Plant
+
+Minimise the infinite-horizon quadratic cost
+
+$$J = \sum_{k=0}^{\infty} \bigl(\xi_k^\top Q \xi_k + u_k^\top R u_k\bigr)$$
+
+by solving the Discrete Algebraic Riccati Equation (DARE) for $P$:
+
+$$P = A_a^\top P A_a - A_a^\top P B_a (R + B_a^\top P B_a)^{-1} B_a^\top P A_a + Q$$
+
+The optimal gain partitions as $K_a = [K_x \mid K_i]$ where $K_x \in \mathbb{R}^{m \times n}$ acts
+on the physical states and $K_i \in \mathbb{R}^{m \times p}$ acts on the integral states.
+
+### Control Law
+
+$$u_k = -K_x x_k - K_i x_{i,k}$$
+
+At equilibrium $r - y = 0$, so $x_{i}$ stops changing, and the control law holds $y = r$ exactly.
+
+## Complexity Analysis
+
+| Case   | Time         | Space        | Notes                              |
+|--------|--------------|--------------|------------------------------------|
+| Design | $O((n+p)^3)$ | $O((n+p)^2)$ | DARE iteration on augmented system |
+| Update | $O(m(n+p))$  | $O(p)$       | Two matrix-vector products         |
+
+Design is a one-time offline computation. The per-sample update cost is dominated by the two
+gain-state products and the integral accumulation.
+
+## Step-by-Step Walkthrough
+
+Consider a scalar plant ($n=1$, $m=1$, $p=1$, $T_s = 0.01$):
+
+1. Form $A_a$ (2×2), $B_a$ (2×1) from plant matrices and $T_s$.
+2. Choose $Q$ (2×2 diagonal) and $R$ (scalar) to weight states and input.
+3. Solve DARE → $P$ (2×2) → $K_a = [k_x \; k_i]$ (1×2).
+4. Per step: accumulate $x_i \mathrel{+}= (r - y) T_s$, output $u = -k_x x - k_i x_i$.
+5. After ~200 steps the output converges to $r$ within numerical tolerance.
+
+## Pitfalls & Edge Cases
+
+- **Integral windup**: when the actuator saturates, the integral keeps growing because the
+  control cannot reach the demanded value. Apply a clamp on $x_i$ or back-calculate
+  (anti-windup) to prevent divergence after the saturation clears.
+- **Marginally stable plant**: a plant with an open-loop integrator combined with the error
+  integrator yields a double-integrator augmented system. The DARE still converges if the
+  augmented pair $(A_a, B_a)$ is stabilisable; verify that condition before deploying.
+- **Slow integral weighting**: under-weighting the integral state in $Q$ allows steady-state
+  error to persist for many steps before correcting; over-weighting causes overshoot.
+- **Sample-time mismatch**: the discrete integral $x_i$ accumulates $T_s$-scaled errors.
+  Using the wrong $T_s$ at run time shifts the effective integral gain and breaks zero-error
+  convergence.
+
+## Variants & Generalizations
+
+- **Continuous-time LQI**: replace the discrete integrator with $\dot{x}_i = r - y$ and solve
+  the continuous ARE.
+- **Output-feedback LQI (LQGI)**: combine with a Kalman filter when only the output (not the
+  full state) is measurable — the separation principle still holds.
+- **Anti-windup**: add a saturation block on $x_i$ with back-calculation to recover from
+  actuator limits without integral drift.
+- **Multiple outputs**: the design extends directly to $p > 1$ by stacking $p$ integral states.
+
+## Applications
+
+- **Motor position/speed servo**: eliminate gravity or friction offsets without manual trim.
+- **Process control**: temperature or pressure regulation with constant load disturbances.
+- **Aerospace attitude control**: integral action compensates for persistent aerodynamic moments.
+- **Robotics**: joint torque control with payload-induced constant forces.
+
+## Connections to Other Algorithms
+
+- **Lqr**: the base regulator; LQI delegates the DARE solve to `Lqr` on the augmented plant.
+- **DiscreteAlgebraicRiccatiEquation**: the inner solver used by `Lqr`.
+- **Lqg**: pairs `Lqr` with a Kalman filter; LQI could similarly pair with `Lqg` for
+  output-feedback servo control.
+- **Pid**: the integral channel of a PID is the scalar, single-output analogue of $K_i x_i$.
+- **SaturationRateLimiter**: provides output clamping for anti-windup on the LQI integral.
+
+## References & Further Reading
+
+- B. D. O. Anderson, J. B. Moore, *Optimal Control: Linear Quadratic Methods*, Prentice Hall, 1990.
+- G. F. Franklin, J. D. Powell, A. Emami-Naeini, *Feedback Control of Dynamic Systems*, 8th ed., Pearson, 2019. Chapter 9.
+- K. J. Åström, B. Wittenmark, *Computer-Controlled Systems: Theory and Design*, 3rd ed., Prentice Hall, 1997. Chapter 5.

doc/controllers/README.md

@@ -9,6 +9,7 @@ Feedback control algorithms for regulating dynamic systems in real time.
 | [Bang-Bang / Hysteresis Controller](BangBangHysteresis.md) | Two-state relay controller with Schmitt-trigger dead-band to prevent chatter |
 | [PID Controller](Pid.md) | Proportional-Integral-Derivative controller using a discrete recursive formulation      |
 | [LQR Controller](Lqr.md) | Linear Quadratic Regulator — optimal state-feedback control minimizing a quadratic cost |
+| [LQI / Servo Controller](IntegralStateFeedbackLqi.md) | Integral state feedback (LQI) — LQR augmented with integrators on the tracking error for zero steady-state offset |
 | [LQG Controller](Lqg.md) | Linear Quadratic Gaussian — output-feedback optimal control via LQR + Kalman Filter     |
 | [MPC Controller](Mpc.md) | Model Predictive Controller — receding-horizon optimal control with constraint handling |
 | [Linear Time-Invariant Model](LinearTimeInvariant.md) | Discrete-time state-space plant model (A, B, C, D) shared across controllers and filters |

numerical/controllers/implementations/CMakeLists.txt

@@ -15,6 +15,7 @@ target_sources(numerical.controllers.implementations PRIVATE
     BangBangHysteresis.hpp
     Feedforward2Dof.hpp
     GainScheduledController.hpp
+    IntegralStateFeedbackLqi.hpp
     LeadLagCompensator.hpp
     Lqg.hpp
     Lqr.hpp
@@ -28,6 +29,7 @@ numerical_add_coverage_sources(numerical.controllers.implementations
     BangBangHysteresis.cpp
     Feedforward2Dof.cpp
     GainScheduledController.cpp
+    IntegralStateFeedbackLqi.cpp
     LeadLagCompensator.cpp
     Lqg.cpp
     Lqr.cpp

numerical/controllers/implementations/IntegralStateFeedbackLqi.cpp

@@ -0,0 +1,6 @@
+#include "numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp"
+
+namespace controllers
+{
+    template class IntegralStateFeedbackLqi<float, 2, 1, 1>;
+}

numerical/controllers/implementations/IntegralStateFeedbackLqi.hpp

@@ -0,0 +1,124 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/controllers/implementations/Lqr.hpp"
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/LinearTimeInvariant.hpp"
+#include "numerical/math/Matrix.hpp"
+
+namespace controllers
+{
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    class IntegralStateFeedbackLqi
+    {
+        static_assert(std::is_floating_point_v<T>, "IntegralStateFeedbackLqi supports floating-point types");
+
+        static constexpr std::size_t AugmentedSize = StateSize + OutputSize;
+
+    public:
+        using StateVector = math::Vector<T, StateSize>;
+        using InputVector = math::Vector<T, InputSize>;
+        using OutputVector = math::Vector<T, OutputSize>;
+        using GainStateMatrix = math::Matrix<T, InputSize, StateSize>;
+        using GainIntegralMatrix = math::Matrix<T, InputSize, OutputSize>;
+        using IntegralVector = math::Vector<T, OutputSize>;
+
+        IntegralStateFeedbackLqi(const GainStateMatrix& kx, const GainIntegralMatrix& ki, T ts);
+
+        IntegralStateFeedbackLqi(
+            const math::LinearTimeInvariant<T, StateSize, InputSize, OutputSize>& plant,
+            const math::SquareMatrix<T, AugmentedSize>& Q,
+            const math::SquareMatrix<T, InputSize>& R,
+            T ts);
+
+        OPTIMIZE_FOR_SPEED InputVector ComputeControl(
+            const StateVector& x,
+            const OutputVector& reference,
+            const OutputVector& measured);
+
+        void Reset();
+
+        [[nodiscard]] const GainStateMatrix& GetGainState() const;
+        [[nodiscard]] const GainIntegralMatrix& GetGainIntegral() const;
+
+    private:
+        GainStateMatrix gainState{};
+        GainIntegralMatrix gainIntegral{};
+        IntegralVector integral{};
+        T sampleTime{};
+    };
+
+    // Implementation //
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::IntegralStateFeedbackLqi(
+        const GainStateMatrix& kx, const GainIntegralMatrix& ki, T ts)
+        : gainState{ kx }
+        , gainIntegral{ ki }
+        , sampleTime{ ts }
+    {}
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::IntegralStateFeedbackLqi(
+        const math::LinearTimeInvariant<T, StateSize, InputSize, OutputSize>& plant,
+        const math::SquareMatrix<T, AugmentedSize>& Q,
+        const math::SquareMatrix<T, InputSize>& R,
+        T ts)
+        : sampleTime{ ts }
+    {
+        math::SquareMatrix<T, AugmentedSize> Aa{};
+        math::Matrix<T, AugmentedSize, InputSize> Ba{};
+
+        Aa.SetBlock(plant.A, 0, 0);
+        Aa.SetBlock(plant.C * T(-ts), StateSize, 0);
+        for (std::size_t r = 0; r < OutputSize; ++r)
+            Aa.at(StateSize + r, StateSize + r) = T(1);
+        Ba.SetBlock(plant.B, 0, 0);
+
+        Lqr<T, AugmentedSize, InputSize> lqr{ Aa, Ba, Q, R };
+        const auto& Ka = lqr.GetGain();
+
+        gainState = Ka.template GetBlock<InputSize, StateSize>(0, 0);
+        gainIntegral = Ka.template GetBlock<InputSize, OutputSize>(0, StateSize);
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    OPTIMIZE_FOR_SPEED
+        typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::InputVector
+        IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::ComputeControl(
+            const StateVector& x,
+            const OutputVector& reference,
+            const OutputVector& measured)
+    {
+        auto error = reference - measured;
+        integral = integral + error * sampleTime;
+        return (gainState * x + gainIntegral * integral) * T(-1);
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    void IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::Reset()
+    {
+        integral = IntegralVector{};
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GainStateMatrix&
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GetGainState() const
+    {
+        return gainState;
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    const typename IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GainIntegralMatrix&
+    IntegralStateFeedbackLqi<T, StateSize, InputSize, OutputSize>::GetGainIntegral() const
+    {
+        return gainIntegral;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class IntegralStateFeedbackLqi<float, 2, 1, 1>;
+#endif
+}

numerical/controllers/implementations/test/CMakeLists.txt

@@ -12,6 +12,7 @@ target_sources(numerical.controllers_test PRIVATE
     TestBangBangHysteresis.cpp
     TestFeedforward2Dof.cpp
     TestGainScheduledController.cpp
+    TestIntegralStateFeedbackLqi.cpp
     TestLeadLagCompensator.cpp
     TestLqg.cpp
     TestLqr.cpp