add biquad filter

Author: gabrielfrasantos

README.md

@@ -21,7 +21,7 @@ Refer to the documentation to quickly integrate and utilize the library's signal
 | [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, 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 |
+| [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 |
 | [Kinematics](doc/kinematics/README.md)                             | Forward Kinematics                                                   |
 | [Neural Network](doc/neural_network/README.md)                     | Layers, activations, losses, model                                   |
 | [Optimization](doc/optimization/README.md)                         | Gradient Descent                                                     |

ROADMAP.md

@@ -27,7 +27,6 @@ Difficulty legend:
 
 | #  | Component                                            | Target module             | Difficulty |
 |----|------------------------------------------------------|---------------------------|------------|
-| 15 | Biquad / Second-Order-Section cascade                | `filters/passive`         | ★★★☆☆      |
 | 20 | Integral / servo state feedback (LQI)                | `controllers`             | ★★★☆☆      |
 | 27 | QR decomposition (Householder / Givens)              | `solvers`                 | ★★★★☆      |
 | 28 | LU decomposition with partial pivoting               | `solvers`                 | ★★★★☆      |
@@ -123,12 +122,6 @@ Difficulty legend:
 
 ## Tier 3 — Moderate ★★★☆☆
 
-### 15. Biquad / Second-Order-Section cascade
-- **What:** High-order IIR realized as a cascade of second-order sections (Direct Form I / Transposed Direct Form II).
-- **Embedded value:** **Highest-value passive-filter gap.** Direct-form high-order IIR is numerically fragile in `Q15`/`Q31`; SOS cascade is the standard robust realization. A float biquad already lives only in the [IIR simulator](simulator/filters/IirFilter/application/IirFilterSimulator.hpp); [Iir.md](doc/filters/passive/Iir.md) explicitly lists SOS as future work.
-- **Algorithm / paper:** Oppenheim & Schafer, *DTSP*, Ch. 6 (cascade/parallel structures); R. Bristow-Johnson, "Cookbook formulae for audio EQ biquad filter coefficients."
-- **Reuses:** [Iir.hpp](numerical/filters/passive/Iir.hpp), `math::RecursiveBuffer`.
-
 ### 16. Notch / comb filter
 - **What:** Narrow-band rejection (notch biquad) and periodic comb rejection.
 - **Embedded value:** Removes 50/60 Hz mains hum and harmonic interference from bio-signals and instrumentation.

doc/filters/passive/BiquadCascade.md

@@ -0,0 +1,140 @@
+# Biquad / Second-Order-Section Cascade
+
+## Overview & Motivation
+
+High-order IIR filters realized as a single monolithic direct-form transfer function suffer
+from severe numerical sensitivity: small perturbations in any coefficient can shift poles
+dramatically, and round-off noise accumulates in proportion to the filter order. These
+effects are catastrophic on fixed-point hardware and non-trivial even in single-precision
+floating-point.
+
+The solution adopted universally in professional DSP is to factor the transfer function into
+quadratic terms — each with exactly two poles and two zeros — and chain them in series.
+This cascade of second-order sections ("biquads") keeps each section's coefficients
+well-scaled, its pole-sensitivity small, and its round-off noise bounded independently of
+the total filter order.
+
+## Mathematical Theory
+
+### Transfer Function Factorization
+
+Any real-coefficient rational transfer function of order $N$ factors as:
+
+$$H(z) = G \prod_{k=1}^{\lceil N/2 \rceil} \frac{b_{0,k} + b_{1,k}z^{-1} + b_{2,k}z^{-2}}{1 + a_{1,k}z^{-1} + a_{2,k}z^{-2}}$$
+
+Each factor is a second-order section (SOS). The overall frequency response is the product
+of the per-section responses; the total group delay is the sum of per-section group delays.
+
+### RBJ Cookbook Design Equations
+
+For standard shelving, peaking, and pass/reject types, closed-form normalized coefficients
+derive from the bilinear transform applied to an analog prototype. Define:
+
+$$\omega_0 = \frac{2\pi f_c}{f_s}, \qquad c_\omega = \cos(\omega_0), \qquad \alpha = \frac{\sin(\omega_0)}{2Q}$$
+
+**Low-pass:**
+
+$$b_0 = \frac{1 - c_\omega}{2(1+\alpha)}, \quad b_1 = \frac{1-c_\omega}{1+\alpha}, \quad b_2 = b_0$$
+
+$$a_1 = \frac{-2c_\omega}{1+\alpha}, \quad a_2 = \frac{1-\alpha}{1+\alpha}$$
+
+**High-pass:** replace $(1 - c_\omega)$ with $(1 + c_\omega)$ and negate $b_1$.
+
+**Notch (band-reject):**
+
+$$b_0 = b_2 = \frac{1}{1+\alpha}, \quad b_1 = a_1 = \frac{-2c_\omega}{1+\alpha}, \quad a_2 = \frac{1-\alpha}{1+\alpha}$$
+
+All coefficients are pre-normalized by $a_0 = 1 + \alpha$, so the feedback denominator leading
+coefficient is always 1 and the hot path requires no division.
+
+### Transposed Direct Form II (TDF-II)
+
+TDF-II is the canonical embedded realization. It minimizes the number of state registers to
+two per section while achieving low round-off noise. The recurrence for section $k$ is:
+
+$$y[n] = b_0 x[n] + z_1[n-1]$$
+
+$$z_1[n] = b_1 x[n] - a_1 y[n] + z_2[n-1]$$
+
+$$z_2[n] = b_2 x[n] - a_2 y[n]$$
+
+This is exactly five multiplies and four adds per sample — the theoretical minimum for an
+arbitrary biquad — and requires no intermediate storage beyond $z_1$ and $z_2$.
+
+## Complexity Analysis
+
+| Case       | Time          | Space         | Notes                                          |
+|------------|---------------|---------------|------------------------------------------------|
+| Per sample | $O(S)$        | $O(S)$        | $S$ = number of sections; 5 mults + 4 adds each |
+| Design     | $O(1)$        | $O(1)$        | Closed-form RBJ formulas; no iteration         |
+| Reset      | $O(S)$        | —             | Zero two state words per section               |
+
+## Step-by-Step Walkthrough
+
+**Example:** design a 4th-order low-pass as two cascaded 2nd-order sections.
+
+Given $f_c = 100$ Hz, $f_s = 1000$ Hz, $Q = 0.707$ (Butterworth factor for each section):
+
+1. Compute $\omega_0 = 2\pi \cdot 0.1 \approx 0.6283$
+2. $c_\omega = \cos(0.6283) \approx 0.8090$, $\alpha = \sin(0.6283)/(2 \times 0.707) \approx 0.4142$
+3. $a_0 = 1.4142$; normalize: $b_0 \approx 0.06745$, $b_1 \approx 0.13490$, $b_2 \approx 0.06745$,
+   $a_1 \approx -1.1429$, $a_2 \approx 0.4128$
+4. Use the same section twice to realize a 4th-order filter.
+5. At each sample, pass input through section 1, then section 2: $y = H_2(H_1(x))$.
+
+**Impulse response trace** for bypass section $\{b_0=1, b_1=b_2=a_1=a_2=0\}$:
+Input $[1, 0, 0, \ldots]$ → output $[1, 0, 0, \ldots]$ — identity passthrough.
+
+## Pitfalls & Edge Cases
+
+- **Section ordering**: sections should be ordered by increasing peak gain and poles paired with
+  their nearest zeros. Misordering can cause intermediate signals to overflow before later
+  sections attenuate them, especially in fixed-point implementations.
+- **Poles near the unit circle**: high-$Q$ or near-Nyquist designs push poles close to
+  $|z| = 1$. Finite-precision rounding can move a pole just outside, causing instability.
+  Use $Q \leq 30$ in single precision; double precision or lattice realizations for higher $Q$.
+- **Denormal floats**: small state values approaching the denormal range stall the FPU pipeline
+  on many embedded cores. Enabling flush-to-zero (FTZ) or the fast-math pragma prevents this
+  at the cost of negligible numerical error.
+- **DC gain normalization**: the RBJ low-pass has unity DC gain by construction. Gain-staging
+  between sections is not required; each section's output is well-scaled relative to its input.
+
+## Variants & Generalizations
+
+- **Direct Form I**: maintains four state variables per section (input and output history) but
+  provides extra dynamic range at the cost of higher memory. Preferred when signal levels are
+  difficult to bound.
+- **Lattice / wave-digital**: alternative topologies that remain stable under very aggressive
+  coefficient quantization; used in high-speed fixed-point applications.
+- **Parallel SOS**: sections are summed rather than chained; useful for multi-band equalizers.
+- **Second-order allpass**: $b_0 = a_2$, $b_1 = a_1$, $b_2 = 1$ — phase rotation without
+  magnitude change, used in crossover networks and polyphase systems.
+
+## Applications
+
+- Multi-pole anti-aliasing and reconstruction filters in ADC/DAC chains.
+- Audio equalization (parametric EQ, shelving filters) in embedded audio processors.
+- Vibration isolation and sensor conditioning in industrial control systems.
+- ECG/EEG baseline wander removal with high-pass SOS cascades.
+- Motor drive current-sensing loop compensation when a precise roll-off characteristic is needed.
+
+## Connections to Other Algorithms
+
+- **IIR (direct form)**: the single-section IIR is the primitive building block. A cascade of
+  biquads is simply a structured composition of single sections with superior numerical properties.
+- **NotchCombFilter**: the notch filter is a single biquad specialized to place zeros exactly
+  on the unit circle; a cascade realizes multi-pole notch designs.
+- **IirFilterDesign**: a companion design routine generates SOS coefficient arrays from an analog
+  prototype via the bilinear transform, feeding directly into the cascade.
+- **RecursiveBuffer**: the IIR simulator uses `math::RecursiveBuffer` for time-domain state;
+  the biquad avoids the overhead by keeping only two explicit state scalars per section.
+
+## References & Further Reading
+
+- R. Bristow-Johnson, "Cookbook formulae for audio EQ biquad filter coefficients",
+  <https://www.w3.org/TR/audio-eq-cookbook/>
+- A. V. Oppenheim & R. W. Schafer, *Discrete-Time Signal Processing*, 3rd ed., Ch. 6
+  (cascade and parallel structures).
+- R. G. Lyons, *Understanding Digital Signal Processing*, 3rd ed., Prentice Hall, 2011, Ch. 6.
+- S. J. Orfanidis, *Introduction to Signal Processing*, Prentice Hall, 1996, Ch. 12
+  (second-order sections and ladder filters).

numerical/filters/passive/BiquadCascade.cpp

@@ -0,0 +1,8 @@
+#include "numerical/filters/passive/BiquadCascade.hpp"
+
+namespace filters::passive
+{
+    template struct BiquadCoeffs<float>;
+    template class Biquad<float>;
+    template class BiquadCascade<float, 2>;
+}

numerical/filters/passive/BiquadCascade.hpp

@@ -0,0 +1,205 @@
+#pragma once
+
+#if defined(__GNUC__) || defined(__clang__)
+#pragma GCC optimize("O3", "fast-math")
+#endif
+
+#include "numerical/math/CompilerOptimizations.hpp"
+#include <array>
+#include <cmath>
+#include <cstddef>
+#include <numbers>
+#include <type_traits>
+
+namespace filters::passive
+{
+    template<typename T>
+    struct BiquadCoeffs
+    {
+        static_assert(std::is_floating_point_v<T>, "BiquadCoeffs supports floating-point types");
+
+        T b0{};
+        T b1{};
+        T b2{};
+        T a1{};
+        T a2{};
+    };
+
+    template<typename T>
+    class Biquad
+    {
+        static_assert(std::is_floating_point_v<T>, "Biquad supports floating-point types");
+
+    public:
+        explicit Biquad(BiquadCoeffs<T> coeffs) noexcept;
+
+        OPTIMIZE_FOR_SPEED T Filter(T x) noexcept;
+        void Reset() noexcept;
+
+        static BiquadCoeffs<T> LowPass(T fc, T fs, T Q) noexcept;
+        static BiquadCoeffs<T> HighPass(T fc, T fs, T Q) noexcept;
+        static BiquadCoeffs<T> BandPass(T fc, T fs, T Q) noexcept;
+        static BiquadCoeffs<T> Notch(T fc, T fs, T Q) noexcept;
+        static BiquadCoeffs<T> Peaking(T fc, T fs, T Q, T gainDb) noexcept;
+
+    private:
+        BiquadCoeffs<T> c{};
+        T z1{};
+        T z2{};
+    };
+
+    template<typename T, std::size_t Sections>
+    class BiquadCascade
+    {
+        static_assert(std::is_floating_point_v<T>, "BiquadCascade supports floating-point types");
+        static_assert(Sections > 0, "BiquadCascade must have at least one section");
+
+    public:
+        explicit BiquadCascade(std::array<BiquadCoeffs<T>, Sections> coeffs) noexcept;
+
+        OPTIMIZE_FOR_SPEED T Filter(T x) noexcept;
+        void Reset() noexcept;
+
+    private:
+        std::array<Biquad<T>, Sections> stages;
+    };
+
+    ////    Biquad Implementation    ////
+
+    template<typename T>
+    Biquad<T>::Biquad(BiquadCoeffs<T> coeffs) noexcept
+        : c{ coeffs }
+    {}
+
+    template<typename T>
+    OPTIMIZE_FOR_SPEED T Biquad<T>::Filter(T x) noexcept
+    {
+        const T y{ c.b0 * x + z1 };
+        z1 = c.b1 * x - c.a1 * y + z2;
+        z2 = c.b2 * x - c.a2 * y;
+        return y;
+    }
+
+    template<typename T>
+    void Biquad<T>::Reset() noexcept
+    {
+        z1 = T{};
+        z2 = T{};
+    }
+
+    template<typename T>
+    BiquadCoeffs<T> Biquad<T>::LowPass(T fc, T fs, T Q) noexcept
+    {
+        const T w0{ T{ 2 } * std::numbers::pi_v<T> * fc / fs };
+        const T cw{ std::cos(w0) };
+        const T alpha{ std::sin(w0) / (T{ 2 } * Q) };
+        const T a0{ T{ 1 } + alpha };
+        return BiquadCoeffs<T>{
+            (T{ 1 } - cw) / (T{ 2 } * a0),
+            (T{ 1 } - cw) / a0,
+            (T{ 1 } - cw) / (T{ 2 } * a0),
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha) / a0
+        };
+    }
+
+    template<typename T>
+    BiquadCoeffs<T> Biquad<T>::HighPass(T fc, T fs, T Q) noexcept
+    {
+        const T w0{ T{ 2 } * std::numbers::pi_v<T> * fc / fs };
+        const T cw{ std::cos(w0) };
+        const T alpha{ std::sin(w0) / (T{ 2 } * Q) };
+        const T a0{ T{ 1 } + alpha };
+        return BiquadCoeffs<T>{
+            (T{ 1 } + cw) / (T{ 2 } * a0),
+            -(T{ 1 } + cw) / a0,
+            (T{ 1 } + cw) / (T{ 2 } * a0),
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha) / a0
+        };
+    }
+
+    template<typename T>
+    BiquadCoeffs<T> Biquad<T>::BandPass(T fc, T fs, T Q) noexcept
+    {
+        const T w0{ T{ 2 } * std::numbers::pi_v<T> * fc / fs };
+        const T alpha{ std::sin(w0) / (T{ 2 } * Q) };
+        const T cw{ std::cos(w0) };
+        const T a0{ T{ 1 } + alpha };
+        return BiquadCoeffs<T>{
+            (std::sin(w0) / T{ 2 }) / a0,
+            T{ 0 },
+            -(std::sin(w0) / T{ 2 }) / a0,
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha) / a0
+        };
+    }
+
+    template<typename T>
+    BiquadCoeffs<T> Biquad<T>::Notch(T fc, T fs, T Q) noexcept
+    {
+        const T w0{ T{ 2 } * std::numbers::pi_v<T> * fc / fs };
+        const T cw{ std::cos(w0) };
+        const T alpha{ std::sin(w0) / (T{ 2 } * Q) };
+        const T a0{ T{ 1 } + alpha };
+        return BiquadCoeffs<T>{
+            T{ 1 } / a0,
+            (T{ -2 } * cw) / a0,
+            T{ 1 } / a0,
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha) / a0
+        };
+    }
+
+    template<typename T>
+    BiquadCoeffs<T> Biquad<T>::Peaking(T fc, T fs, T Q, T gainDb) noexcept
+    {
+        const T A{ std::pow(T{ 10 }, gainDb / T{ 40 }) };
+        const T w0{ T{ 2 } * std::numbers::pi_v<T> * fc / fs };
+        const T cw{ std::cos(w0) };
+        const T alpha{ std::sin(w0) / (T{ 2 } * Q) };
+        const T a0{ T{ 1 } + alpha / A };
+        return BiquadCoeffs<T>{
+            (T{ 1 } + alpha * A) / a0,
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha * A) / a0,
+            (T{ -2 } * cw) / a0,
+            (T{ 1 } - alpha / A) / a0
+        };
+    }
+
+    ////    BiquadCascade Implementation    ////
+
+    template<typename T, std::size_t Sections>
+    BiquadCascade<T, Sections>::BiquadCascade(std::array<BiquadCoeffs<T>, Sections> coeffs) noexcept
+        : stages{ [&]()
+            {
+                std::array<Biquad<T>, Sections> arr{ [&]<std::size_t... Is>(std::index_sequence<Is...>) -> std::array<Biquad<T>, Sections>
+                    {
+                        return { Biquad<T>{ coeffs[Is] }... };
+                    }(std::make_index_sequence<Sections>{}) };
+                return arr;
+            }() }
+    {}
+
+    template<typename T, std::size_t Sections>
+    OPTIMIZE_FOR_SPEED T BiquadCascade<T, Sections>::Filter(T x) noexcept
+    {
+        for (auto& stage : stages)
+            x = stage.Filter(x);
+        return x;
+    }
+
+    template<typename T, std::size_t Sections>
+    void BiquadCascade<T, Sections>::Reset() noexcept
+    {
+        for (auto& stage : stages)
+            stage.Reset();
+    }
+
+#ifdef NUMERICAL_TOOLBOX_COVERAGE_BUILD
+    extern template struct BiquadCoeffs<float>;
+    extern template class Biquad<float>;
+    extern template class BiquadCascade<float, 2>;
+#endif
+}