Merge branch 'main' into feature/add-hilbert-transform

Author: gabrielfrasantos

README.md

@@ -28,7 +28,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 |
-| [Robust Control](doc/robust_control/README.md)                     | Sliding Mode Control (SMC)                                           |
+| [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                                         |
 
 Each category page lists its algorithms with a brief description and links to the detailed documentation.

ROADMAP.md

@@ -27,8 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 35 | Disturbance Observer (DOB)                           | `robust_control` (new)    | ★★★★☆      |
-| 36 | Active Disturbance Rejection Control (ADRC + ESO)    | `robust_control` (new)    | ★★★★☆      |
 | 38 | Discrete Wavelet Transform (Haar / Daubechies)       | `analysis`                | ★★★★☆      |
 | 39 | Square-root / Information Kalman filter              | `filters/active`          | ★★★★☆      |
 | 40 | Feedback linearization                               | `nonlinear_control` (new) | ★★★★☆      |
@@ -206,12 +204,6 @@ the library does not yet expose. Detailed below under
 - **Reuses:** **Item 18 (Quaternion)**, `math::Geometry3D`.
 
 
-### 35. Disturbance Observer (DOB)  *(float-first)*
-- **What:** Estimates and cancels lumped disturbance/model mismatch using the plant inverse and a Q-filter.
-- **Embedded value:** Bolt-on robustness for existing loops — strong disturbance rejection without redesigning the nominal controller.
-- **Algorithm / paper:** W.-H. Chen, J. Yang, L. Guo, S. Li, "Disturbance-Observer-Based Control and Related Methods—An Overview," *IEEE Trans. Ind. Electron.*, 63(2), 2016.
-- **Reuses:** Item 15 (Q-filter), item 19 (observer), `math::LinearTimeInvariant`.
-
 ### 36. Active Disturbance Rejection Control (ADRC + ESO)  *(float-first)*
 - **What:** Extended State Observer estimates total disturbance as an augmented state; a feedback law cancels it in real time.
 - **Embedded value:** Near model-free, strongly robust motion control; increasingly standard in industrial drives.

doc/robust_control/ActiveDisturbanceRejection.md

@@ -0,0 +1,123 @@
+# Active Disturbance Rejection Control (ADRC + ESO)
+
+## Overview & Motivation
+
+Active Disturbance Rejection Control addresses a fundamental tension in feedback design: high-performance control normally requires an accurate plant model, yet accurate models are expensive to identify and degrade with temperature, load, and wear. ADRC resolves this by treating everything beyond a known input gain — unmodeled dynamics, parameter variation, and external disturbances — as a single lumped signal called the *total disturbance*. An Extended State Observer (ESO) estimates this signal in real time, and the control law subtracts the estimate before issuing the command. What remains behaves like a clean chain of integrators that a simple PD law can regulate with textbook bandwidth.
+
+The practical payoff on embedded hardware is significant: you need only one plant number ($b_0$, the rough input gain) and two tuning dials. The controller then survives a bad model because any mismatch is absorbed into the disturbance estimate.
+
+## Mathematical Theory
+
+### Plant Representation
+
+An $n$-th order SISO plant is written as the canonical integrator chain plus a total-disturbance term $f$:
+
+$$y^{(n)} = f(t, y, \dot{y}, \ldots, d) + b_0 u$$
+
+where $f$ captures unmodeled dynamics, nonlinearities, and external loads; $b_0$ is a nominal input-gain estimate; and $u$ is the control input.
+
+### Extended State Observer
+
+Augmenting the $n$ plant states with $x_{n+1} = f$ yields an $(n+1)$-dimensional system. The continuous ESO is a Luenberger-type observer driven by the output error:
+
+$$\dot{\hat{x}}_i = \hat{x}_{i+1} + \beta_i (y - \hat{x}_1), \quad i = 1, \ldots, n$$
+$$\dot{\hat{x}}_{n+1} = \beta_{n+1} (y - \hat{x}_1)$$
+
+with the convention $\hat{x}_{n+1} = \hat{f}$ and $\hat{x}_2$ through $\hat{x}_n$ as derivative estimates.
+
+The forward-Euler discretization used here is:
+
+$$\hat{x}_i[k+1] = \hat{x}_i[k] + T_s \bigl(\beta_i \, e[k] + \hat{x}_{i+1}[k]\bigr), \quad e[k] = y[k] - \hat{x}_1[k]$$
+
+with $b_0 u[k-1]$ injected into the $(n)$-th state to drive the highest derivative.
+
+### Bandwidth Parameterization (Gao)
+
+All observer poles are placed at $-\omega_o$ (Gao's bandwidth parameterization). The resulting gains follow the binomial expansion of $(\lambda + \omega_o)^{n+1}$:
+
+$$\beta_i = \binom{n+1}{i} \omega_o^i, \quad i = 1, \ldots, n+1$$
+
+All control poles are placed at $-\omega_c$ via the expansion of $(\lambda + \omega_c)^n$:
+
+$$k_i = \binom{n}{i} \omega_c^i, \quad i = 1, \ldots, n$$
+
+For a second-order plant ($n = 2$):
+
+$$\beta = [3\omega_o,\; 3\omega_o^2,\; \omega_o^3], \quad k = [\omega_c^2,\; 2\omega_c]$$
+
+### Control Law
+
+After disturbance estimation the control is:
+
+$$u = \frac{u_0 - \hat{f}}{b_0}, \qquad u_0 = k_1(r - \hat{x}_1) - \sum_{i=2}^{n} k_i \hat{x}_i$$
+
+Substituting into the plant equation and using $\hat{f} \approx f$ gives the closed-loop residual $y^{(n)} \approx u_0$, a pure integrator chain under a PD law — independent of the original plant dynamics.
+
+## Complexity Analysis
+
+| Case    | Time   | Space  | Notes                                   |
+|---------|--------|--------|-----------------------------------------|
+| Best    | $O(n)$ | $O(n)$ | Linear sweep over $n+1$ ESO states      |
+| Average | $O(n)$ | $O(n)$ | Same; gains precomputed at construction |
+| Worst   | $O(n)$ | $O(n)$ | No branching in the hot path            |
+
+Gains are computed once at construction from closed-form binomial formulas in $O(n)$ time. The `Compute` hot path is a pair of $O(n)$ loops with no dynamic allocation.
+
+## Step-by-Step Walkthrough
+
+Second-order plant ($n=2$), $\omega_o = 30$, $\omega_c = 6$, $b_0 = 1$, $T_s = 0.001$ s.
+
+Observer gains: $\beta_1 = 90$, $\beta_2 = 2700$, $\beta_3 = 27000$.  
+Control gains: $k_p = 36$, $k_d = 12$.
+
+At sample $k$ with state $\hat{x} = [\hat{y}, \hat{\dot{y}}, \hat{f}]$, measurement $y[k]$, reference $r$:
+
+1. Output error: $e = y[k] - \hat{y}$.
+2. Inject correction into all three states: $\hat{x}_i \mathrel{+}= T_s \beta_i e$.
+3. Chain integration: $\hat{y} \mathrel{+}= T_s \hat{\dot{y}}$; then $\hat{\dot{y}} \mathrel{+}= T_s b_0 u[k-1]$.
+4. PD law on integrator chain: $u_0 = k_p(r - \hat{y}) - k_d \hat{\dot{y}}$.
+5. Disturbance cancellation: $u = (u_0 - \hat{f}) / b_0$.
+
+After a transient of roughly $5/\omega_o \approx 0.17$ s the observer converges; the output tracks $r$ with bandwidth $\omega_c$.
+
+## Pitfalls & Edge Cases
+
+**ESO peaking.** Large initial estimation errors drive high-magnitude corrections, temporarily saturating the actuator. Mitigation: initialize the observer near the first measurement, or schedule $\omega_o$ upward from a low value during the first few samples.
+
+**Observer bandwidth vs. noise.** Increasing $\omega_o$ speeds convergence but amplifies measurement noise because $\beta_3 = \omega_o^3$ grows cubically. A practical rule of thumb is $\omega_o \in [3\omega_c, 10\omega_c]$.
+
+**$b_0$ mismatch.** The ESO is robust to moderate mismatch (factor of 2–3), but large errors shrink the stability margin. If $b_0 \gg b_\text{true}$ the effective loop gain drops and response slows; if $b_0 \ll b_\text{true}$ the loop gain rises and may oscillate.
+
+**Euler discretization accuracy.** The forward-Euler ESO introduces phase lag proportional to $\omega_o T_s$. Keeping $\omega_o T_s \ll 1$ (e.g., $\omega_o T_s \leq 0.1$) maintains accuracy; at higher $\omega_o T_s$ a ZOH or bilinear discretization is preferred.
+
+**Integer overflow in gain computation.** Binomial coefficients are computed with integer arithmetic at compile time. For large orders or very high bandwidths the intermediate product may exceed `std::size_t` before the division; keep $n \leq 5$ in practice.
+
+## Variants & Generalizations
+
+**Nonlinear ESO (NESO).** Replace the linear correction $\beta_i e$ with Han's fal function to reduce peaking while preserving fast convergence.
+
+**Discrete ESO.** Exact discretization of the observer (ZOH or pole-matched) improves accuracy when $\omega_o T_s$ is not small.
+
+**Higher-order plants.** The template parameter `Order` generalizes the same bandwidth-parameterized structure to $n > 2$ — gains grow binomially and the `Compute` loop extends automatically.
+
+**Multi-input / multi-output (MIMO).** Each output channel runs an independent ADRC; cross-coupling is absorbed into the respective disturbance estimates.
+
+## Applications
+
+- Electric motor drives (rejects friction, load torque, and back-EMF variation with a single $b_0$ estimate).
+- Attitude and position control of UAVs and satellites (absorbs aerodynamic and thruster uncertainty).
+- Industrial process control where the plant model is poorly known or time-varying.
+- Hard-disk drive servo (high-bandwidth disturbance rejection without a detailed head-media model).
+
+## Connections to Other Algorithms
+
+- **Luenberger Observer** — the ESO is a Luenberger observer augmented with one extra disturbance state.
+- **Disturbance Observer (DOB)** — the transfer-function sibling; DOB works in the frequency domain while ESO works in the state-space domain.
+- **PID** — ADRC generalizes PID: a first-order ADRC with proportional-plus-integral action recovers a PI with disturbance feed-forward.
+- **LQR / LQI** — state-feedback alternatives that require a full model; ADRC trades optimality for model-independence.
+
+## References & Further Reading
+
+- J. Han, "From PID to Active Disturbance Rejection Control," *IEEE Transactions on Industrial Electronics*, vol. 56, no. 3, pp. 900–906, 2009.
+- Z. Gao, "Scaling and Bandwidth-Parameterization Based Controller Tuning," *Proceedings of the American Control Conference*, 2003, pp. 4989–4996.
+- R. Miklosovic, A. Radke, Z. Gao, "Discrete implementation and generalization of the extended state observer," *ACC*, 2006.

doc/robust_control/DisturbanceObserver.md

@@ -0,0 +1,116 @@
+# Disturbance Observer
+
+## Overview & Motivation
+
+Real plants never match their nominal models. External loads, friction, actuator nonlinearities, and parameter drift inject unmodeled energy into the loop. A Disturbance Observer (DOB) lumps all of these effects into a single **equivalent disturbance** signal, estimates it online, and subtracts it from the control input so the plant behaves as if it were the clean nominal model.
+
+The key insight is that the DOB wraps around any existing controller without redesigning it. An engineer who has already tuned a PID or LQR for the nominal plant can bolt on a DOB and gain strong disturbance rejection without revisiting the nominal design. This makes DOBs especially attractive for embedded motion controllers — motor drives, precision stages, robotic joints — where the plant is moderately well-known but subject to load variations the nominal model ignores.
+
+## Mathematical Theory
+
+### Setup
+
+Let the true discrete-time plant be $P(z)$ and the nominal model be $P_n(z)$. The control input seen by the true plant is $u_a = u + d$, where $u$ is the commanded input and $d$ is the lumped equivalent disturbance that captures model mismatch, external loads, and friction. The plant output is
+
+$$y = P(z)\, u_a = P(z)(u + d).$$
+
+### Disturbance Estimate
+
+If $P_n^{-1}(z)$ is applied to $y$, it reconstructs the effective input that the nominal plant would have needed to produce that output:
+
+$$P_n^{-1}(z)\, y \approx u + d \quad \text{(if } P \approx P_n\text{)}.$$
+
+Subtracting the actual commanded input $u$ isolates the disturbance:
+
+$$\hat{d} = P_n^{-1}(z)\, y - u.$$
+
+### Q-Filter and Properness
+
+The plant inverse $P_n^{-1}(z)$ is generally improper (more zeros than poles) and amplifies high-frequency measurement noise. A low-pass **Q-filter** $Q(z)$ is cascaded to make the combination $Q(z)\,P_n^{-1}(z)$ proper and bandwidth-limited:
+
+$$\hat{d} = Q(z)\,P_n^{-1}(z)\, y - Q(z)\, u.$$
+
+The filter $Q(z)$ must have relative degree at least equal to the relative degree of $P_n(z)$ so the realization does not differentiate. Unity DC gain, $Q(1) = 1$, is required for complete rejection of constant (step) disturbances.
+
+### Control Law
+
+The DOB corrects the nominal controller output $c$ by subtracting the estimate:
+
+$$u = c - \hat{d}.$$
+
+The closed-loop system then sees an effective plant of $P_n(z)$ inside the Q-filter bandwidth — the actual mismatch and disturbances are cancelled — and approaches the uncorrected nominal plant behaviour outside the bandwidth.
+
+### Frequency-Domain Interpretation
+
+Let $L(z) = Q(z)\,P_n^{-1}(z)\,P(z)$. The closed-loop sensitivity from disturbance $d$ to output $y$ is
+
+$$S_d(z) = \frac{P(z)(1 - Q(z))}{1 + P(z)C(z)(1 - Q(z))}.$$
+
+Inside the Q-filter passband ($Q \approx 1$): $S_d \approx 0$ — the disturbance is rejected.
+Outside the passband ($Q \approx 0$): $S_d$ equals the nominal sensitivity — the DOB is transparent.
+
+### Stability Robustness
+
+Robust stability requires the complementary sensitivity of the inner DOB loop to satisfy
+
+$$\left|Q(e^{j\omega})\,\Delta_m(e^{j\omega})\right| < 1 \quad \forall\, \omega,$$
+
+where $\Delta_m = (P - P_n)/P_n$ is the relative model uncertainty. Widening $Q$ improves disturbance rejection but shrinks the robust-stability margin — this is the fundamental DOB trade-off.
+
+## Complexity Analysis
+
+| Operation | Time             | Space        | Notes                                          |
+|-----------|------------------|--------------|------------------------------------------------|
+| Construct | $O(N \cdot S^2)$ | $O(S^2 + N)$ | DC gain simulation, $S$ = StateSize, $N$ = 512 |
+| Compute   | $O(N_{\rm in})$  | $O(1)$ extra | Per-channel biquad filter pair                 |
+| Reset     | $O(N_{\rm in})$  | $O(1)$ extra | Clears filter states                           |
+
+All storage is fixed-size; no heap allocation occurs at any point in the lifecycle.
+
+## Step-by-Step Walkthrough
+
+Consider a first-order discrete plant ($n=1$, $m=p=1$) with $a=0.9$, $b=0.1$, $c=1$, DC gain $= b/(1-a) = 1$, and a second-order Butterworth Q-filter at 20 Hz (sample rate 1 kHz).
+
+**Steady-state with constant disturbance $d = 0.5$, nominal command $c = 0$:**
+
+1. Plant output settles to $y_{ss} = P(1)\,d = 1 \cdot 0.5 = 0.5$.
+2. Q-filter path 1: $Q(1)\,P_n^{-1}(1)\,y_{ss} = 1 \cdot 1 \cdot 0.5 = 0.5$.
+3. Q-filter path 2: $Q(1)\,u_{ss} = 1 \cdot (c - \hat{d})_{ss}$.
+4. At equilibrium path 1 $-$ path 2 $= \hat{d}$ and $u_{ss} = c - \hat{d}$, giving $\hat{d} = 0.5 = d$. The estimate converges exactly.
+5. The corrected input is $u = 0 - 0.5 = -0.5$, so the effective input to the plant is $-0.5 + 0.5 = 0$ — the disturbance is cancelled.
+
+## Pitfalls & Edge Cases
+
+- **Non-minimum-phase plants**: $P_n^{-1}(z)$ has unstable poles when $P_n$ has zeros outside the unit circle. The DOB inner loop becomes unstable; non-minimum-phase zeros must be treated specially or the DOB must not be applied directly.
+- **DC gain of zero**: if the nominal plant has no steady-state response to the input, the inverse gain is ill-defined. The implementation guards against division by zero but the DOB will not function correctly.
+- **Wide Q bandwidth**: increasing the cutoff trades rejection bandwidth for noise amplification and reduced robustness to model mismatch. The trade-off is captured by the robust-stability bound above.
+- **Large model mismatch**: when $|\Delta_m|$ is not small, the DOB may amplify rather than cancel the disturbance. The bandwidth of $Q$ must be restricted so the robustness condition holds across the frequency range of significant mismatch.
+- **Unstable nominal plant**: the steady-state DC-gain simulation used during construction diverges; only stable nominal plants are supported.
+
+## Variants & Generalizations
+
+- **Two-degree-of-freedom DOB**: a separate reference pre-filter shapes the tracking response independently of the disturbance rejection channel.
+- **Nonlinear DOB**: replaces the linear inverse with a nonlinear observer (e.g., extended high-gain observer) for plants with known nonlinear structure.
+- **Time-varying Q**: adapts the Q-filter bandwidth online to balance rejection versus robustness as operating conditions change.
+- **Multi-input multi-output (MIMO) DOB**: generalises the scalar channel-pairing to full matrix $P_n^{-1}$, requiring the nominal plant to be square and invertible.
+
+## Applications
+
+- Precision motion control: rejects cutting forces in CNC machines and friction in ball-screw drives.
+- Robotic joint torque control: cancels gravity, Coriolis, and friction terms without explicit model inversion.
+- Hard-disk drive servo: one of the earliest industrial applications; Q-filter bandwidth sets the track-following bandwidth.
+- Power electronics: rejects grid-voltage disturbances in inverter current control.
+
+## Connections to Other Algorithms
+
+- **Sliding Mode Control**: an alternative approach to matched-disturbance rejection via a switching term; SMC is discontinuous and model-free while the DOB is smooth and model-based.
+- **Luenberger Observer / Kalman Filter**: estimate state from measurements; the DOB estimates disturbance from input-output pairs without augmenting the state.
+- **Active Disturbance Rejection Control (ADRC)**: treats total disturbance as an augmented state in a full observer; conceptually similar to DOB but parameterised through observer bandwidth rather than a Q-filter.
+- **BiquadCascade**: the Q-filter is realised directly as a second-order IIR section.
+
+## References & Further Reading
+
+- W.-H. Chen, J. Yang, L. Guo, S. Li, "Disturbance-Observer-Based Control and Related Methods — An Overview," *IEEE Transactions on Industrial Electronics*, 63(2), pp. 1083–1095, 2016.
+- K. Ohishi, M. Nakao, K. Ohnishi, K. Miyachi, "Microprocessor-Controlled DC Motor for Load-Insensitive Position Servo System," *IEEE Transactions on Industrial Electronics*, 34(1), pp. 44–49, 1987.
+- E. Schrijver, J. van Dijk, "Disturbance Observers for Rigid Mechanical Systems: Equivalence, Stability, and Design," *ASME Journal of Dynamic Systems, Measurement, and Control*, 124(4), pp. 539–548, 2002.
+- S. Komada, K. Ohnishi, "Force Feedback Control of Robot Manipulator by the Acceleration Tracing Orientation Method," *IEEE Transactions on Industrial Electronics*, 37(1), pp. 6–12, 1990.