Merge branch 'main' into feature/add-luenberger-observer

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, Power Spectral Density, DCT, Window Functions, Signal Detectors, Convolution & Correlation, Goertzel Algorithm |
 | [Control Analysis](doc/control_analysis/README.md)                 | Frequency Response, Root Locus                                        |
-| [Controllers](doc/controllers/README.md)                           | Bang-Bang/Hysteresis, PID, LQR, MPC, Saturation, Rate Limiter, Slew-Limited Saturation, Feedforward/2-DOF, Gain-Scheduled Controller, Luenberger Observer |
+| [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 |
 | [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 (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 |

ROADMAP.md

@@ -28,7 +28,6 @@ Difficulty legend:
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
 | 15 | Biquad / Second-Order-Section cascade                | `filters/passive`         | ★★★☆☆      |
-| 17 | Lead-lag compensator                                 | `controllers`             | ★★★☆☆      |
 | 20 | Integral / servo state feedback (LQI)                | `controllers`             | ★★★☆☆      |
 | 21 | LMS / NLMS adaptive filter                           | `estimators/online`       | ★★★☆☆      |
 | 22 | Savitzky-Golay filter                                | `filters/passive`         | ★★★☆☆      |

doc/controllers/LeadLagCompensator.md

@@ -0,0 +1,102 @@
+# Lead-Lag Compensator
+
+## Overview & Motivation
+
+Classical feedback loops require a mechanism to reshape the open-loop frequency response without the full overhead of a state-space design. A one-pole/one-zero compensator achieves this with three tuning parameters and two state words, making it practical for any microcontroller control loop. When phase margin is insufficient, a lead configuration injects extra phase near the gain crossover frequency, raising stability margin and permitting a higher bandwidth. When steady-state error is the concern, a lag configuration boosts low-frequency gain to drive the error toward zero while leaving the crossover region essentially unchanged.
+
+## Mathematical Theory
+
+### Continuous-Time Transfer Function
+
+The compensator is defined in the Laplace domain as:
+
+$$C(s) = K \cdot \frac{s + z}{s + p}$$
+
+where $K$ is the overall gain, $z$ is the zero frequency (rad/s), and $p$ is the pole frequency (rad/s).
+
+The DC gain is $C(0) = K \cdot z / p$.
+
+- **Lead network** ($z < p$): the zero sits below the pole, so phase rises at mid frequencies and then falls again, providing a phase bump near crossover.
+- **Lag network** ($z > p$): the pole sits below the zero, so the compensator acts as a high-gain integrator approximation at low frequencies and rolls back to unity at high frequencies.
+
+### Bilinear (Tustin) Discretization
+
+The bilinear transform substitutes $s \leftarrow \frac{2}{T_s} \cdot \frac{1 - z^{-1}}{1 + z^{-1}}$, mapping the entire left half of the $s$-plane to the interior of the unit circle in the $z$-plane and preserving stability.
+
+Define $c = 2/T_s$. The numerator and denominator polynomials in $z$ are:
+
+$$n_0 = K(c + z), \quad n_1 = K(z - c)$$
+$$d_0 = c + p, \quad d_1 = p - c$$
+
+Normalizing by $d_0$:
+
+$$b_0 = \frac{n_0}{d_0}, \quad b_1 = \frac{n_1}{d_0}, \quad a_1 = \frac{d_1}{d_0}$$
+
+### Discrete-Time Recurrence (Direct Form I)
+
+$$y[n] = b_0 \, x[n] + b_1 \, x[n-1] - a_1 \, y[n-1]$$
+
+The sign convention places the feedback term with a minus sign on $a_1$, so positive $a_1$ in the formula corresponds to a pole at $+a_1$ inside the unit disk.
+
+## Complexity Analysis
+
+| Case    | Time   | Space  | Notes                                          |
+|---------|--------|--------|------------------------------------------------|
+| Best    | $O(1)$ | $O(1)$ | Three multiply-adds, two state updates         |
+| Average | $O(1)$ | $O(1)$ | Fixed instruction count per sample             |
+| Worst   | $O(1)$ | $O(1)$ | No branching; deterministic real-time behavior |
+
+Design (Tustin coefficient computation) is $O(1)$ and occurs once in the constructor.
+
+## Step-by-Step Walkthrough
+
+Parameters: $K=1$, $z=1\,\text{rad/s}$, $p=10\,\text{rad/s}$, $T_s = 0.01\,\text{s}$ (lead network).
+
+1. $c = 2/0.01 = 200$
+2. $n_0 = 1 \cdot (200 + 1) = 201$, $\quad n_1 = 1 \cdot (1 - 200) = -199$
+3. $d_0 = 200 + 10 = 210$, $\quad d_1 = 10 - 200 = -190$
+4. $b_0 = 201/210 \approx 0.9571$, $\quad b_1 = -199/210 \approx -0.9476$, $\quad a_1 = -190/210 \approx -0.9048$
+
+Unit-step response (first two samples):
+
+| $n$ | $x[n]$ | $b_0 x[n]$ | $b_1 x[n-1]$ | $-a_1 y[n-1]$ | $y[n]$ |
+|-----|--------|------------|--------------|---------------|--------|
+| 0   | 1      | 0.9571     | 0            | 0             | 0.9571 |
+| 1   | 1      | 0.9571     | −0.9476      | 0.8664        | 0.8759 |
+
+The first output (≈ 0.957) already exceeds the DC steady-state gain of 0.1, illustrating the phase-lead kick.
+
+Verification of DC gain: $b_0 + b_1 = 2/210$; $1 + a_1 = 20/210$; ratio $= 2/20 = 0.1 = K \cdot z/p$.
+
+## Pitfalls & Edge Cases
+
+- **Near-Nyquist poles/zeros**: when $z$ or $p$ is comparable to $\pi/T_s$, the bilinear transform introduces frequency warping. Pre-warp the analog corner frequencies to $\hat\omega = (2/T_s)\tan(\omega T_s/2)$ before applying Tustin if exact placement matters.
+- **Degenerate case $z = p$**: the compensator collapses to a pure gain $K$ with no dynamics. The discrete recurrence remains valid; the pole and zero cancel.
+- **Unstable discretization**: a plant with a very fast analog pole relative to $T_s$ can map outside the unit disk; verify $|a_1| < 1$ after computing coefficients.
+- **Floating-point accumulation**: the two state variables accumulate rounding error indefinitely. For long-running loops, periodic resets or double-precision state registers mitigate drift.
+
+## Variants & Generalizations
+
+- **Lead-lag cascade**: a lead section followed by a lag section in series provides simultaneous bandwidth improvement and steady-state accuracy. Reuse two first-order sections rather than chaining first-order blocks through a single instance.
+- **Phase-lead only / phase-lag only**: selecting $z$ and $p$ exclusively achieves single-objective shaping; the structure is unchanged.
+- **Pre-warped Tustin**: replace $z, p$ with $\hat z = (2/T_s)\tan(z T_s/2)$ and $\hat p = (2/T_s)\tan(p T_s/2)$ before computing Tustin coefficients to achieve exact frequency matching.
+- **Second-order extension**: cascading two first-order sections or using a biquad second-order section enables lead-lag-lead or other compound shapes.
+
+## Applications
+
+- **Motor velocity loops**: a lead compensator raises phase margin to allow a higher proportional gain, which directly increases bandwidth and disturbance rejection.
+- **Voltage regulators**: a lag compensator adds integrating action at mains frequency to eliminate steady-state regulation error without destabilizing the switching loop.
+- **Flight control inner loops**: classical lead-lag design from Bode plots remains the dominant method for aircraft inner-loop stabilization due to its transparency and robustness to model uncertainty.
+- **Thermal control**: a lag network boosts gain at low frequencies to null steady-state temperature offset while keeping the loop stable against slow sensor dynamics.
+
+## Connections to Other Algorithms
+
+- **BiquadCascade**: a second-order section generalizes to two poles and two zeros; cascading biquads is the standard approach when more than one lead-lag stage is required.
+- **PidIncremental**: a PID controller contains an implicit lead-lag structure; tuning the derivative and integral terms is equivalent to placing the compensator zero and pole.
+- **FrequencyResponse**: use the Bode magnitude and phase plots to verify that the shaped open-loop response achieves the desired gain crossover frequency and phase margin after adding the compensator.
+
+## References & Further Reading
+
+- G. F. Franklin, J. D. Powell, A. Emami-Naeini, *Feedback Control of Dynamic Systems*, 8th ed. (2019) — Chapter 6: The Frequency-Response Design Method.
+- K. J. Åström, R. M. Murray, *Feedback Systems: An Introduction for Scientists and Engineers* (2008), Chapter 9: Frequency Domain Design.
+- R. C. Dorf, R. H. Bishop, *Modern Control Systems*, 13th ed. (2017) — Lead and lag compensator design.

numerical/controllers/implementations/CMakeLists.txt

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

numerical/controllers/implementations/LeadLagCompensator.cpp

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

numerical/controllers/implementations/LeadLagCompensator.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 <type_traits>
+
+namespace controllers
+{
+    template<typename T>
+    struct LeadLagParameters
+    {
+        static_assert(std::is_floating_point_v<T>, "LeadLagParameters supports floating-point types");
+
+        T gain;
+        T zero;
+        T pole;
+        T sampleTime;
+    };
+
+    template<typename T>
+    class LeadLagCompensator
+    {
+        static_assert(std::is_floating_point_v<T>, "LeadLagCompensator supports floating-point types");
+
+    public:
+        explicit LeadLagCompensator(LeadLagParameters<T> p);
+
+        OPTIMIZE_FOR_SPEED T Compute(T input);
+
+        void Reset(T value = T{ 0 });
+
+    private:
+        T b0{};
+        T b1{};
+        T a1{};
+        T prevInput{};
+        T prevOutput{};
+    };
+
+    template<typename T>
+    LeadLagCompensator<T>::LeadLagCompensator(LeadLagParameters<T> p)
+    {
+        T c{ T{ 2 } / p.sampleTime };
+        T n0{ p.gain * (c + p.zero) };
+        T n1{ p.gain * (p.zero - c) };
+        T d0{ c + p.pole };
+        T d1{ p.pole - c };
+        b0 = n0 / d0;
+        b1 = n1 / d0;
+        a1 = d1 / d0;
+    }
+
+    template<typename T>
+    OPTIMIZE_FOR_SPEED T LeadLagCompensator<T>::Compute(T input)
+    {
+        T output{ b0 * input + b1 * prevInput - a1 * prevOutput };
+        prevInput = input;
+        prevOutput = output;
+        return output;
+    }
+
+    template<typename T>
+    void LeadLagCompensator<T>::Reset(T value)
+    {
+        prevInput = value;
+        prevOutput = value;
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class LeadLagCompensator<float>;
+#endif
+}

numerical/controllers/implementations/test/CMakeLists.txt

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

numerical/controllers/implementations/test/TestLeadLagCompensator.cpp

@@ -0,0 +1,118 @@
+#include "numerical/controllers/implementations/LeadLagCompensator.hpp"
+#include "numerical/math/Tolerance.hpp"
+#include "gtest/gtest.h"
+
+namespace
+{
+    class TestLeadLagCompensator
+        : public ::testing::Test
+    {
+    public:
+        controllers::LeadLagParameters<float> params{ 1.0f, 1.0f, 10.0f, 0.01f };
+        controllers::LeadLagCompensator<float> comp{ params };
+    };
+}
+
+TEST_F(TestLeadLagCompensator, dc_gain_matches_continuous)
+{
+    for (int i = 0; i < 2000; ++i)
+        comp.Compute(1.0f);
+
+    float steadyState{ comp.Compute(1.0f) };
+    float expected{ params.gain * params.zero / params.pole };
+    EXPECT_NEAR(steadyState, expected, 1e-3f);
+}
+
+TEST_F(TestLeadLagCompensator, lead_produces_initial_overshoot)
+{
+    float firstOut{ comp.Compute(1.0f) };
+
+    for (int i = 0; i < 2000; ++i)
+        comp.Compute(1.0f);
+    float steadyState{ comp.Compute(1.0f) };
+
+    EXPECT_GT(firstOut, steadyState);
+}
+
+TEST_F(TestLeadLagCompensator, lag_has_no_derivative_kick)
+{
+    controllers::LeadLagParameters<float> lagParams{ 1.0f, 10.0f, 1.0f, 0.01f };
+    controllers::LeadLagCompensator<float> lagComp{ lagParams };
+
+    float prev{ lagComp.Compute(1.0f) };
+    bool monotone{ true };
+    for (int i = 0; i < 100; ++i)
+    {
+        float cur{ lagComp.Compute(1.0f) };
+        if (cur < prev)
+        {
+            monotone = false;
+            break;
+        }
+        prev = cur;
+    }
+    EXPECT_TRUE(monotone);
+}
+
+TEST_F(TestLeadLagCompensator, impulse_response_is_stable)
+{
+    comp.Compute(1.0f);
+    for (int i = 0; i < 1000; ++i)
+        comp.Compute(0.0f);
+
+    float tail{ comp.Compute(0.0f) };
+    EXPECT_NEAR(tail, 0.0f, 1e-3f);
+}
+
+TEST_F(TestLeadLagCompensator, coefficients_match_tustin_design)
+{
+    float c{ 2.0f / params.sampleTime };
+    float d0{ c + params.pole };
+    float expectedB0{ params.gain * (c + params.zero) / d0 };
+    float expectedB1{ params.gain * (params.zero - c) / d0 };
+    float expectedA1{ (params.pole - c) / d0 };
+
+    float y0{ comp.Compute(1.0f) };
+    EXPECT_NEAR(y0, expectedB0, 1e-5f);
+
+    float y1{ comp.Compute(0.0f) };
+    EXPECT_NEAR(y1, expectedB1 - expectedA1 * y0, 1e-5f);
+}
+
+TEST_F(TestLeadLagCompensator, reset_clears_history)
+{
+    for (int i = 0; i < 10; ++i)
+        comp.Compute(1.0f);
+
+    comp.Reset(0.0f);
+
+    float c{ 2.0f / params.sampleTime };
+    float d0{ c + params.pole };
+    float b0{ params.gain * (c + params.zero) / d0 };
+
+    float nextInput{ 0.5f };
+    float out{ comp.Compute(nextInput) };
+    EXPECT_NEAR(out, b0 * nextInput, 1e-5f);
+}
+
+TEST_F(TestLeadLagCompensator, step_reaches_expected_steady_state)
+{
+    for (int i = 0; i < 5000; ++i)
+        comp.Compute(1.0f);
+
+    float finalVal{ comp.Compute(1.0f) };
+    float expected{ params.gain * params.zero / params.pole };
+    EXPECT_NEAR(finalVal, expected, 1e-3f);
+}
+
+TEST_F(TestLeadLagCompensator, unity_when_zero_equals_pole)
+{
+    controllers::LeadLagParameters<float> unityParams{ 2.0f, 5.0f, 5.0f, 0.01f };
+    controllers::LeadLagCompensator<float> unityComp{ unityParams };
+
+    for (int i = 0; i < 2000; ++i)
+        unityComp.Compute(1.0f);
+
+    float steadyState{ unityComp.Compute(1.0f) };
+    EXPECT_NEAR(steadyState, unityParams.gain, 1e-3f);
+}