feat: add disturbance observer (#209)

* add disturbance observer

* Apply suggestions from code review

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

* remove duplication

---------

Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>

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)                     | 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,7 +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)    | ★★★★☆      |
 | 37 | Hilbert transform / analytic signal / envelope       | `analysis`                | ★★★★☆      |
 | 38 | Discrete Wavelet Transform (Haar / Daubechies)       | `analysis`                | ★★★★☆      |
@@ -207,12 +206,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/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.

doc/robust_control/README.md

@@ -6,4 +6,5 @@ Algorithms for robust control design: controllers that explicitly account for di
 
 | Algorithm                                     | Description                                                                                                                                                                    |
 |-----------------------------------------------|--------------------------------------------------------------------------------------------------------------------------------------------------------------------------------|
-| [Sliding Mode Control](SlidingModeControl.md) | Variable-structure controller driving the state onto a sliding surface with a boundary layer to suppress chattering — robust to matched disturbances and parameter uncertainty |
+| [Sliding Mode Control](SlidingModeControl.md)         | Variable-structure controller driving the state onto a sliding surface with a boundary layer to suppress chattering — robust to matched disturbances and parameter uncertainty |
+| [Disturbance Observer](DisturbanceObserver.md)        | Estimates lumped disturbance and model mismatch via the nominal plant inverse and a Q-filter, cancelling the disturbance to make the real plant behave like the nominal model  |

numerical/robust_control/CMakeLists.txt

@@ -7,15 +7,18 @@ target_include_directories(numerical.robust_control ${NUMERICAL_VISIBILITY}
 
 target_link_libraries(numerical.robust_control ${NUMERICAL_VISIBILITY}
     numerical.math
+    numerical.filters.passive
     numerical.solver
     infra.util
 )
 
 target_sources(numerical.robust_control PRIVATE
+    DisturbanceObserver.hpp
     SlidingModeControl.hpp
 )
 
 numerical_add_coverage_sources(numerical.robust_control
+    DisturbanceObserver.cpp
     SlidingModeControl.cpp
 )
 

numerical/robust_control/DisturbanceObserver.cpp

@@ -0,0 +1,7 @@
+// Copyright (c) 2024, Numerical Toolbox Contributors. All rights reserved.
+#include "numerical/robust_control/DisturbanceObserver.hpp"
+
+namespace robust_control
+{
+    template class DisturbanceObserver<float, 1, 1, 1>;
+}

numerical/robust_control/DisturbanceObserver.hpp

@@ -0,0 +1,164 @@
+// Copyright (c) 2024, Numerical Toolbox Contributors. All rights reserved.
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/filters/passive/BiquadCascade.hpp"
+#include "numerical/math/CompilerOptimizations.hpp"
+#include "numerical/math/LinearTimeInvariant.hpp"
+#include "numerical/math/Matrix.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <type_traits>
+
+namespace robust_control
+{
+    template<typename T,
+        std::size_t StateSize,
+        std::size_t InputSize,
+        std::size_t OutputSize>
+    class DisturbanceObserver
+    {
+        static_assert(std::is_floating_point_v<T>, "DisturbanceObserver supports floating-point types");
+        static_assert(StateSize > 0 && InputSize > 0 && OutputSize > 0,
+            "DisturbanceObserver requires positive dimensions");
+        static_assert(InputSize == OutputSize,
+            "DisturbanceObserver requires InputSize == OutputSize for per-channel disturbance pairing");
+
+    public:
+        using PlantType = math::LinearTimeInvariant<T, StateSize, InputSize, OutputSize>;
+        using InputVector = math::Vector<T, InputSize>;
+        using OutputVector = math::Vector<T, OutputSize>;
+
+        using QCoeffs = filters::passive::BiquadCoeffs<T>;
+
+        DisturbanceObserver(const PlantType& nominalPlant, const QCoeffs& q);
+
+        OPTIMIZE_FOR_SPEED InputVector Compute(const InputVector& nominalControl,
+            const OutputVector& measuredOutput);
+
+        [[nodiscard]] const InputVector& Disturbance() const;
+
+        void Reset();
+
+    private:
+        static std::array<T, InputSize> ComputeDcGainInverses(const PlantType& plant);
+        static std::array<filters::passive::BiquadCascade<T, 1>, InputSize> MakeQFilters(const QCoeffs& q);
+
+        PlantType nominalPlant;
+        std::array<T, InputSize> dcGainInv;
+        std::array<filters::passive::BiquadCascade<T, 1>, InputSize> qInvFilters;
+        std::array<filters::passive::BiquadCascade<T, 1>, InputSize> qFilters;
+        InputVector disturbance{};
+        InputVector appliedPrev{};
+    };
+
+    namespace detail
+    {
+        template<typename T, std::size_t N>
+        T ComputeSisoSteadyStateDcGain(
+            const math::LinearTimeInvariant<T, N, 1, 1>& plant)
+        {
+            math::Vector<T, N> x{};
+            math::Vector<T, 1> u{};
+            u.at(0, 0) = T{ 1 };
+            for (std::size_t k{ 0 }; k < 512; ++k)
+                x = plant.Step(x, u);
+            const auto y{ plant.Output(x, u) };
+            return y.at(0, 0);
+        }
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    std::array<T, InputSize>
+    DisturbanceObserver<T, StateSize, InputSize, OutputSize>::ComputeDcGainInverses(
+        const PlantType& plant)
+    {
+        std::array<T, InputSize> result{};
+        for (std::size_t ch{ 0 }; ch < InputSize; ++ch)
+        {
+            using SisoPlant = math::LinearTimeInvariant<T, StateSize, 1, 1>;
+            SisoPlant siso{};
+            for (std::size_t r{ 0 }; r < StateSize; ++r)
+            {
+                for (std::size_t c{ 0 }; c < StateSize; ++c)
+                    siso.A.at(r, c) = plant.A.at(r, c);
+                siso.B.at(r, 0) = plant.B.at(r, ch);
+                siso.C.at(0, r) = plant.C.at(ch, r);
+            }
+            siso.D.at(0, 0) = plant.D.at(ch, ch);
+
+            const T gain{ detail::ComputeSisoSteadyStateDcGain(siso) };
+            result[ch] = (gain == T{ 0 }) ? T{ 1 } : T{ 1 } / gain;
+        }
+        return result;
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    DisturbanceObserver<T, StateSize, InputSize, OutputSize>::DisturbanceObserver(
+        const PlantType& nominalPlant,
+        const QCoeffs& q)
+        : nominalPlant{ nominalPlant }
+        , dcGainInv{ ComputeDcGainInverses(nominalPlant) }
+        , qInvFilters{ MakeQFilters(q) }
+        , qFilters{ MakeQFilters(q) }
+    {}
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    std::array<filters::passive::BiquadCascade<T, 1>, InputSize>
+    DisturbanceObserver<T, StateSize, InputSize, OutputSize>::MakeQFilters(const QCoeffs& q)
+    {
+        return [&]<std::size_t... Is>(std::index_sequence<Is...>)
+            -> std::array<filters::passive::BiquadCascade<T, 1>, InputSize>
+        {
+            return { ((void)Is, filters::passive::BiquadCascade<T, 1>{ { q } })... };
+        }(std::make_index_sequence<InputSize>{});
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    OPTIMIZE_FOR_SPEED typename DisturbanceObserver<T, StateSize, InputSize, OutputSize>::InputVector
+    DisturbanceObserver<T, StateSize, InputSize, OutputSize>::Compute(
+        const InputVector& nominalControl,
+        const OutputVector& measuredOutput)
+    {
+        for (std::size_t ch{ 0 }; ch < InputSize; ++ch)
+        {
+            const T a{ qInvFilters[ch].Filter(measuredOutput.at(ch, 0) * dcGainInv[ch]) };
+            const T b{ qFilters[ch].Filter(appliedPrev.at(ch, 0)) };
+            disturbance.at(ch, 0) = a - b;
+        }
+
+        InputVector u{};
+        for (std::size_t ch{ 0 }; ch < InputSize; ++ch)
+            u.at(ch, 0) = nominalControl.at(ch, 0) - disturbance.at(ch, 0);
+
+        appliedPrev = u;
+        return u;
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    const typename DisturbanceObserver<T, StateSize, InputSize, OutputSize>::InputVector&
+    DisturbanceObserver<T, StateSize, InputSize, OutputSize>::Disturbance() const
+    {
+        return disturbance;
+    }
+
+    template<typename T, std::size_t StateSize, std::size_t InputSize, std::size_t OutputSize>
+    void DisturbanceObserver<T, StateSize, InputSize, OutputSize>::Reset()
+    {
+        disturbance = InputVector{};
+        appliedPrev = InputVector{};
+        for (std::size_t ch{ 0 }; ch < InputSize; ++ch)
+        {
+            qInvFilters[ch].Reset();
+            qFilters[ch].Reset();
+        }
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template class DisturbanceObserver<float, 1, 1, 1>;
+#endif
+}

numerical/robust_control/test/CMakeLists.txt

@@ -8,5 +8,6 @@ target_link_libraries(numerical.robust_control_test PUBLIC
 )
 
 target_sources(numerical.robust_control_test PRIVATE
+    TestDisturbanceObserver.cpp
     TestSlidingModeControl.cpp
 )