polesUpper = new ArrayList<>();
+ for (JacobiElliptic.JacobiResult jr : jacobi) {
+ double s = jr.sn;
+ double c = jr.cn;
+ double d = jr.dn;
+ Complex num = new Complex(c * d * sv.sn * sv.cn, s * sv.dn);
+ double denom = 1.0 - (d * sv.sn) * (d * sv.sn);
+ polesUpper.add(num.div(denom).negate());
+ }
+
+ if (order % 2 != 0) {
+ // Odd order: one pole is purely real (from j=0). Append complex
+ // conjugates for the others; leave the real one alone.
+ for (Complex p : polesUpper) {
+ out.poles.add(p);
+ }
+ for (Complex p : polesUpper) {
+ if (Math.abs(p.imag()) > COEF_EPS) {
+ out.poles.add(p.conj());
+ }
+ }
+ } else {
+ for (Complex p : polesUpper) {
+ out.poles.add(p);
+ }
+ for (Complex p : polesUpper) {
+ out.poles.add(p.conj());
+ }
+ }
+
+ // Gain: real(prod(-p) / prod(-z)). Even-order trims by sqrt(1+eps²) so DC
+ // sits at the ripple trough, matching scipy's convention.
+ Complex prodNegP = Complex.ONE;
+ for (Complex p : out.poles) {
+ prodNegP = prodNegP.mul(p.negate());
+ }
+ Complex prodNegZ = Complex.ONE;
+ for (Complex z : out.zeros) {
+ prodNegZ = prodNegZ.mul(z.negate());
+ }
+ double k = prodNegP.div(prodNegZ).real();
+ if (order % 2 == 0) {
+ k /= Math.sqrt(1.0 + epsSq);
+ }
+ out.gain = k;
+ return out;
+ }
+}
diff --git a/wpimath/src/main/java/org/wpilib/math/filter/internal/BilinearTransform.java b/wpimath/src/main/java/org/wpilib/math/filter/internal/BilinearTransform.java
new file mode 100644
index 0000000000..6b8543852a
--- /dev/null
+++ b/wpimath/src/main/java/org/wpilib/math/filter/internal/BilinearTransform.java
@@ -0,0 +1,127 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter.internal;
+
+import java.util.List;
+import org.wpilib.math.filter.BiquadFilter;
+
+/**
+ * Bilinear transform plus the kind-specific dispatch shared by every classical IIR factory.
+ *
+ * Standard analog→digital conversion: pre-warp the digital cutoff so the post-bilinear digital
+ * response hits the requested edge exactly, then map each analog pole p (resp. zero z) via s →
+ * 2·fs·(z-1)/(z+1), giving p_d = (2fs + p) / (2fs - p).
+ *
+ *
Background:
+ *
+ *
+ * - https://en.wikipedia.org/wiki/Bilinear_transform
+ *
- Oppenheim & Schafer, "Discrete-Time Signal Processing" §7.2.2
+ *
+ *
+ * SciPy implementations to compare against, line for line, in
+ * https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py — functions {@code
+ * bilinear_zpk} and {@code _zpkbilinear}; constant prewarping is folded into the {@code
+ * lp2{lp,hp,bp,bs}_zpk} callers above the bilinear step.
+ */
+final class BilinearTransform {
+ private BilinearTransform() {}
+
+ /**
+ * Pre-warp a digital cutoff frequency (Hz) for use as the analog-domain cutoff that, after the
+ * bilinear transform at the same {@code fs}, maps back to exactly that digital cutoff.
+ */
+ static double preWarp(double fc, double fs) {
+ return 2.0 * fs * Math.tan(Math.PI * fc / fs);
+ }
+
+ /**
+ * Bilinear transform of an analog ZPK to a digital ZPK at sample rate {@code fs}. Analog zeros at
+ * infinity map to digital zeros at z = -1 (Nyquist).
+ */
+ static Zpk bilinearTransform(Zpk analog, double fs) {
+ // Substituting s = 2fs(z-1)/(z+1) into H(s) and solving for the image of
+ // each finite root gives p_d = (2fs + p)/(2fs - p) (and the same form for
+ // zeros). The substitution also rescales the leading polynomial coefficient
+ // by Π(2fs - z) / Π(2fs - p) over the analog roots — that's the gain
+ // adjustment at the bottom.
+ Zpk out = new Zpk();
+ double fs2 = 2.0 * fs;
+ Complex zNumProd = Complex.ONE;
+ Complex zDenProd = Complex.ONE;
+ for (Complex z : analog.zeros) {
+ Complex denom = new Complex(fs2, 0).sub(z);
+ out.zeros.add(new Complex(fs2, 0).add(z).div(denom));
+ zNumProd = zNumProd.mul(denom);
+ }
+ for (Complex p : analog.poles) {
+ Complex denom = new Complex(fs2, 0).sub(p);
+ out.poles.add(new Complex(fs2, 0).add(p).div(denom));
+ zDenProd = zDenProd.mul(denom);
+ }
+ // Analog filters with fewer zeros than poles have `degree` zeros at s=∞.
+ // The bilinear maps s=∞ to z=-1 (Nyquist), so materialize them here. This
+ // is what gives a Butterworth low-pass its N digital zeros at Nyquist and
+ // hence its hard rolloff at the top of the band.
+ int degree = Zpk.relativeDegree(analog);
+ for (int i = 0; i < degree; i++) {
+ out.zeros.add(new Complex(-1.0, 0.0));
+ }
+ out.gain = analog.gain * zNumProd.div(zDenProd).real();
+ return out;
+ }
+
+ /**
+ * Apply the kind-specific frequency transform (LP/HP/BP/BS) to an analog LP prototype, run the
+ * bilinear transform at {@code fs}, and convert to a SOS cascade. Shared by every classical IIR
+ * design factory (Butterworth, Chebyshev I/II, Elliptic).
+ *
+ *
Caller is responsible for validating inputs (positive fs, f1 in (0, fs/2), and for BP/BS, f1
+ * < f2 < fs/2). This helper does no validation itself.
+ */
+ static List designFromAnalogLp(
+ Zpk analogLp, BiquadFilter.Kind kind, double fs, double f1, double f2) {
+ // Pipeline:
+ // 1. Pre-warp the requested digital cutoff(s) into the analog cutoff
+ // that maps back to them under the bilinear transform.
+ // 2. Reshape the 1 rad/s LP prototype with a kind-specific s-plane
+ // substitution (LP→LP/HP/BP/BS), giving an analog filter at the
+ // requested kind and cutoff.
+ // 3. Bilinear-transform the resulting analog ZPK to a digital ZPK
+ // (s-plane → z-plane).
+ // 4. Pair conjugate digital roots into a cascade of real-coefficient
+ // biquad sections.
+ Zpk analog = analogLp;
+ switch (kind) {
+ case LowPass:
+ analog = Zpk.analogLpToLp(analog, preWarp(f1, fs));
+ break;
+ case HighPass:
+ analog = Zpk.analogLpToHp(analog, preWarp(f1, fs));
+ break;
+ case BandPass:
+ {
+ double w1 = preWarp(f1, fs);
+ double w2 = preWarp(f2, fs);
+ double wo = Math.sqrt(w1 * w2);
+ double bw = w2 - w1;
+ analog = Zpk.analogLpToBp(analog, wo, bw);
+ break;
+ }
+ case BandStop:
+ {
+ double w1 = preWarp(f1, fs);
+ double w2 = preWarp(f2, fs);
+ double wo = Math.sqrt(w1 * w2);
+ double bw = w2 - w1;
+ analog = Zpk.analogLpToBs(analog, wo, bw);
+ break;
+ }
+ default:
+ throw new IllegalArgumentException("Unknown BiquadFilter.Kind: " + kind);
+ }
+ return Zpk.zpkToSos(bilinearTransform(analog, fs));
+ }
+}
diff --git a/wpimath/src/main/java/org/wpilib/math/filter/internal/BiquadFilterDesigner.java b/wpimath/src/main/java/org/wpilib/math/filter/internal/BiquadFilterDesigner.java
new file mode 100644
index 0000000000..dd3ca74dab
--- /dev/null
+++ b/wpimath/src/main/java/org/wpilib/math/filter/internal/BiquadFilterDesigner.java
@@ -0,0 +1,405 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter.internal;
+
+import java.util.ArrayList;
+import java.util.List;
+import org.wpilib.math.filter.BiquadFilter;
+import org.wpilib.math.filter.BiquadFilter.Kind;
+import org.wpilib.math.filter.BiquadFilter.Section;
+
+/**
+ * Implementation entrypoint for {@link BiquadFilter}'s static design factories. Lives in this
+ * sub-package to keep the design machinery (complex arithmetic, Jacobi elliptic functions, ZPK→SOS
+ * conversion) out of the public {@code org.wpilib.math.filter} surface.
+ *
+ * Robot code should call the factories on {@code BiquadFilter} directly — those handle argument
+ * validation and wrap the section list in a ready-to-run filter. Calling into this class is
+ * supported for niche cases (e.g. inspecting the section list before constructing a filter) but is
+ * not part of the documented public API.
+ *
+ *
Each classical-IIR factory (Butterworth, Chebyshev I/II, Elliptic) drives the same three-step
+ * pipeline that {@code scipy.signal.iirfilter} does:
+ *
+ *
+ * - {@link AnalogPrototypes} — analog LP prototype, cutoff 1 rad/s
+ *
- {@link BilinearTransform#designFromAnalogLp}: kind-specific frequency transform via {@link
+ * Zpk}, bilinear analog→digital at sample rate fs, then ZPK→SOS biquad pairing
+ *
+ *
+ * SciPy reference for the whole pipeline:
+ * https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py (functions {@code
+ * iirfilter}, {@code butter}, {@code cheby1}, {@code cheby2}, {@code ellip}).
+ *
+ *
{@code notch} and {@code movingAverage} are closed-form and do not go through the
+ * prototype/bilinear path. They are documented inline below.
+ */
+public final class BiquadFilterDesigner {
+ private BiquadFilterDesigner() {}
+
+ private static void validateClassicalArgs(
+ Kind kind, int order, double sampleRate, double lowCutoff, double highCutoff) {
+ if (order < 1) {
+ throw new IllegalArgumentException("BiquadFilter design: order must be >= 1.");
+ }
+ if (sampleRate <= 0.0) {
+ throw new IllegalArgumentException("BiquadFilter design: sample rate must be positive.");
+ }
+ final double nyquist = 0.5 * sampleRate;
+ if (lowCutoff <= 0.0 || lowCutoff >= nyquist) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: cutoff must lie in (0, sampleRate/2).");
+ }
+ if ((kind == Kind.BandPass || kind == Kind.BandStop)
+ && (highCutoff <= lowCutoff || highCutoff >= nyquist)) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: BandPass/BandStop requires "
+ + "lowCutoff < highCutoff < sampleRate/2.");
+ }
+ }
+
+ private static void rejectBandKindForLpHpOverload(String factoryName, Kind kind) {
+ if (kind == Kind.BandPass || kind == Kind.BandStop) {
+ throw new IllegalArgumentException(
+ "BiquadFilter."
+ + factoryName
+ + ": BandPass/BandStop requires the overload that takes both "
+ + "lowCutoff and highCutoff.");
+ }
+ }
+
+ private static void rejectLpHpKindForBandOverload(String factoryName, Kind kind) {
+ if (kind == Kind.LowPass || kind == Kind.HighPass) {
+ throw new IllegalArgumentException(
+ "BiquadFilter."
+ + factoryName
+ + ": LowPass/HighPass requires the single-cutoff overload.");
+ }
+ }
+
+ /**
+ * Designs a Butterworth IIR band-pass or band-stop filter.
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param lowCutoff Low edge of the band (Hz). Must satisfy 0 < lowCutoff < highCutoff <
+ * sampleRate/2.
+ * @param highCutoff High edge of the band (Hz).
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#butterworth(Kind, int, double, double, double)
+ */
+ public static Section[] butterworth(
+ Kind kind, int order, double sampleRate, double lowCutoff, double highCutoff) {
+ rejectLpHpKindForBandOverload("butterworth", kind);
+ validateClassicalArgs(kind, order, sampleRate, lowCutoff, highCutoff);
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.butterworthPrototype(order), kind, sampleRate, lowCutoff, highCutoff));
+ }
+
+ /**
+ * Designs a Butterworth IIR low-pass or high-pass filter (single cutoff).
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param cutoff Cutoff frequency (Hz). Must satisfy 0 < cutoff < sampleRate/2.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#butterworth(Kind, int, double, double)
+ */
+ public static Section[] butterworth(Kind kind, int order, double sampleRate, double cutoff) {
+ rejectBandKindForLpHpOverload("butterworth", kind);
+ validateClassicalArgs(kind, order, sampleRate, cutoff, 0.0);
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.butterworthPrototype(order), kind, sampleRate, cutoff, 0.0));
+ }
+
+ /**
+ * Designs a Chebyshev Type I IIR band-pass or band-stop filter.
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1). The cascade has 2*order poles.
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param lowCutoff Low edge of the band (Hz). Must satisfy 0 < lowCutoff < highCutoff <
+ * sampleRate/2.
+ * @param highCutoff High edge of the band (Hz).
+ * @param rippleDb Peak-to-peak passband ripple in dB. Must be > 0.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#chebyshevI(Kind, int, double, double, double, double)
+ */
+ public static Section[] chebyshevI(
+ Kind kind,
+ int order,
+ double sampleRate,
+ double lowCutoff,
+ double highCutoff,
+ double rippleDb) {
+ rejectLpHpKindForBandOverload("chebyshevI", kind);
+ validateClassicalArgs(kind, order, sampleRate, lowCutoff, highCutoff);
+ if (rippleDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: ChebyshevI passband ripple must be positive.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.chebyshevIPrototype(order, rippleDb),
+ kind,
+ sampleRate,
+ lowCutoff,
+ highCutoff));
+ }
+
+ /**
+ * Designs a Chebyshev Type I IIR low-pass or high-pass filter (single cutoff).
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param cutoff Cutoff frequency (Hz). Must satisfy 0 < cutoff < sampleRate/2.
+ * @param rippleDb Peak-to-peak passband ripple in dB. Must be > 0.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#chebyshevI(Kind, int, double, double, double)
+ */
+ public static Section[] chebyshevI(
+ Kind kind, int order, double sampleRate, double cutoff, double rippleDb) {
+ rejectBandKindForLpHpOverload("chebyshevI", kind);
+ validateClassicalArgs(kind, order, sampleRate, cutoff, 0.0);
+ if (rippleDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: ChebyshevI passband ripple must be positive.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.chebyshevIPrototype(order, rippleDb), kind, sampleRate, cutoff, 0.0));
+ }
+
+ /**
+ * Designs a Chebyshev Type II IIR band-pass or band-stop filter.
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1). The cascade has 2*order poles.
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param lowCutoff Low edge of the stop band (Hz). Must satisfy 0 < lowCutoff < highCutoff
+ * < sampleRate/2.
+ * @param highCutoff High edge of the stop band (Hz).
+ * @param stopAttenDb Stopband attenuation in dB. Must be > 0.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#chebyshevII(Kind, int, double, double, double, double)
+ */
+ public static Section[] chebyshevII(
+ Kind kind,
+ int order,
+ double sampleRate,
+ double lowCutoff,
+ double highCutoff,
+ double stopAttenDb) {
+ rejectLpHpKindForBandOverload("chebyshevII", kind);
+ validateClassicalArgs(kind, order, sampleRate, lowCutoff, highCutoff);
+ if (stopAttenDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: ChebyshevII stopband attenuation must be positive.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.chebyshevIIPrototype(order, stopAttenDb),
+ kind,
+ sampleRate,
+ lowCutoff,
+ highCutoff));
+ }
+
+ /**
+ * Designs a Chebyshev Type II IIR low-pass or high-pass filter (single cutoff).
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param cutoff Stopband-edge frequency (Hz). Must satisfy 0 < cutoff < sampleRate/2.
+ * @param stopAttenDb Stopband attenuation in dB. Must be > 0.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#chebyshevII(Kind, int, double, double, double)
+ */
+ public static Section[] chebyshevII(
+ Kind kind, int order, double sampleRate, double cutoff, double stopAttenDb) {
+ rejectBandKindForLpHpOverload("chebyshevII", kind);
+ validateClassicalArgs(kind, order, sampleRate, cutoff, 0.0);
+ if (stopAttenDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: ChebyshevII stopband attenuation must be positive.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.chebyshevIIPrototype(order, stopAttenDb),
+ kind,
+ sampleRate,
+ cutoff,
+ 0.0));
+ }
+
+ /**
+ * Designs an elliptic IIR band-pass or band-stop filter.
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Filter order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param lowCutoff Low edge of the band (Hz). Must satisfy 0 < lowCutoff < highCutoff <
+ * sampleRate/2.
+ * @param highCutoff High edge of the band (Hz).
+ * @param rippleDb Passband ripple in dB (> 0).
+ * @param stopAttenDb Stopband attenuation in dB (must exceed {@code rippleDb}).
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#elliptic(Kind, int, double, double, double, double, double)
+ */
+ public static Section[] elliptic(
+ Kind kind,
+ int order,
+ double sampleRate,
+ double lowCutoff,
+ double highCutoff,
+ double rippleDb,
+ double stopAttenDb) {
+ rejectLpHpKindForBandOverload("elliptic", kind);
+ validateClassicalArgs(kind, order, sampleRate, lowCutoff, highCutoff);
+ if (rippleDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: Elliptic passband ripple must be positive.");
+ }
+ if (stopAttenDb <= rippleDb) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: Elliptic stopband attenuation must exceed passband ripple.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.ellipticPrototype(order, rippleDb, stopAttenDb),
+ kind,
+ sampleRate,
+ lowCutoff,
+ highCutoff));
+ }
+
+ /**
+ * Designs an elliptic IIR low-pass or high-pass filter (single cutoff).
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Filter order (>= 1).
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param cutoff Cutoff frequency (Hz). Must satisfy 0 < cutoff < sampleRate/2.
+ * @param rippleDb Passband ripple in dB (> 0).
+ * @param stopAttenDb Stopband attenuation in dB (must exceed {@code rippleDb}).
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#elliptic(Kind, int, double, double, double, double)
+ */
+ public static Section[] elliptic(
+ Kind kind, int order, double sampleRate, double cutoff, double rippleDb, double stopAttenDb) {
+ rejectBandKindForLpHpOverload("elliptic", kind);
+ validateClassicalArgs(kind, order, sampleRate, cutoff, 0.0);
+ if (rippleDb <= 0.0) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: Elliptic passband ripple must be positive.");
+ }
+ if (stopAttenDb <= rippleDb) {
+ throw new IllegalArgumentException(
+ "BiquadFilter design: Elliptic stopband attenuation must exceed passband ripple.");
+ }
+ return toArray(
+ BilinearTransform.designFromAnalogLp(
+ AnalogPrototypes.ellipticPrototype(order, rippleDb, stopAttenDb),
+ kind,
+ sampleRate,
+ cutoff,
+ 0.0));
+ }
+
+ /**
+ * Designs a notch (band-stop) IIR filter.
+ *
+ * @param sampleRate Sample rate (Hz). Must be positive.
+ * @param centerFrequency Notch center frequency (Hz). Must satisfy 0 < centerFrequency <
+ * sampleRate/2.
+ * @param qualityFactor Quality factor (Q). Higher values give a narrower notch. Must be positive.
+ * @return A single-section cascade implementing the notch.
+ * @see BiquadFilter#notch(double, double, double)
+ */
+ public static Section[] notch(double sampleRate, double centerFrequency, double qualityFactor) {
+ if (sampleRate <= 0.0) {
+ throw new IllegalArgumentException("BiquadFilter.notch: sample rate must be positive.");
+ }
+ if (qualityFactor <= 0.0) {
+ throw new IllegalArgumentException("BiquadFilter.notch: quality factor must be positive.");
+ }
+ final double nyquist = 0.5 * sampleRate;
+ if (centerFrequency <= 0.0 || centerFrequency >= nyquist) {
+ throw new IllegalArgumentException(
+ "BiquadFilter.notch: center frequency must lie in (0, sampleRate/2).");
+ }
+
+ // Standard second-order IIR notch (zero pair on the unit circle at ±w0,
+ // pole pair just inside on the same radial line). Matches
+ // scipy.signal.iirnotch(f0, Q, fs) exactly:
+ // w0 = 2π·f0/fs, bw = w0/Q, β = tan(bw/2), g = 1/(1 + β)
+ // b = g · [1, -2cos(w0), 1], a = [1, -2g·cos(w0), 2g - 1]
+ // SciPy reference (function iirnotch):
+ // https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ // Background: Sophocles Orfanidis, "Introduction to Signal Processing"
+ // §11.3.2 ("Parametric resonators and notch filters").
+ final double w0 = 2.0 * Math.PI * centerFrequency / sampleRate;
+ final double bw = w0 / qualityFactor;
+ final double beta = Math.tan(0.5 * bw);
+ final double gain = 1.0 / (1.0 + beta);
+ final double cosW0 = Math.cos(w0);
+ return new Section[] {
+ new Section(gain, -2.0 * gain * cosW0, gain, -2.0 * gain * cosW0, 2.0 * gain - 1.0)
+ };
+ }
+
+ /**
+ * Designs a moving-average FIR filter expressed as cascaded biquad sections.
+ *
+ * @param taps Length of the moving-average window. Must be >= 1.
+ * @return The cascade of biquad sections implementing the filter.
+ * @see BiquadFilter#movingAverage(int)
+ */
+ public static Section[] movingAverage(int taps) {
+ if (taps < 1) {
+ throw new IllegalArgumentException("BiquadFilter.movingAverage: taps must be >= 1.");
+ }
+ // A length-N moving average has H(z) = (1/N)·(1 - z⁻ᴺ)/(1 - z⁻¹), whose
+ // non-trivial zeros are the Nth roots of unity except z = 1:
+ // z_k = exp(i·2π·k/N), k = 1..N-1
+ // Pair each (z_k, z_{N-k}) into one all-zero biquad
+ // (b0, b1, b2, a1, a2) = (1, -2·cos(2πk/N), 1, 0, 0)
+ // and, if N is even, emit a single-zero first-order biquad for the unpaired
+ // root at z = -1:
+ // (1, 1, 0, 0, 0)
+ // The overall 1/N gain is folded into the first section.
+ //
+ // The factorization of (1 - z⁻ᴺ) into roots of unity is textbook:
+ // https://en.wikipedia.org/wiki/Root_of_unity#Polynomial_form
+ // Equivalent to scipy.signal.tf2sos applied to b = [1/N, ..., 1/N], a = [1].
+ if (taps == 1) {
+ return new Section[] {new Section(1.0, 0.0, 0.0, 0.0, 0.0)};
+ }
+ final double N = taps;
+ final int pairs = (taps - 1) / 2;
+ List out = new ArrayList<>();
+ for (int k = 1; k <= pairs; k++) {
+ double c = Math.cos(2.0 * Math.PI * k / N);
+ out.add(new Section(1.0, -2.0 * c, 1.0, 0.0, 0.0));
+ }
+ if (taps % 2 == 0) {
+ out.add(new Section(1.0, 1.0, 0.0, 0.0, 0.0));
+ }
+ final double g = 1.0 / N;
+ Section first = out.get(0);
+ out.set(0, new Section(first.b0 * g, first.b1 * g, first.b2 * g, first.a1, first.a2));
+ return out.toArray(new Section[0]);
+ }
+
+ private static Section[] toArray(List list) {
+ return list.toArray(new Section[0]);
+ }
+}
diff --git a/wpimath/src/main/java/org/wpilib/math/filter/internal/Complex.java b/wpimath/src/main/java/org/wpilib/math/filter/internal/Complex.java
new file mode 100644
index 0000000000..0fa4e0cdd8
--- /dev/null
+++ b/wpimath/src/main/java/org/wpilib/math/filter/internal/Complex.java
@@ -0,0 +1,123 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter.internal;
+
+/**
+ * Minimal immutable complex number, package-private to support the BiquadFilter design code.
+ * Operations are named to match the C++ {@code std::complex} surface used by the filter
+ * design helpers, not Java's {@code BigDecimal}-style arithmetic.
+ */
+final class Complex {
+ static final Complex ONE = new Complex(1.0, 0.0);
+
+ final double re;
+ final double im;
+
+ Complex(double re, double im) {
+ this.re = re;
+ this.im = im;
+ }
+
+ /** Construct from polar form: r·(cos(θ) + i·sin(θ)). */
+ static Complex polar(double r, double theta) {
+ return new Complex(r * Math.cos(theta), r * Math.sin(theta));
+ }
+
+ double real() {
+ return re;
+ }
+
+ double imag() {
+ return im;
+ }
+
+ /** Magnitude |z| = sqrt(x² + y²). */
+ double abs() {
+ return Math.hypot(re, im);
+ }
+
+ /** Squared magnitude |z|² = x² + y² (matches std::norm). */
+ double normSq() {
+ return re * re + im * im;
+ }
+
+ Complex conj() {
+ return new Complex(re, -im);
+ }
+
+ Complex negate() {
+ return new Complex(-re, -im);
+ }
+
+ Complex add(Complex other) {
+ return new Complex(re + other.re, im + other.im);
+ }
+
+ Complex add(double s) {
+ return new Complex(re + s, im);
+ }
+
+ Complex sub(Complex other) {
+ return new Complex(re - other.re, im - other.im);
+ }
+
+ Complex sub(double s) {
+ return new Complex(re - s, im);
+ }
+
+ Complex mul(Complex other) {
+ return new Complex(re * other.re - im * other.im, re * other.im + im * other.re);
+ }
+
+ Complex mul(double s) {
+ return new Complex(re * s, im * s);
+ }
+
+ Complex div(Complex other) {
+ double d = other.normSq();
+ return new Complex((re * other.re + im * other.im) / d, (im * other.re - re * other.im) / d);
+ }
+
+ Complex div(double s) {
+ return new Complex(re / s, im / s);
+ }
+
+ /** Sinh on the principal branch: sinh(x+iy) = sinh(x)·cos(y) + i·cosh(x)·sin(y). */
+ Complex sinh() {
+ return new Complex(Math.sinh(re) * Math.cos(im), Math.cosh(re) * Math.sin(im));
+ }
+
+ /**
+ * Principal-branch square root. Matches std::sqrt for std::complex — the result has
+ * non-negative real part, and im ≥ 0 when input has im ≥ 0.
+ */
+ Complex sqrt() {
+ if (re == 0.0 && im == 0.0) {
+ return new Complex(0.0, 0.0);
+ }
+ double r = abs();
+ double sgn = im >= 0.0 ? 1.0 : -1.0;
+ return new Complex(Math.sqrt(0.5 * (r + re)), sgn * Math.sqrt(0.5 * (r - re)));
+ }
+
+ /** Principal-branch complex log: log(z) = ln|z| + i·arg(z). */
+ Complex log() {
+ return new Complex(Math.log(abs()), Math.atan2(im, re));
+ }
+
+ /** Principal-branch arcsin via -i·log(i·z + sqrt(1 - z²)). */
+ Complex asin() {
+ Complex inside = new Complex(-im, re).add(ONE.sub(this.mul(this)).sqrt());
+ Complex l = inside.log();
+ return new Complex(l.im, -l.re);
+ }
+
+ /** Principal-branch atanh via 0.5·log((1+z)/(1-z)). */
+ Complex atanh() {
+ Complex num = new Complex(1.0 + re, im);
+ Complex den = new Complex(1.0 - re, -im);
+ return num.div(den).log().mul(0.5);
+ }
+}
diff --git a/wpimath/src/main/java/org/wpilib/math/filter/internal/JacobiElliptic.java b/wpimath/src/main/java/org/wpilib/math/filter/internal/JacobiElliptic.java
new file mode 100644
index 0000000000..d99d1091db
--- /dev/null
+++ b/wpimath/src/main/java/org/wpilib/math/filter/internal/JacobiElliptic.java
@@ -0,0 +1,232 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter.internal;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Elliptic-integral / Jacobi elliptic function helpers used by the elliptic filter prototype.
+ *
+ * Ports the C++ {@code wpi::math::filter::internal} versions; algorithms and tolerances match
+ * Numerical Recipes / SciPy exactly so the generated coefficients agree with {@code
+ * scipy.signal.ellip} to within ~1e-10 (LP) for the same inputs.
+ *
+ *
All four routines below ({@code ellipticK}, {@code ellipj}, {@code inverseJacobiSn}, {@code
+ * ellipticDegree}) follow the derivations and equation numbers in:
+ *
+ *
Orfanidis, "Introduction to Signal Processing Second Edition (2023)",
+ * https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
+ *
+ *
Specific equations cited inline below (e.g. "Eq. (49)", "Eq. (56)") refer to that PDF. SciPy
+ * algorithm parity is also maintained — {@code scipy.special.ellipk} / {@code scipy.special.ellipj}
+ * / private {@code _arc_jac_sn} / {@code _arc_jac_sc1} / {@code _ellipdeg} drive {@code
+ * scipy.signal.ellipap} (https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py).
+ * Numerical Recipes 3rd ed. §6.12 ("Elliptic Integrals and Jacobian Elliptic Functions") covers the
+ * AGM and Landen iterations used here.
+ */
+final class JacobiElliptic {
+ private JacobiElliptic() {}
+
+ private static final int MAX_ITER = 60;
+
+ /** Jacobi elliptic functions evaluated at a single point. */
+ static final class JacobiResult {
+ final double sn;
+ final double cn;
+ final double dn;
+
+ JacobiResult(double sn, double cn, double dn) {
+ this.sn = sn;
+ this.cn = cn;
+ this.dn = dn;
+ }
+ }
+
+ // sqrt(1 - k^2) computed as sqrt((1-k)(1+k)) — preserves precision when k is
+ // small. Real-valued path used by ellipj/inverseJacobiSn.
+ private static double complement(double k) {
+ return Math.sqrt((1.0 - k) * (1.0 + k));
+ }
+
+ private static Complex complement(Complex k) {
+ Complex one = Complex.ONE;
+ return one.sub(k).mul(one.add(k)).sqrt();
+ }
+
+ /**
+ * Complete elliptic integral of the first kind, K(m), via the arithmetic-geometric mean
+ * iteration.
+ *
+ * @param m Parameter (m = k², where k is the modulus). Domain: [0, 1]. m=0 returns π/2; m=1
+ * returns +∞.
+ */
+ static double ellipticK(double m) {
+ if (m < 0.0 || m > 1.0) {
+ return Double.NaN;
+ }
+ if (m == 1.0) {
+ return Double.POSITIVE_INFINITY;
+ }
+ // AGM: K(m) = π / (2 · AGM(1, sqrt(1-m))).
+ double a = 1.0;
+ double b = Math.sqrt(1.0 - m);
+ for (int i = 0; i < MAX_ITER; i++) {
+ if (Math.abs(a - b) <= Math.ulp(1.0) * a) {
+ break;
+ }
+ double aNext = 0.5 * (a + b);
+ double bNext = Math.sqrt(a * b);
+ a = aNext;
+ b = bNext;
+ }
+ return Math.PI / (2.0 * a);
+ }
+
+ /**
+ * Jacobi elliptic functions sn(u, m), cn(u, m), dn(u, m) for real u and parameter m ∈ [0, 1].
+ * Computed via the descending Landen transformation followed by ascending recovery — the same
+ * scheme used by Numerical Recipes and (under the hood) SciPy's special.ellipj.
+ */
+ static JacobiResult ellipj(double u, double m) {
+ if (m == 0.0) {
+ return new JacobiResult(Math.sin(u), Math.cos(u), 1.0);
+ }
+ if (m == 1.0) {
+ double t = Math.tanh(u);
+ double sech = 1.0 / Math.cosh(u);
+ return new JacobiResult(t, sech, sech);
+ }
+
+ // Ascending Landen: store a_n, c_n at each level until c_n is negligible.
+ List aSeq = new ArrayList<>();
+ List cSeq = new ArrayList<>();
+ aSeq.add(1.0);
+ double b = Math.sqrt(1.0 - m);
+ cSeq.add(Math.sqrt(m));
+
+ int n = 0;
+ while (n < MAX_ITER) {
+ if (Math.abs(cSeq.get(cSeq.size() - 1))
+ <= Math.ulp(1.0) * Math.abs(aSeq.get(aSeq.size() - 1))) {
+ break;
+ }
+ double aN = aSeq.get(aSeq.size() - 1);
+ double bN = b;
+ aSeq.add(0.5 * (aN + bN));
+ b = Math.sqrt(aN * bN);
+ cSeq.add(0.5 * (aN - bN));
+ n++;
+ }
+
+ // Descend: phi_n = u · 2^n · a_n, then unwind.
+ double phi = u * Math.scalb(aSeq.get(aSeq.size() - 1), n);
+ for (int j = n; j >= 1; j--) {
+ phi = 0.5 * (phi + Math.asin((cSeq.get(j) / aSeq.get(j)) * Math.sin(phi)));
+ }
+ double sn = Math.sin(phi);
+ double cn = Math.cos(phi);
+ // dn = sqrt(1 - m·sn²) — branch chosen so dn ≥ 0 in the principal interval,
+ // which matches scipy's convention.
+ double dn = Math.sqrt(1.0 - m * sn * sn);
+ return new JacobiResult(sn, cn, dn);
+ }
+
+ /**
+ * Inverse Jacobi sn: solves sn(z, m) = w for z, where w may be complex. Used by the elliptic
+ * filter design to compute v0.
+ *
+ * Implements the descending-Landen iteration from Orfanidis, "Lecture Notes on Elliptic Filter
+ * Design", Eq. (56).
+ */
+ static Complex inverseJacobiSn(Complex w, double m) {
+ // Descending Landen on the modulus: build a sequence of decreasing moduli
+ // until the smallest is effectively zero, then invert via arcsin and lift.
+ double k = Math.sqrt(m);
+ if (k > 1.0) {
+ return new Complex(Double.NaN, Double.NaN);
+ }
+ if (k == 1.0) {
+ // sn(z, 1) = tanh(z), so the inverse is atanh(w).
+ return w.atanh();
+ }
+
+ List ks = new ArrayList<>();
+ ks.add(k);
+ for (int i = 0; i < MAX_ITER; i++) {
+ double last = ks.get(ks.size() - 1);
+ if (last == 0.0) {
+ break;
+ }
+ double kp = complement(last);
+ double next = (1.0 - kp) / (1.0 + kp);
+ ks.add(next);
+ }
+
+ // Capital K of the original modulus equals (π/2) · ∏(1 + k_i) for i ≥ 1.
+ double K = 1.0;
+ for (int i = 1; i < ks.size(); i++) {
+ K *= 1.0 + ks.get(i);
+ }
+ K *= Math.PI / 2.0;
+
+ List wns = new ArrayList<>();
+ wns.add(w);
+ for (int i = 0; i + 1 < ks.size(); i++) {
+ Complex wn = wns.get(wns.size() - 1);
+ Complex denom =
+ new Complex(1.0 + ks.get(i + 1), 0).mul(Complex.ONE.add(complement(wn.mul(ks.get(i)))));
+ Complex wnext = wn.mul(2.0).div(denom);
+ wns.add(wnext);
+ }
+
+ Complex u = wns.get(wns.size() - 1).asin().mul(2.0 / Math.PI);
+ return u.mul(K);
+ }
+
+ /**
+ * Real inverse Jacobi sc with complementary modulus: solves sc(z, 1-m) = w for real z. Equivalent
+ * to scipy's {@code _arc_jac_sc1(w, m)}.
+ */
+ static double inverseJacobiSc1(double w, double m) {
+ // sc(z, 1-m) = -j · sn(j·z, m), so sc(z, 1-m) = w → sn(j·z, m) = j·w →
+ // j·z = arcsn(j·w, m). The result is purely imaginary; return its imag part.
+ Complex z = inverseJacobiSn(new Complex(0.0, w), m);
+ return z.imag();
+ }
+
+ /**
+ * Solves the elliptic degree equation.
+ *
+ *
+ * N · K(m) / K(1-m) = K(m1) / K(1-m1)
+ *
+ *
+ * For m given the order N and the small modulus parameter m1. Uses the q-nome series of
+ * Orfanidis Eq. (49).
+ */
+ static double ellipticDegree(int N, double m1) {
+ // Solve N · K(m)/K'(m) = K(m1)/K'(m1) for m using the q-nome series:
+ // q1 = exp(-π · K'(m1) / K(m1)), q = q1^(1/N),
+ // m = 16q · (Σ q^{i(i+1)})⁴ / (1 + 2 Σ q^{i²})⁴
+ final int MMAX = 7;
+ double K1 = ellipticK(m1);
+ double K1p = ellipticK(1.0 - m1);
+ double q1 = Math.exp(-Math.PI * K1p / K1);
+ double q = Math.pow(q1, 1.0 / N);
+
+ double num = 0.0;
+ for (int i = 0; i <= MMAX; i++) {
+ num += Math.pow(q, (double) i * (i + 1));
+ }
+ double den = 1.0;
+ for (int i = 1; i <= MMAX + 1; i++) {
+ den += 2.0 * Math.pow(q, (double) i * i);
+ }
+
+ double ratio = num / den;
+ return 16.0 * q * ratio * ratio * ratio * ratio;
+ }
+}
diff --git a/wpimath/src/main/java/org/wpilib/math/filter/internal/Zpk.java b/wpimath/src/main/java/org/wpilib/math/filter/internal/Zpk.java
new file mode 100644
index 0000000000..b3da4eab01
--- /dev/null
+++ b/wpimath/src/main/java/org/wpilib/math/filter/internal/Zpk.java
@@ -0,0 +1,328 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter.internal;
+
+import java.util.ArrayList;
+import java.util.Comparator;
+import java.util.List;
+import org.wpilib.math.filter.BiquadFilter;
+
+/**
+ * Zeros/poles/gain representation of a rational transfer function.
+ *
+ *
+ * H(s) = gain · ∏(s - z_i) / ∏(s - p_j) (analog)
+ * H(z) = gain · ∏(z - z_i) / ∏(z - p_j) (digital)
+ *
+ *
+ * Complex roots must appear in conjugate pairs; that invariant is preserved by every transform
+ * below.
+ *
+ *
The four {@code analogLpTo*} helpers are the standard frequency-domain spectral
+ * transformations (Oppenheim & Schafer, "Discrete-Time Signal Processing" §7.1.5;
+ * Constantinides, "Spectral transformations for digital filters", IEE Proc. 117 (1970) 1585–1590).
+ * They each correspond to a SciPy helper:
+ *
+ *
+ * - {@code analogLpToLp} ↔ {@code scipy.signal.lp2lp_zpk}
+ *
- {@code analogLpToHp} ↔ {@code scipy.signal.lp2hp_zpk}
+ *
- {@code analogLpToBp} ↔ {@code scipy.signal.lp2bp_zpk}
+ *
- {@code analogLpToBs} ↔ {@code scipy.signal.lp2bs_zpk}
+ *
+ *
+ * Source for all four: https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ *
+ *
{@code zpkToSos} pairs conjugate roots into biquad sections using the same "nearest pole/zero"
+ * pairing that {@code scipy.signal.zpk2sos} uses by default (helper {@code _cplxreal}). We diverge
+ * from scipy in only one place: section ordering. SciPy can return a "minimum phase" ordering; we
+ * always sort by ascending |pole| (least aggressive first). The cascade product is identical; only
+ * the per-section numerical conditioning differs.
+ */
+final class Zpk {
+ final List zeros = new ArrayList<>();
+ final List poles = new ArrayList<>();
+ double gain = 1.0;
+
+ // A root is treated as real when |imag| falls below this. The same tolerance
+ // is used to match conjugates, since after bilinear/LP→BP transforms the real
+ // and imaginary drift of a true pair is of the same order.
+ private static final double IMAG_TOLERANCE = 1e-10;
+
+ private static final class Partitioned {
+ final List complexPairs = new ArrayList<>();
+ final List realRoots = new ArrayList<>();
+ }
+
+ // Partition a (conjugate-symmetric) root list into a vector of complex roots
+ // represented by the upper-half-plane conjugate-pair representative, plus a
+ // vector of real roots.
+ private static Partitioned partition(List roots) {
+ Partitioned out = new Partitioned();
+ boolean[] matched = new boolean[roots.size()];
+ for (int i = 0; i < roots.size(); i++) {
+ if (matched[i]) {
+ continue;
+ }
+ matched[i] = true;
+ Complex r = roots.get(i);
+ if (Math.abs(r.imag()) < IMAG_TOLERANCE) {
+ out.realRoots.add(r.real());
+ continue;
+ }
+ Complex rep = r.imag() > 0 ? r : r.conj();
+ // Find unmatched conjugate in the remaining list. Callers pass
+ // conjugate-symmetric inputs; if no partner is found the input violated
+ // that invariant (or drifted numerically past IMAG_TOLERANCE), and the
+ // cascade that follows would silently double-count the orphan.
+ boolean found = false;
+ for (int j = i + 1; j < roots.size(); j++) {
+ if (matched[j]) {
+ continue;
+ }
+ Complex rj = roots.get(j);
+ if (Math.abs(rj.imag() + r.imag()) < IMAG_TOLERANCE
+ && Math.abs(rj.real() - r.real()) < IMAG_TOLERANCE) {
+ matched[j] = true;
+ found = true;
+ break;
+ }
+ }
+ if (!found) {
+ throw new IllegalStateException("Zpk root list is not conjugate-symmetric");
+ }
+ out.complexPairs.add(rep);
+ }
+ return out;
+ }
+
+ static int relativeDegree(Zpk p) {
+ return p.poles.size() - p.zeros.size();
+ }
+
+ // The underlying LP→BP/BS substitutions are s → (s² + wo²)/(bw·s) for BP and
+ // the reciprocal for BS. Plugging either into a prototype factor (s - r) and
+ // clearing denominators yields a quadratic in s whose two roots become a
+ // conjugate pair around ±j·wo. Specifically:
+ // BP: s² - bw·r·s + wo² = 0
+ // BS: s² - (bw/r)·s + wo² = 0
+ // The caller folds the family-specific scaling into rScaled (bw·r/2 for BP,
+ // bw/(2·r) for BS) so this helper just solves the unified quadratic
+ // s² - 2·rScaled·s + wo² = 0 → rScaled ± sqrt(rScaled² - wo²).
+ private static Complex[] bpRoots(Complex rScaled, double wo) {
+ Complex disc = rScaled.mul(rScaled).sub(wo * wo).sqrt();
+ return new Complex[] {rScaled.add(disc), rScaled.sub(disc)};
+ }
+
+ /** Analog LP→LP transform: cutoff 1 rad/s → cutoff {@code wo} rad/s. */
+ static Zpk analogLpToLp(Zpk p, double wo) {
+ Zpk out = new Zpk();
+ out.gain = p.gain;
+ for (Complex z : p.zeros) {
+ out.zeros.add(z.mul(wo));
+ }
+ for (Complex pole : p.poles) {
+ out.poles.add(pole.mul(wo));
+ }
+ out.gain *= Math.pow(wo, relativeDegree(p));
+ return out;
+ }
+
+ /** Analog LP→HP transform: LP cutoff 1 → HP cutoff {@code wo} rad/s. */
+ static Zpk analogLpToHp(Zpk p, double wo) {
+ // Mirror: s → wo/s. Finite zeros/poles invert and scale; zeros at infinity
+ // become zeros at the origin to balance the pole count.
+ Zpk out = new Zpk();
+ Complex zProd = Complex.ONE;
+ Complex pProd = Complex.ONE;
+ for (Complex z : p.zeros) {
+ out.zeros.add(new Complex(wo, 0).div(z));
+ zProd = zProd.mul(z.negate());
+ }
+ for (Complex pole : p.poles) {
+ out.poles.add(new Complex(wo, 0).div(pole));
+ pProd = pProd.mul(pole.negate());
+ }
+ int degree = relativeDegree(p);
+ for (int i = 0; i < degree; i++) {
+ out.zeros.add(new Complex(0.0, 0.0));
+ }
+ out.gain = p.gain * zProd.div(pProd).real();
+ return out;
+ }
+
+ /**
+ * Analog LP→BP transform centered at {@code wo} rad/s with bandwidth {@code bw} rad/s. Each
+ * prototype pole becomes two; each prototype zero becomes two; plus {@code degree} zeros at the
+ * origin.
+ */
+ static Zpk analogLpToBp(Zpk p, double wo, double bw) {
+ Zpk out = new Zpk();
+ for (Complex z : p.zeros) {
+ Complex[] zs = bpRoots(z.mul(bw * 0.5), wo);
+ out.zeros.add(zs[0]);
+ out.zeros.add(zs[1]);
+ }
+ for (Complex pole : p.poles) {
+ Complex[] ps = bpRoots(pole.mul(bw * 0.5), wo);
+ out.poles.add(ps[0]);
+ out.poles.add(ps[1]);
+ }
+ int degree = relativeDegree(p);
+ for (int i = 0; i < degree; i++) {
+ out.zeros.add(new Complex(0.0, 0.0));
+ }
+ out.gain = p.gain * Math.pow(bw, degree);
+ return out;
+ }
+
+ /**
+ * Analog LP→BS transform centered at {@code wo} rad/s with bandwidth {@code bw} rad/s. Same
+ * fan-out as LpToBp; the added zeros go to ±j·wo instead of the origin.
+ */
+ static Zpk analogLpToBs(Zpk p, double wo, double bw) {
+ Zpk out = new Zpk();
+ Complex zProd = Complex.ONE;
+ Complex pProd = Complex.ONE;
+ Complex halfBw = new Complex(bw * 0.5, 0);
+ for (Complex z : p.zeros) {
+ Complex[] zs = bpRoots(halfBw.div(z), wo);
+ out.zeros.add(zs[0]);
+ out.zeros.add(zs[1]);
+ zProd = zProd.mul(z.negate());
+ }
+ for (Complex pole : p.poles) {
+ Complex[] ps = bpRoots(halfBw.div(pole), wo);
+ out.poles.add(ps[0]);
+ out.poles.add(ps[1]);
+ pProd = pProd.mul(pole.negate());
+ }
+ int degree = relativeDegree(p);
+ Complex jwo = new Complex(0.0, wo);
+ for (int i = 0; i < degree; i++) {
+ out.zeros.add(jwo);
+ out.zeros.add(jwo.negate());
+ }
+ out.gain = p.gain * zProd.div(pProd).real();
+ return out;
+ }
+
+ /**
+ * Pair conjugate poles (and zeros) into biquad sections. Sections are ordered by ascending |pole|
+ * (innermost / least-aggressive first); the overall gain is folded into the first section's
+ * numerator.
+ */
+ static List zpkToSos(Zpk digital) {
+ // A conjugate pair (p, p̄) factors to (z - p)(z - p̄) = z² - 2·Re(p)·z + |p|²,
+ // a real-coefficient quadratic — that's how complex roots become the real
+ // (b0,b1,b2) and (1,a1,a2) the runtime needs. Same identity for zero pairs.
+ //
+ // Below: partition roots into complex pairs + lone reals, sort poles by
+ // |pole| (least aggressive first, for numerical conditioning), pair each
+ // pole pair with its nearest zero pair (scipy's "nearest" rule), and emit
+ // one biquad per pole pair (or per real pole for odd order). Leftover real
+ // zeros fill in the remaining biquad numerators.
+ Partitioned polePart = partition(digital.poles);
+ Partitioned zeroPart = partition(digital.zeros);
+
+ // Least-aggressive (smallest |pole|) sections go first, so scipy-style
+ // golden values line up and the numerically tightest biquad sits last.
+ polePart.complexPairs.sort(Comparator.comparingDouble(Complex::normSq));
+
+ // Pre-assign complex zeros to complex poles using scipy's 'nearest' pairing:
+ // process from worst pole (largest |p|, last in ascending sort) to best,
+ // each picking the nearest unused complex zero by Euclidean distance.
+ // This is deterministic even when all zeros have equal magnitude (e.g.
+ // Chebyshev II LP where every digital zero sits on the unit circle).
+ int numCplxPoles = polePart.complexPairs.size();
+ Complex[] cplxZeroForPole = new Complex[numCplxPoles];
+ boolean[] hasCplxZero = new boolean[numCplxPoles];
+ for (int i = numCplxPoles - 1; i >= 0 && !zeroPart.complexPairs.isEmpty(); i--) {
+ final Complex p = polePart.complexPairs.get(i);
+ int bestIdx = 0;
+ double bestDist = Double.POSITIVE_INFINITY;
+ for (int j = 0; j < zeroPart.complexPairs.size(); j++) {
+ double d = zeroPart.complexPairs.get(j).sub(p).normSq();
+ if (d < bestDist) {
+ bestDist = d;
+ bestIdx = j;
+ }
+ }
+ cplxZeroForPole[i] = zeroPart.complexPairs.get(bestIdx);
+ hasCplxZero[i] = true;
+ zeroPart.complexPairs.remove(bestIdx);
+ }
+
+ // Largest |zero| first on the list so removeLast() below pops in the
+ // same pole-order match.
+ zeroPart.realRoots.sort(Comparator.comparingDouble((Double d) -> Math.abs(d)).reversed());
+
+ List out = new ArrayList<>();
+
+ for (int i = 0; i < numCplxPoles; i++) {
+ Complex p = polePart.complexPairs.get(i);
+ double a1 = -2.0 * p.real();
+ double a2 = p.normSq();
+ double b0;
+ double b1;
+ double b2;
+ if (hasCplxZero[i]) {
+ Complex z = cplxZeroForPole[i];
+ b0 = 1.0;
+ b1 = -2.0 * z.real();
+ b2 = z.normSq();
+ } else if (zeroPart.realRoots.size() >= 2) {
+ double z1 = zeroPart.realRoots.remove(zeroPart.realRoots.size() - 1);
+ double z2 = zeroPart.realRoots.remove(zeroPart.realRoots.size() - 1);
+ b0 = 1.0;
+ b1 = -(z1 + z2);
+ b2 = z1 * z2;
+ } else if (!zeroPart.realRoots.isEmpty()) {
+ double z = zeroPart.realRoots.remove(zeroPart.realRoots.size() - 1);
+ b0 = 1.0;
+ b1 = -z;
+ b2 = 0.0;
+ } else {
+ b0 = 1.0;
+ b1 = 0.0;
+ b2 = 0.0;
+ }
+ out.add(new BiquadFilter.Section(b0, b1, b2, a1, a2));
+ }
+
+ for (double p : polePart.realRoots) {
+ double a1 = -p;
+ double a2 = 0.0;
+ double b0;
+ double b1;
+ double b2;
+ // A real pole takes at most one real zero; leave the rest for any
+ // subsequent first-order section.
+ if (!zeroPart.realRoots.isEmpty()) {
+ double z = zeroPart.realRoots.remove(zeroPart.realRoots.size() - 1);
+ b0 = 1.0;
+ b1 = -z;
+ b2 = 0.0;
+ } else {
+ b0 = 1.0;
+ b1 = 0.0;
+ b2 = 0.0;
+ }
+ out.add(new BiquadFilter.Section(b0, b1, b2, a1, a2));
+ }
+
+ if (!out.isEmpty()) {
+ BiquadFilter.Section first = out.get(0);
+ out.set(
+ 0,
+ new BiquadFilter.Section(
+ first.b0 * digital.gain,
+ first.b1 * digital.gain,
+ first.b2 * digital.gain,
+ first.a1,
+ first.a2));
+ }
+ return out;
+ }
+}
diff --git a/wpimath/src/main/native/cpp/filter/BiquadFilterDesign.cpp b/wpimath/src/main/native/cpp/filter/BiquadFilterDesign.cpp
new file mode 100644
index 0000000000..9de276c9a6
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/BiquadFilterDesign.cpp
@@ -0,0 +1,300 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include
+#include
+#include
+#include
+#include
+
+#include "internal/AnalogPrototypes.hpp"
+#include "internal/BilinearTransform.hpp"
+#include "internal/Zpk.hpp"
+#include "wpi/math/filter/BiquadFilter.hpp"
+
+// Public design factories. Each classical-IIR factory (Butterworth,
+// Chebyshev I/II, Elliptic) drives the same three-step pipeline that
+// scipy.signal's iirfilter does:
+//
+// 1. AnalogPrototypes::*Prototype — analog LP prototype, cutoff 1 rad/s
+// 2. BilinearTransform::DesignFromAnalogLp:
+// a. Zpk::AnalogLpTo{Lp,Hp,Bp,Bs} — kind-specific frequency transform
+// b. BilinearTransform — analog → digital at sample rate fs
+// c. Zpk::ZpkToSos — pair conjugate roots into biquads
+//
+// SciPy reference for the whole pipeline:
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+// (functions iirfilter, butter, cheby1, cheby2, ellip)
+//
+// Notch and MovingAverage are closed-form and do not go through the
+// prototype/bilinear path. They are documented inline below.
+
+namespace wpi::math {
+
+namespace {
+
+void ValidateClassicalArgs(BiquadFilter::Kind kind, int order,
+ double sampleRate, double lowCutoff,
+ double highCutoff) {
+ if (order < 1) {
+ throw std::invalid_argument("BiquadFilter design: order must be >= 1.");
+ }
+ if (sampleRate <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: sample rate must be positive.");
+ }
+ const double nyquist = 0.5 * sampleRate;
+ if (lowCutoff <= 0.0 || lowCutoff >= nyquist) {
+ throw std::invalid_argument(
+ "BiquadFilter design: cutoff must lie in (0, sampleRate/2).");
+ }
+ if (kind == BiquadFilter::Kind::BandPass ||
+ kind == BiquadFilter::Kind::BandStop) {
+ if (highCutoff <= lowCutoff || highCutoff >= nyquist) {
+ throw std::invalid_argument(
+ "BiquadFilter design: BandPass/BandStop requires "
+ "lowCutoff < highCutoff < sampleRate/2.");
+ }
+ }
+}
+
+void RejectBandKindForLpHpOverload(const char* factoryName,
+ BiquadFilter::Kind kind) {
+ if (kind == BiquadFilter::Kind::BandPass ||
+ kind == BiquadFilter::Kind::BandStop) {
+ throw std::invalid_argument(
+ std::string{"BiquadFilter::"} + factoryName +
+ ": BandPass/BandStop requires the overload that takes both "
+ "lowCutoff and highCutoff.");
+ }
+}
+
+void RejectLpHpKindForBandOverload(const char* factoryName,
+ BiquadFilter::Kind kind) {
+ if (kind == BiquadFilter::Kind::LowPass ||
+ kind == BiquadFilter::Kind::HighPass) {
+ throw std::invalid_argument(
+ std::string{"BiquadFilter::"} + factoryName +
+ ": LowPass/HighPass requires the single-cutoff overload.");
+ }
+}
+
+} // namespace
+
+BiquadFilter BiquadFilter::Butterworth(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff) {
+ RejectBandKindForLpHpOverload("Butterworth", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), cutoff.value(), 0.0);
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ButterworthPrototype(order), kind, sampleRate.value(),
+ cutoff.value(), 0.0)};
+}
+
+BiquadFilter BiquadFilter::Butterworth(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff) {
+ RejectLpHpKindForBandOverload("Butterworth", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), lowCutoff.value(),
+ highCutoff.value());
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ButterworthPrototype(order), kind, sampleRate.value(),
+ lowCutoff.value(), highCutoff.value())};
+}
+
+BiquadFilter BiquadFilter::ChebyshevI(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff,
+ double rippleDb) {
+ RejectLpHpKindForBandOverload("ChebyshevI", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), lowCutoff.value(),
+ highCutoff.value());
+ if (rippleDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: ChebyshevI passband ripple must be positive.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ChebyshevIPrototype(order, rippleDb), kind,
+ sampleRate.value(), lowCutoff.value(), highCutoff.value())};
+}
+
+BiquadFilter BiquadFilter::ChebyshevI(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff,
+ double rippleDb) {
+ RejectBandKindForLpHpOverload("ChebyshevI", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), cutoff.value(), 0.0);
+ if (rippleDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: ChebyshevI passband ripple must be positive.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ChebyshevIPrototype(order, rippleDb), kind,
+ sampleRate.value(), cutoff.value(), 0.0)};
+}
+
+BiquadFilter BiquadFilter::ChebyshevII(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff,
+ double stopAttenDb) {
+ RejectLpHpKindForBandOverload("ChebyshevII", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), lowCutoff.value(),
+ highCutoff.value());
+ if (stopAttenDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: ChebyshevII stopband attenuation must be "
+ "positive.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ChebyshevIIPrototype(order, stopAttenDb), kind,
+ sampleRate.value(), lowCutoff.value(), highCutoff.value())};
+}
+
+BiquadFilter BiquadFilter::ChebyshevII(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff,
+ double stopAttenDb) {
+ RejectBandKindForLpHpOverload("ChebyshevII", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), cutoff.value(), 0.0);
+ if (stopAttenDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: ChebyshevII stopband attenuation must be "
+ "positive.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::ChebyshevIIPrototype(order, stopAttenDb), kind,
+ sampleRate.value(), cutoff.value(), 0.0)};
+}
+
+BiquadFilter BiquadFilter::Elliptic(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff,
+ double rippleDb, double stopAttenDb) {
+ RejectLpHpKindForBandOverload("Elliptic", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), lowCutoff.value(),
+ highCutoff.value());
+ if (rippleDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: Elliptic passband ripple must be positive.");
+ }
+ if (stopAttenDb <= rippleDb) {
+ throw std::invalid_argument(
+ "BiquadFilter design: Elliptic stopband attenuation must exceed "
+ "passband ripple.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::EllipticPrototype(order, rippleDb, stopAttenDb), kind,
+ sampleRate.value(), lowCutoff.value(), highCutoff.value())};
+}
+
+BiquadFilter BiquadFilter::Elliptic(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff, double rippleDb,
+ double stopAttenDb) {
+ RejectBandKindForLpHpOverload("Elliptic", kind);
+ ValidateClassicalArgs(kind, order, sampleRate.value(), cutoff.value(), 0.0);
+ if (rippleDb <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter design: Elliptic passband ripple must be positive.");
+ }
+ if (stopAttenDb <= rippleDb) {
+ throw std::invalid_argument(
+ "BiquadFilter design: Elliptic stopband attenuation must exceed "
+ "passband ripple.");
+ }
+ return BiquadFilter{filter::internal::DesignFromAnalogLp(
+ filter::internal::EllipticPrototype(order, rippleDb, stopAttenDb), kind,
+ sampleRate.value(), cutoff.value(), 0.0)};
+}
+
+BiquadFilter BiquadFilter::Notch(wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t centerFrequency,
+ double qualityFactor) {
+ const double fs = sampleRate.value();
+ const double f0 = centerFrequency.value();
+ if (fs <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter::Notch: sample rate must be positive.");
+ }
+ if (qualityFactor <= 0.0) {
+ throw std::invalid_argument(
+ "BiquadFilter::Notch: quality factor must be positive.");
+ }
+ const double nyquist = 0.5 * fs;
+ if (f0 <= 0.0 || f0 >= nyquist) {
+ throw std::invalid_argument(
+ "BiquadFilter::Notch: center frequency must lie in (0, "
+ "sampleRate/2).");
+ }
+
+ // Standard second-order IIR notch (zero pair on the unit circle at ±w0,
+ // pole pair just inside on the same radial line). Matches
+ // scipy.signal.iirnotch(f0, Q, fs) exactly:
+ // w0 = 2π·f0/fs, bw = w0/Q, β = tan(bw/2), g = 1/(1 + β)
+ // b = g · [1, -2cos(w0), 1], a = [1, -2g·cos(w0), 2g - 1]
+ // SciPy reference (function iirnotch):
+ // https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ // Background: Sophocles Orfanidis, "Introduction to Signal Processing"
+ // §11.3.2 ("Parametric resonators and notch filters").
+ const double w0 = 2.0 * std::numbers::pi * f0 / fs;
+ const double bw = w0 / qualityFactor;
+ const double beta = std::tan(0.5 * bw);
+ const double gain = 1.0 / (1.0 + beta);
+ const double cosW0 = std::cos(w0);
+
+ Section s{
+ .b0 = gain,
+ .b1 = -2.0 * gain * cosW0,
+ .b2 = gain,
+ .a1 = -2.0 * gain * cosW0,
+ .a2 = 2.0 * gain - 1.0,
+ };
+ return BiquadFilter{{s}};
+}
+
+BiquadFilter BiquadFilter::MovingAverage(int taps) {
+ if (taps < 1) {
+ throw std::invalid_argument(
+ "BiquadFilter::MovingAverage: taps must be >= 1.");
+ }
+
+ // A length-N moving average has H(z) = (1/N)·(1 - z⁻ᴺ)/(1 - z⁻¹), whose
+ // non-trivial zeros are the Nth roots of unity except z = 1:
+ // z_k = exp(i·2π·k/N), k = 1..N-1
+ // Pair each (z_k, z_{N-k}) into one all-zero biquad
+ // (b0, b1, b2, a1, a2) = (1, -2·cos(2πk/N), 1, 0, 0)
+ // and, if N is even, emit a single-zero first-order biquad for the unpaired
+ // root at z = -1:
+ // (1, 1, 0, 0, 0)
+ // The overall 1/N gain is folded into the first section.
+ //
+ // The factorization of (1 - z⁻ᴺ) into roots of unity is textbook:
+ // https://en.wikipedia.org/wiki/Root_of_unity#Polynomial_form
+ // Equivalent to scipy.signal.tf2sos applied to b = [1/N, ..., 1/N], a = [1].
+ std::vector out;
+ if (taps == 1) {
+ out.push_back({1.0, 0.0, 0.0, 0.0, 0.0});
+ return BiquadFilter{out};
+ }
+ const double N = static_cast(taps);
+ const int pairs = (taps - 1) / 2;
+ for (int k = 1; k <= pairs; ++k) {
+ double c = std::cos(2.0 * std::numbers::pi * k / N);
+ out.push_back({1.0, -2.0 * c, 1.0, 0.0, 0.0});
+ }
+ if (taps % 2 == 0) {
+ out.push_back({1.0, 1.0, 0.0, 0.0, 0.0});
+ }
+ const double g = 1.0 / N;
+ out[0].b0 *= g;
+ out[0].b1 *= g;
+ out[0].b2 *= g;
+ return BiquadFilter{out};
+}
+
+} // namespace wpi::math
diff --git a/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.cpp b/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.cpp
new file mode 100644
index 0000000000..8a0e235c8b
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.cpp
@@ -0,0 +1,282 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "AnalogPrototypes.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+#include "JacobiElliptic.hpp"
+
+// Each prototype mirrors the corresponding *_ap helper in scipy.signal:
+// buttap, cheb1ap, cheb2ap, ellipap. Those live in
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+// and are the canonical reference implementations. To verify a coefficient,
+// search that file for the function name and compare the closed-form
+// pole/zero/gain expressions line-by-line against the body below. Differences
+// here are limited to: hand-rolled Complex arithmetic in Java, and
+// expm1-based 10^x-1 evaluation for small ripple values (numerically more
+// accurate than std::pow(10, x) - 1).
+//
+// Textbook references for the families themselves:
+// - Butterworth poles on the unit circle:
+// https://en.wikipedia.org/wiki/Butterworth_filter#Transfer_function
+// - Chebyshev I/II pole/zero geometry:
+// https://en.wikipedia.org/wiki/Chebyshev_filter
+// - Elliptic (Cauer): Orfanidis, "Introduction to Signal Processing Second
+// Edition (2023)"
+// https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
+
+namespace wpi::math::filter::internal {
+
+namespace {
+
+constexpr double kCoefEps = 2e-16;
+
+// 10^x - 1 evaluated as expm1(x · ln10) for accuracy when x is small.
+double Pow10m1(double x) {
+ constexpr double kLn10 = 2.302585092994045684017991454684;
+ return std::expm1(kLn10 * x);
+}
+
+} // namespace
+
+Zpk ButterworthPrototype(int order) {
+ // Order-N Butterworth analog low-pass prototype (cutoff 1 rad/s). Poles are
+ // the LHP half of the unit circle, evenly spaced at:
+ // p_k = exp( j · (π/2 + π·(2k+1)/(2N)) ), k = 0..N-1
+ // No finite zeros; gain = 1. Matches scipy.signal.buttap.
+ // Reference:
+ // https://en.wikipedia.org/wiki/Butterworth_filter#Transfer_function
+ Zpk p;
+ p.gain = 1.0;
+ for (int k = 0; k < order; ++k) {
+ double angle = std::numbers::pi / 2.0 +
+ std::numbers::pi * (2.0 * k + 1.0) / (2.0 * order);
+ p.poles.push_back(std::polar(1.0, angle));
+ }
+ return p;
+}
+
+Zpk ChebyshevIPrototype(int order, double rippleDb) {
+ // Order-N Chebyshev type-I analog low-pass prototype (cutoff 1 rad/s).
+ // Equiripple in the passband. Matches scipy.signal.cheb1ap exactly.
+ // Reference:
+ // https://en.wikipedia.org/wiki/Chebyshev_filter#Type_I_Chebyshev_filters_(Chebyshev_filters)
+ // SciPy implementation:
+ // https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ // (function cheb1ap)
+ //
+ // Poles lie on an ellipse in the LHP at:
+ // p_k = -sinh(mu + j*theta_k)
+ // where mu = (1/N) * asinh(1/eps), eps = sqrt(10^(rp/10) - 1), and
+ // theta_k = pi*(2k - N + 1) / (2N) for k = 0..N-1.
+ Zpk out;
+ const double eps = std::sqrt(std::pow(10.0, 0.1 * rippleDb) - 1.0);
+ const double mu = std::asinh(1.0 / eps) / order;
+
+ cplx prodNegP{1.0, 0.0};
+ for (int k = 0; k < order; ++k) {
+ const double m = static_cast(-order + 1 + 2 * k);
+ const double theta = std::numbers::pi * m / (2.0 * order);
+ const cplx pole = -std::sinh(cplx{mu, theta});
+ out.poles.push_back(pole);
+ prodNegP *= -pole;
+ }
+
+ // Gain: forces |H(j0)| = 1 for odd N, 1/sqrt(1+eps^2) for even N (the
+ // ripple trough at DC).
+ double k = prodNegP.real();
+ if (order % 2 == 0) {
+ k /= std::sqrt(1.0 + eps * eps);
+ }
+ out.gain = k;
+ return out;
+}
+
+Zpk ChebyshevIIPrototype(int order, double stopAttenDb) {
+ // Order-N Chebyshev type-II ("inverse Chebyshev") analog low-pass prototype
+ // (stopband edge normalized to 1 rad/s — the point at which the response
+ // first reaches the stopband attenuation). Equiripple in the stopband.
+ // Matches scipy.signal.cheb2ap exactly.
+ // Reference:
+ // https://en.wikipedia.org/wiki/Chebyshev_filter#Type_II_Chebyshev_filters_(inverse_Chebyshev_filters)
+ // SciPy implementation:
+ // https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ // (function cheb2ap)
+ //
+ // Poles are reciprocals of the deformed unit-circle points; zeros sit on the
+ // imaginary axis at j/sin(theta_k).
+ Zpk out;
+ const double delta = 1.0 / std::sqrt(std::pow(10.0, 0.1 * stopAttenDb) - 1.0);
+ const double mu = std::asinh(1.0 / delta) / order;
+
+ cplx prodNegP{1.0, 0.0};
+ for (int k = 0; k < order; ++k) {
+ const double m1 = static_cast(-order + 1 + 2 * k);
+ const double theta = std::numbers::pi * m1 / (2.0 * order);
+ const cplx pole = -1.0 / std::sinh(cplx{mu, theta});
+ out.poles.push_back(pole);
+ prodNegP *= -pole;
+ }
+
+ // Zeros at z_k = j / sin(theta_k). For odd order the m=0 entry would give
+ // sin(0) → infinite zero; we skip it (one fewer zero than poles, matching
+ // scipy's pole-pair conventions for odd-order Chebyshev II).
+ cplx prodNegZ{1.0, 0.0};
+ for (int k = 0; k < order; ++k) {
+ const double m = static_cast(-order + 1 + 2 * k);
+ if (m == 0.0) {
+ continue;
+ }
+ const cplx zero{0.0, 1.0 / std::sin(std::numbers::pi * m / (2.0 * order))};
+ out.zeros.push_back(zero);
+ prodNegZ *= -zero;
+ }
+
+ out.gain = (prodNegP / prodNegZ).real();
+ return out;
+}
+
+Zpk EllipticPrototype(int order, double rippleDb, double stopAttenDb) {
+ // Order-N elliptic (Cauer) analog low-pass prototype (cutoff 1 rad/s).
+ // Equiripple in both passband and stopband. Matches scipy.signal.ellipap
+ // exactly within ~1e-12 for the orders/ripples we test.
+ //
+ // Primary reference (used to derive the construction below):
+ // Orfanidis, "Introduction to Signal Processing Second Edition (2023)"
+ // https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
+ // SciPy implementation (verbatim algorithm parity):
+ // https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+ // (function ellipap)
+ //
+ // The design proceeds in three stages:
+ // 1. Compute the small modulus m1 = eps^2 / (10^(As/10) - 1) and call
+ // EllipticDegree to find the large modulus m that satisfies the
+ // degree equation N·K(m)/K'(m) = K(m1)/K'(m1) (Orfanidis Eq. 49).
+ // 2. Place finite zeros at j/(sqrt(m)·sn(j·K/N, m)) for the appropriate
+ // index set (Orfanidis Eq. 64). Conjugate-mirror them.
+ // 3. Place poles using the auxiliary point v0 found by inverting
+ // sc(·, 1-m) at 1/eps (Orfanidis §10, Eq. 67–68).
+ Zpk out;
+
+ // Two corner cases mirror scipy.signal.ellipap: orders 0 and 1 collapse to
+ // closed forms with no zeros / a single real pole. Higher orders run the
+ // full Cauer construction below.
+ if (order <= 0) {
+ out.gain = std::pow(10.0, -rippleDb / 20.0);
+ return out;
+ }
+ if (order == 1) {
+ double p = -std::sqrt(1.0 / Pow10m1(0.1 * rippleDb));
+ out.poles.push_back(cplx{p, 0.0});
+ out.gain = -p;
+ return out;
+ }
+
+ const double epsSq = Pow10m1(0.1 * rippleDb);
+ const double eps = std::sqrt(epsSq);
+ const double ck1Sq = epsSq / Pow10m1(0.1 * stopAttenDb);
+ if (ck1Sq <= 0.0) {
+ throw std::invalid_argument(
+ "Elliptic design: invalid ripple/atten combination (small modulus = "
+ "0)");
+ }
+
+ const double K1 = EllipticK(ck1Sq);
+ const double m = EllipticDegree(order, ck1Sq);
+ const double capK = EllipticK(m);
+ const double sqrtM = std::sqrt(m);
+
+ // Build the index list: for odd N, j = [0, 2, ..., N-1]; for even N,
+ // j = [1, 3, ..., N-1]. Length is ceil(N/2).
+ std::vector jIdx;
+ for (int j = 1 - (order % 2); j < order; j += 2) {
+ jIdx.push_back(j);
+ }
+
+ // Cache (sn, cn, dn) at each j·K/N — needed for both zeros and poles.
+ std::vector jacobi;
+ jacobi.reserve(jIdx.size());
+ for (int j : jIdx) {
+ jacobi.push_back(Ellipj(j * capK / order, m));
+ }
+
+ // Zeros: z = j · 1/(sqrt(m)·sn), one per index where sn ≠ 0 (drops the j=0
+ // entry for odd N — the reciprocal would be a zero at infinity, which we
+ // simply omit). Each finite zero pairs with its complex conjugate.
+ std::vector zerosUpper;
+ for (size_t i = 0; i < jacobi.size(); ++i) {
+ double sn = jacobi[i].sn;
+ if (std::abs(sn) <= kCoefEps) {
+ continue;
+ }
+ cplx z{0.0, 1.0 / (sqrtM * sn)};
+ zerosUpper.push_back(z);
+ }
+ for (const auto& z : zerosUpper) {
+ out.zeros.push_back(z);
+ }
+ for (const auto& z : zerosUpper) {
+ out.zeros.push_back(std::conj(z));
+ }
+
+ // Poles use an auxiliary point v0 found by inverting sc(·, 1-m) at 1/eps,
+ // then ellipj at v0 with the complementary modulus.
+ const double r = InverseJacobiSc1(1.0 / eps, ck1Sq);
+ const double v0 = capK * r / (order * K1);
+ const JacobiResult sv = Ellipj(v0, 1.0 - m);
+
+ std::vector polesUpper;
+ for (size_t i = 0; i < jacobi.size(); ++i) {
+ const double s = jacobi[i].sn;
+ const double c = jacobi[i].cn;
+ const double d = jacobi[i].dn;
+ cplx num{c * d * sv.sn * sv.cn, s * sv.dn};
+ double denom = 1.0 - (d * sv.sn) * (d * sv.sn);
+ polesUpper.push_back(-num / denom);
+ }
+
+ if (order % 2 != 0) {
+ // Odd order: one pole is purely real (from j=0). Append complex
+ // conjugates for the others; leave the real one alone.
+ for (const auto& p : polesUpper) {
+ out.poles.push_back(p);
+ }
+ for (const auto& p : polesUpper) {
+ if (std::abs(p.imag()) > kCoefEps) {
+ out.poles.push_back(std::conj(p));
+ }
+ }
+ } else {
+ for (const auto& p : polesUpper) {
+ out.poles.push_back(p);
+ }
+ for (const auto& p : polesUpper) {
+ out.poles.push_back(std::conj(p));
+ }
+ }
+
+ // Gain: real(prod(-p) / prod(-z)). Even-order trims by sqrt(1+eps²) so DC
+ // sits at the ripple trough, matching scipy's convention.
+ cplx prodNegP{1.0, 0.0};
+ for (const auto& p : out.poles) {
+ prodNegP *= -p;
+ }
+ cplx prodNegZ{1.0, 0.0};
+ for (const auto& z : out.zeros) {
+ prodNegZ *= -z;
+ }
+ double k = (prodNegP / prodNegZ).real();
+ if (order % 2 == 0) {
+ k /= std::sqrt(1.0 + epsSq);
+ }
+ out.gain = k;
+ return out;
+}
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.hpp b/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.hpp
new file mode 100644
index 0000000000..7939796f87
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.hpp
@@ -0,0 +1,42 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "Zpk.hpp"
+
+namespace wpi::math::filter::internal {
+
+/** Analog Butterworth low-pass prototype, cutoff 1 rad/s. */
+Zpk ButterworthPrototype(int order);
+
+/**
+ * Analog Chebyshev type-I low-pass prototype, equiripple in passband,
+ * cutoff 1 rad/s (the point at which the response first drops to -ripple dB).
+ *
+ * @param rippleDb Peak-to-peak passband ripple in dB (must be > 0).
+ */
+Zpk ChebyshevIPrototype(int order, double rippleDb);
+
+/**
+ * Analog Chebyshev type-II low-pass prototype, equiripple in stopband,
+ * stopband-edge frequency 1 rad/s (the point at which the response first
+ * reaches @a stopAttenDb of attenuation).
+ *
+ * @param stopAttenDb Stopband attenuation in dB (must be > 0).
+ */
+Zpk ChebyshevIIPrototype(int order, double stopAttenDb);
+
+/**
+ * Analog elliptic (Cauer) low-pass prototype: equiripple in both passband
+ * and stopband. Cutoff is normalized to 1 rad/s (the point at which the
+ * gain first drops below -rippleDb).
+ *
+ * @param order Filter order (>= 1).
+ * @param rippleDb Peak-to-peak passband ripple in dB (> 0).
+ * @param stopAttenDb Stopband attenuation in dB (> @a rippleDb).
+ */
+Zpk EllipticPrototype(int order, double rippleDb, double stopAttenDb);
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.cpp b/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.cpp
new file mode 100644
index 0000000000..7a0b933562
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.cpp
@@ -0,0 +1,107 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "BilinearTransform.hpp"
+
+#include
+#include
+
+#include "Zpk.hpp"
+
+// Standard analog→digital conversion: pre-warp the digital cutoff so the
+// post-bilinear digital response hits the requested edge exactly, then map
+// each analog pole p (resp. zero z) via s → 2·fs·(z-1)/(z+1):
+//
+// p_d = (2fs + p) / (2fs - p)
+//
+// Background:
+// - https://en.wikipedia.org/wiki/Bilinear_transform
+// - Oppenheim & Schafer, "Discrete-Time Signal Processing" §7.2.2
+// SciPy implementations to compare against, line for line:
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+// (functions bilinear_zpk and _zpkbilinear; constant prewarping is folded
+// into the lp2{lp,hp,bp,bs}_zpk callers above the bilinear step).
+
+namespace wpi::math::filter::internal {
+
+namespace {
+
+int RelativeDegree(const Zpk& p) {
+ return static_cast(p.poles.size()) - static_cast(p.zeros.size());
+}
+
+} // namespace
+
+double PreWarp(double fc, double fs) {
+ return 2.0 * fs * std::tan(std::numbers::pi * fc / fs);
+}
+
+Zpk BilinearTransform(const Zpk& analog, double fs) {
+ Zpk out;
+ double fs2 = 2.0 * fs;
+ cplx zNumProd = 1.0;
+ cplx zDenProd = 1.0;
+ for (auto& z : analog.zeros) {
+ out.zeros.push_back((fs2 + z) / (fs2 - z));
+ zNumProd *= (fs2 - z);
+ }
+ for (auto& p : analog.poles) {
+ out.poles.push_back((fs2 + p) / (fs2 - p));
+ zDenProd *= (fs2 - p);
+ }
+ // Analog filters with fewer zeros than poles have `degree` zeros at s=∞.
+ // The bilinear maps s=∞ to z=-1 (Nyquist), so materialize them here. This
+ // is what gives a Butterworth low-pass its N digital zeros at Nyquist and
+ // hence its hard rolloff at the top of the band.
+ int degree = RelativeDegree(analog);
+ for (int i = 0; i < degree; ++i) {
+ out.zeros.emplace_back(-1.0, 0.0);
+ }
+ out.gain = analog.gain * (zNumProd / zDenProd).real();
+ return out;
+}
+
+Sections DesignFromAnalogLp(const Zpk& analogLp,
+ wpi::math::BiquadFilter::Kind kind, double fs,
+ double f1, double f2) {
+ // Pipeline:
+ // 1. Pre-warp the requested digital cutoff(s) into the analog cutoff
+ // that maps back to them under the bilinear transform.
+ // 2. Reshape the 1 rad/s LP prototype with a kind-specific s-plane
+ // substitution (LP→LP/HP/BP/BS), giving an analog filter at the
+ // requested kind and cutoff.
+ // 3. Bilinear-transform the resulting analog ZPK to a digital ZPK
+ // (s-plane → z-plane).
+ // 4. Pair conjugate digital roots into a cascade of real-coefficient
+ // biquad sections.
+ using Kind = wpi::math::BiquadFilter::Kind;
+ Zpk analog = analogLp;
+ switch (kind) {
+ case Kind::LowPass:
+ analog = AnalogLpToLp(analog, PreWarp(f1, fs));
+ break;
+ case Kind::HighPass:
+ analog = AnalogLpToHp(analog, PreWarp(f1, fs));
+ break;
+ case Kind::BandPass: {
+ const double w1 = PreWarp(f1, fs);
+ const double w2 = PreWarp(f2, fs);
+ const double wo = std::sqrt(w1 * w2);
+ const double bw = w2 - w1;
+ analog = AnalogLpToBp(analog, wo, bw);
+ break;
+ }
+ case Kind::BandStop: {
+ const double w1 = PreWarp(f1, fs);
+ const double w2 = PreWarp(f2, fs);
+ const double wo = std::sqrt(w1 * w2);
+ const double bw = w2 - w1;
+ analog = AnalogLpToBs(analog, wo, bw);
+ break;
+ }
+ }
+ return ZpkToSos(BilinearTransform(analog, fs));
+}
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.hpp b/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.hpp
new file mode 100644
index 0000000000..a52b64cb1f
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/BilinearTransform.hpp
@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include "Zpk.hpp"
+#include "wpi/math/filter/BiquadFilter.hpp"
+
+namespace wpi::math::filter::internal {
+
+/**
+ * Pre-warp a digital cutoff frequency (Hz) for use as the analog-domain
+ * cutoff that, after the bilinear transform at the same @a fs, maps back to
+ * exactly that digital cutoff.
+ */
+double PreWarp(double fc, double fs);
+
+/**
+ * Bilinear transform of an analog ZPK to a digital ZPK at sample rate @a fs.
+ * Analog zeros at infinity map to digital zeros at z = -1 (Nyquist).
+ */
+Zpk BilinearTransform(const Zpk& analog, double fs);
+
+/**
+ * Apply the kind-specific frequency transform (LP/HP/BP/BS) to an analog LP
+ * prototype, run the bilinear transform at @a fs, and convert to a SOS
+ * cascade. Shared by every classical IIR design factory (Butterworth,
+ * Chebyshev I/II, Elliptic).
+ *
+ * Caller is responsible for validating inputs (positive fs, f1 in (0, fs/2),
+ * and for BP/BS, f1 < f2 < fs/2). This helper does no validation itself.
+ */
+Sections DesignFromAnalogLp(const Zpk& analogLp,
+ wpi::math::BiquadFilter::Kind kind, double fs,
+ double f1, double f2);
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.cpp b/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.cpp
new file mode 100644
index 0000000000..7e9edc3e68
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.cpp
@@ -0,0 +1,182 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "JacobiElliptic.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+// All four routines below (EllipticK, Ellipj, InverseJacobiSn, EllipticDegree)
+// are tools used by the elliptic filter design path. They follow the
+// derivations and equation numbers in:
+//
+// Orfanidis, "Introduction to Signal Processing Second Edition (2023)"
+// https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
+//
+// SciPy implementations (for line-by-line comparison):
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+
+namespace wpi::math::filter::internal {
+
+namespace {
+
+constexpr int kMaxIter = 60;
+
+// sqrt(1 - k^2) computed as sqrt((1-k)(1+k)) — preserves precision when k is
+// small. Real-valued path used by Ellipj/InverseJacobiSn.
+double Complement(double k) {
+ return std::sqrt((1.0 - k) * (1.0 + k));
+}
+
+cplx Complement(cplx k) {
+ return std::sqrt((1.0 - k) * (1.0 + k));
+}
+
+} // namespace
+
+double EllipticK(double m) {
+ if (m < 0.0 || m > 1.0) {
+ return std::numeric_limits::quiet_NaN();
+ }
+ if (m == 1.0) {
+ return std::numeric_limits::infinity();
+ }
+ // AGM: K(m) = π / (2 · AGM(1, sqrt(1-m))).
+ double a = 1.0;
+ double b = std::sqrt(1.0 - m);
+ for (int i = 0; i < kMaxIter; ++i) {
+ if (std::abs(a - b) <= std::numeric_limits::epsilon() * a) {
+ break;
+ }
+ double aNext = 0.5 * (a + b);
+ double bNext = std::sqrt(a * b);
+ a = aNext;
+ b = bNext;
+ }
+ return std::numbers::pi / (2.0 * a);
+}
+
+JacobiResult Ellipj(double u, double m) {
+ if (m == 0.0) {
+ return {std::sin(u), std::cos(u), 1.0};
+ }
+ if (m == 1.0) {
+ double t = std::tanh(u);
+ double sech = 1.0 / std::cosh(u);
+ return {t, sech, sech};
+ }
+
+ // Ascending Landen: store a_n, c_n at each level until c_n is negligible.
+ std::vector a;
+ std::vector c;
+ a.push_back(1.0);
+ double b = std::sqrt(1.0 - m);
+ c.push_back(std::sqrt(m));
+
+ int n = 0;
+ while (n < kMaxIter) {
+ if (std::abs(c.back()) <=
+ std::numeric_limits::epsilon() * std::abs(a.back())) {
+ break;
+ }
+ double aN = a.back();
+ double bN = b;
+ a.push_back(0.5 * (aN + bN));
+ b = std::sqrt(aN * bN);
+ c.push_back(0.5 * (aN - bN));
+ ++n;
+ }
+
+ // Descend: phi_n = u · 2^n · a_n, then unwind.
+ double phi = u * std::ldexp(a.back(), n);
+ for (int j = n; j >= 1; --j) {
+ phi = 0.5 * (phi + std::asin((c[j] / a[j]) * std::sin(phi)));
+ }
+ double sn = std::sin(phi);
+ double cn = std::cos(phi);
+ // dn = sqrt(1 - m·sn²) — branch chosen so dn ≥ 0 in the principal interval,
+ // which matches scipy's convention.
+ double dn = std::sqrt(1.0 - m * sn * sn);
+ return {sn, cn, dn};
+}
+
+cplx InverseJacobiSn(cplx w, double m) {
+ // Descending Landen on the modulus: build a sequence of decreasing moduli
+ // until the smallest is effectively zero, then invert via arcsin and lift.
+ double k = std::sqrt(m);
+ if (k > 1.0) {
+ return {std::numeric_limits::quiet_NaN(),
+ std::numeric_limits::quiet_NaN()};
+ }
+ if (k == 1.0) {
+ // sn(z, 1) = tanh(z), so the inverse is atanh(w).
+ return std::atanh(w);
+ }
+
+ std::vector ks;
+ ks.push_back(k);
+ for (int i = 0; i < kMaxIter; ++i) {
+ if (ks.back() == 0.0) {
+ break;
+ }
+ double kp = Complement(ks.back());
+ double next = (1.0 - kp) / (1.0 + kp);
+ ks.push_back(next);
+ }
+
+ // Capital K of the original modulus equals (π/2) · ∏(1 + k_i) for i ≥ 1.
+ double K = 1.0;
+ for (size_t i = 1; i < ks.size(); ++i) {
+ K *= (1.0 + ks[i]);
+ }
+ K *= std::numbers::pi / 2.0;
+
+ std::vector wns;
+ wns.reserve(ks.size());
+ wns.push_back(w);
+ for (size_t i = 0; i + 1 < ks.size(); ++i) {
+ cplx wn = wns.back();
+ cplx wnext =
+ (2.0 * wn) / ((1.0 + ks[i + 1]) * (1.0 + Complement(ks[i] * wn)));
+ wns.push_back(wnext);
+ }
+
+ cplx u = (2.0 / std::numbers::pi) * std::asin(wns.back());
+ return K * u;
+}
+
+double InverseJacobiSc1(double w, double m) {
+ // sc(z, 1-m) = -j · sn(j·z, m), so sc(z, 1-m) = w → sn(j·z, m) = j·w →
+ // j·z = arcsn(j·w, m). The result is purely imaginary; return its imag part.
+ cplx z = InverseJacobiSn(cplx{0.0, w}, m);
+ return z.imag();
+}
+
+double EllipticDegree(int N, double m1) {
+ // Solve N · K(m)/K'(m) = K(m1)/K'(m1) for m using the q-nome series:
+ // q1 = exp(-π · K'(m1) / K(m1)), q = q1^(1/N),
+ // m = 16q · (Σ q^{i(i+1)})⁴ / (1 + 2 Σ q^{i²})⁴
+ constexpr int kMmax = 7;
+ double K1 = EllipticK(m1);
+ double K1p = EllipticK(1.0 - m1);
+ double q1 = std::exp(-std::numbers::pi * K1p / K1);
+ double q = std::pow(q1, 1.0 / N);
+
+ double num = 0.0;
+ for (int i = 0; i <= kMmax; ++i) {
+ num += std::pow(q, static_cast(i) * (i + 1));
+ }
+ double den = 1.0;
+ for (int i = 1; i <= kMmax + 1; ++i) {
+ den += 2.0 * std::pow(q, static_cast(i) * i);
+ }
+
+ double ratio = num / den;
+ return 16.0 * q * ratio * ratio * ratio * ratio;
+}
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.hpp b/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.hpp
new file mode 100644
index 0000000000..1e60b0ea21
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.hpp
@@ -0,0 +1,60 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include
+
+namespace wpi::math::filter::internal {
+
+using cplx = std::complex;
+
+/**
+ * Complete elliptic integral of the first kind, K(m), via the
+ * arithmetic-geometric mean iteration.
+ *
+ * @param m Parameter (m = k², where k is the modulus). Domain: [0, 1].
+ * m=0 returns π/2; m=1 returns +∞.
+ */
+double EllipticK(double m);
+
+/** Jacobi elliptic functions evaluated at a single point. */
+struct JacobiResult {
+ double sn;
+ double cn;
+ double dn;
+};
+
+/**
+ * Jacobi elliptic functions sn(u, m), cn(u, m), dn(u, m) for real u and
+ * parameter m ∈ [0, 1]. Computed via the descending Landen transformation
+ * followed by ascending recovery — the same scheme used by Numerical Recipes
+ * and (under the hood) SciPy's special.ellipj.
+ */
+JacobiResult Ellipj(double u, double m);
+
+/**
+ * Inverse Jacobi sn: solves sn(z, m) = w for z, where w may be complex. Used
+ * by the elliptic filter design to compute v0.
+ *
+ * Implements the descending-Landen iteration from Orfanidis, "Lecture Notes
+ * on Elliptic Filter Design", Eq. (56).
+ */
+cplx InverseJacobiSn(cplx w, double m);
+
+/**
+ * Real inverse Jacobi sc with complementary modulus: solves sc(z, 1-m) = w
+ * for real z. Equivalent to scipy's _arc_jac_sc1(w, m).
+ */
+double InverseJacobiSc1(double w, double m);
+
+/**
+ * Solves the elliptic degree equation
+ * N · K(m) / K(1-m) = K(m1) / K(1-m1)
+ * for m given the order N and the small modulus parameter m1. Uses the
+ * q-nome series of Orfanidis Eq. (49).
+ */
+double EllipticDegree(int N, double m1);
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/Zpk.cpp b/wpimath/src/main/native/cpp/filter/internal/Zpk.cpp
new file mode 100644
index 0000000000..731a47b212
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/Zpk.cpp
@@ -0,0 +1,305 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "Zpk.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+// The four AnalogLpTo* helpers are the standard frequency-domain spectral
+// transformations (Oppenheim & Schafer, "Discrete-Time Signal Processing"
+// §7.1.5; Constantinides, "Spectral transformations for digital filters",
+// IEE Proc. 117 (1970) 1585–1590). They each correspond to a SciPy helper:
+// AnalogLpToLp ↔ scipy.signal.lp2lp_zpk
+// AnalogLpToHp ↔ scipy.signal.lp2hp_zpk
+// AnalogLpToBp ↔ scipy.signal.lp2bp_zpk
+// AnalogLpToBs ↔ scipy.signal.lp2bs_zpk
+// Source for all four:
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+//
+// ZpkToSos pairs conjugate roots into biquad sections using the same
+// "nearest pole/zero" pairing scipy.signal.zpk2sos uses by default. SciPy
+// reference (function zpk2sos and the helper _cplxreal):
+// https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
+//
+// We diverge from scipy in only one place: section ordering. SciPy can return
+// a "minimum phase" ordering, while we always sort by ascending |pole| (least
+// aggressive first). The cascade product is identical; only the per-section
+// numerical conditioning differs.
+
+namespace wpi::math::filter::internal {
+
+namespace {
+
+// A root is treated as real when |imag| falls below this. The same tolerance
+// is used to match conjugates, since after bilinear/LP→BP transforms the real
+// and imaginary drift of a true pair is of the same order.
+constexpr double kImagTolerance = 1e-10;
+
+// Partition a (conjugate-symmetric) root list into a vector of complex roots
+// represented by the upper-half-plane conjugate-pair representative, plus a
+// vector of real roots.
+struct Partitioned {
+ std::vector complexPairs; // one representative per conjugate pair
+ std::vector realRoots;
+};
+
+Partitioned Partition(const std::vector& roots) {
+ Partitioned out;
+ std::vector matched(roots.size(), false);
+ for (size_t i = 0; i < roots.size(); ++i) {
+ if (matched[i]) {
+ continue;
+ }
+ matched[i] = true;
+ if (std::abs(roots[i].imag()) < kImagTolerance) {
+ out.realRoots.push_back(roots[i].real());
+ continue;
+ }
+ // Prefer the upper-half representative.
+ cplx rep = roots[i].imag() > 0 ? roots[i] : std::conj(roots[i]);
+ // Find unmatched conjugate in the remaining list. Callers pass
+ // conjugate-symmetric inputs; if no partner is found the input violated
+ // that invariant (or drifted numerically past kImagTolerance), and the
+ // cascade that follows would silently double-count the orphan.
+ bool found = false;
+ for (size_t j = i + 1; j < roots.size(); ++j) {
+ if (matched[j]) {
+ continue;
+ }
+ if (std::abs(roots[j].imag() + roots[i].imag()) < kImagTolerance &&
+ std::abs(roots[j].real() - roots[i].real()) < kImagTolerance) {
+ matched[j] = true;
+ found = true;
+ break;
+ }
+ }
+ if (!found) {
+ throw std::logic_error("Zpk root list is not conjugate-symmetric");
+ }
+ out.complexPairs.push_back(rep);
+ }
+ return out;
+}
+
+int RelativeDegree(const Zpk& p) {
+ return static_cast(p.poles.size()) - static_cast(p.zeros.size());
+}
+
+// The underlying LP→BP/BS substitutions are s → (s² + wo²)/(bw·s) for BP and
+// the reciprocal for BS. Plugging either into a prototype factor (s - r) and
+// clearing denominators yields a quadratic in s whose two roots become a
+// conjugate pair around ±j·wo. Specifically:
+// BP: s² - bw·r·s + wo² = 0
+// BS: s² - (bw/r)·s + wo² = 0
+// The caller folds the family-specific scaling into rScaled (bw·r/2 for BP,
+// bw/(2·r) for BS) so this helper just solves the unified quadratic
+// s² - 2·rScaled·s + wo² = 0 → rScaled ± sqrt(rScaled² - wo²).
+std::pair BpRoots(cplx rScaled, double wo) {
+ cplx disc = std::sqrt(rScaled * rScaled - wo * wo);
+ return {rScaled + disc, rScaled - disc};
+}
+
+} // namespace
+
+Zpk AnalogLpToLp(const Zpk& p, double wo) {
+ Zpk out;
+ out.gain = p.gain;
+ for (auto& z : p.zeros) {
+ out.zeros.push_back(z * wo);
+ }
+ for (auto& pole : p.poles) {
+ out.poles.push_back(pole * wo);
+ }
+ out.gain *= std::pow(wo, RelativeDegree(p));
+ return out;
+}
+
+Zpk AnalogLpToHp(const Zpk& p, double wo) {
+ // Mirror: s → wo/s. Finite zeros/poles invert and scale; zeros at infinity
+ // become zeros at the origin to balance the pole count.
+ Zpk out;
+ cplx zProd = 1.0;
+ cplx pProd = 1.0;
+ for (auto& z : p.zeros) {
+ out.zeros.push_back(wo / z);
+ zProd *= -z;
+ }
+ for (auto& pole : p.poles) {
+ out.poles.push_back(wo / pole);
+ pProd *= -pole;
+ }
+ int degree = RelativeDegree(p);
+ for (int i = 0; i < degree; ++i) {
+ out.zeros.emplace_back(0.0, 0.0);
+ }
+ out.gain = p.gain * (zProd / pProd).real();
+ return out;
+}
+
+Zpk AnalogLpToBp(const Zpk& p, double wo, double bw) {
+ Zpk out;
+ for (auto& z : p.zeros) {
+ auto [z1, z2] = BpRoots(z * bw * 0.5, wo);
+ out.zeros.push_back(z1);
+ out.zeros.push_back(z2);
+ }
+ for (auto& pole : p.poles) {
+ auto [p1, p2] = BpRoots(pole * bw * 0.5, wo);
+ out.poles.push_back(p1);
+ out.poles.push_back(p2);
+ }
+ int degree = RelativeDegree(p);
+ for (int i = 0; i < degree; ++i) {
+ out.zeros.emplace_back(0.0, 0.0);
+ }
+ out.gain = p.gain * std::pow(bw, degree);
+ return out;
+}
+
+Zpk AnalogLpToBs(const Zpk& p, double wo, double bw) {
+ Zpk out;
+ cplx zProd = 1.0;
+ cplx pProd = 1.0;
+ for (auto& z : p.zeros) {
+ auto [z1, z2] = BpRoots(bw * 0.5 / z, wo);
+ out.zeros.push_back(z1);
+ out.zeros.push_back(z2);
+ zProd *= -z;
+ }
+ for (auto& pole : p.poles) {
+ auto [p1, p2] = BpRoots(bw * 0.5 / pole, wo);
+ out.poles.push_back(p1);
+ out.poles.push_back(p2);
+ pProd *= -pole;
+ }
+ int degree = RelativeDegree(p);
+ const cplx jwo{0.0, wo};
+ for (int i = 0; i < degree; ++i) {
+ out.zeros.push_back(jwo);
+ out.zeros.push_back(-jwo);
+ }
+ out.gain = p.gain * (zProd / pProd).real();
+ return out;
+}
+
+Sections ZpkToSos(const Zpk& digital) {
+ // A conjugate pair (p, p̄) factors to (z - p)(z - p̄) = z² - 2·Re(p)·z + |p|²,
+ // a real-coefficient quadratic — that's how complex roots become the real
+ // (b0,b1,b2) and (1,a1,a2) the runtime needs. Same identity for zero pairs.
+ //
+ // Below: partition roots into complex pairs + lone reals, sort poles by
+ // |pole| (least aggressive first, for numerical conditioning), pair each
+ // pole pair with its nearest zero pair (scipy's "nearest" rule), and emit
+ // one biquad per pole pair (or per real pole for odd order). Leftover real
+ // zeros fill in the remaining biquad numerators.
+ auto polePart = Partition(digital.poles);
+ auto zeroPart = Partition(digital.zeros);
+
+ // Least-aggressive (smallest |pole|) sections go first, so scipy-style
+ // golden values line up and the numerically tightest biquad sits last.
+ std::sort(
+ polePart.complexPairs.begin(), polePart.complexPairs.end(),
+ [](const cplx& a, const cplx& b) { return std::norm(a) < std::norm(b); });
+
+ // Pre-assign complex zeros to complex poles using scipy's 'nearest' pairing:
+ // process from worst pole (largest |p|, last in ascending sort) to best,
+ // each picking the nearest unused complex zero by Euclidean distance.
+ // This is deterministic even when all zeros have equal magnitude (e.g.
+ // Chebyshev II LP where every digital zero sits on the unit circle).
+ int numCplxPoles = static_cast(polePart.complexPairs.size());
+ std::vector cplxZeroForPole(numCplxPoles, {0.0, 0.0});
+ std::vector hasCplxZero(numCplxPoles, false);
+ for (int i = numCplxPoles - 1; i >= 0 && !zeroPart.complexPairs.empty();
+ --i) {
+ cplx p = polePart.complexPairs[i];
+ auto best = std::min_element(zeroPart.complexPairs.begin(),
+ zeroPart.complexPairs.end(),
+ [&p](const cplx& a, const cplx& b) {
+ return std::norm(a - p) < std::norm(b - p);
+ });
+ cplxZeroForPole[i] = *best;
+ hasCplxZero[i] = true;
+ zeroPart.complexPairs.erase(best);
+ }
+
+ // Largest |zero| first on the stack so pops below match the pole order.
+ std::sort(zeroPart.realRoots.begin(), zeroPart.realRoots.end(),
+ [](double a, double b) { return std::abs(a) > std::abs(b); });
+
+ auto takeZeroPair = [&](Section& s, int poleIdx) {
+ if (hasCplxZero[poleIdx]) {
+ cplx z = cplxZeroForPole[poleIdx];
+ s.b0 = 1.0;
+ s.b1 = -2.0 * z.real();
+ s.b2 = std::norm(z);
+ return;
+ }
+ if (zeroPart.realRoots.size() >= 2) {
+ double z1 = zeroPart.realRoots.back();
+ zeroPart.realRoots.pop_back();
+ double z2 = zeroPart.realRoots.back();
+ zeroPart.realRoots.pop_back();
+ s.b0 = 1.0;
+ s.b1 = -(z1 + z2);
+ s.b2 = z1 * z2;
+ return;
+ }
+ if (!zeroPart.realRoots.empty()) {
+ double z = zeroPart.realRoots.back();
+ zeroPart.realRoots.pop_back();
+ s.b0 = 1.0;
+ s.b1 = -z;
+ s.b2 = 0.0;
+ return;
+ }
+ s.b0 = 1.0;
+ s.b1 = 0.0;
+ s.b2 = 0.0;
+ };
+
+ Sections out;
+ out.reserve(polePart.complexPairs.size() + polePart.realRoots.size());
+
+ for (int i = 0; i < numCplxPoles; ++i) {
+ const cplx& p = polePart.complexPairs[i];
+ Section s{};
+ s.a1 = -2.0 * p.real();
+ s.a2 = std::norm(p);
+ takeZeroPair(s, i);
+ out.push_back(s);
+ }
+
+ for (double p : polePart.realRoots) {
+ Section s{};
+ s.a1 = -p;
+ s.a2 = 0.0;
+ // A real pole takes at most one real zero; leave the rest for any
+ // subsequent first-order section.
+ if (!zeroPart.realRoots.empty()) {
+ double z = zeroPart.realRoots.back();
+ zeroPart.realRoots.pop_back();
+ s.b0 = 1.0;
+ s.b1 = -z;
+ s.b2 = 0.0;
+ } else {
+ s.b0 = 1.0;
+ s.b1 = 0.0;
+ s.b2 = 0.0;
+ }
+ out.push_back(s);
+ }
+
+ if (!out.empty()) {
+ out[0].b0 *= digital.gain;
+ out[0].b1 *= digital.gain;
+ out[0].b2 *= digital.gain;
+ }
+ return out;
+}
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/cpp/filter/internal/Zpk.hpp b/wpimath/src/main/native/cpp/filter/internal/Zpk.hpp
new file mode 100644
index 0000000000..05cf22c2ba
--- /dev/null
+++ b/wpimath/src/main/native/cpp/filter/internal/Zpk.hpp
@@ -0,0 +1,58 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include
+#include
+
+#include "wpi/math/filter/BiquadFilter.hpp"
+
+namespace wpi::math::filter::internal {
+
+using cplx = std::complex;
+using Section = wpi::math::BiquadFilter::Section;
+using Sections = std::vector;
+
+/**
+ * Zeros/poles/gain representation of a rational transfer function:
+ * H(s) = gain · ∏(s - z_i) / ∏(s - p_j) (analog)
+ * H(z) = gain · ∏(z - z_i) / ∏(z - p_j) (digital)
+ *
+ * Complex roots must appear in conjugate pairs; that invariant is preserved
+ * by every transform below.
+ */
+struct Zpk {
+ std::vector zeros;
+ std::vector poles;
+ double gain = 1.0;
+};
+
+/** Analog LP→LP transform: cutoff 1 rad/s → cutoff @a wo rad/s. */
+Zpk AnalogLpToLp(const Zpk& p, double wo);
+
+/** Analog LP→HP transform: LP cutoff 1 → HP cutoff @a wo rad/s. */
+Zpk AnalogLpToHp(const Zpk& p, double wo);
+
+/**
+ * Analog LP→BP transform centered at @a wo rad/s with bandwidth @a bw rad/s.
+ * Each prototype pole becomes two; each prototype zero becomes two; plus
+ * @c degree zeros at the origin.
+ */
+Zpk AnalogLpToBp(const Zpk& p, double wo, double bw);
+
+/**
+ * Analog LP→BS transform centered at @a wo rad/s with bandwidth @a bw rad/s.
+ * Same fan-out as LpToBp; the added zeros go to ±j·wo instead of the origin.
+ */
+Zpk AnalogLpToBs(const Zpk& p, double wo, double bw);
+
+/**
+ * Pair conjugate poles (and zeros) into biquad sections. Sections are
+ * ordered by ascending |pole| (innermost / least-aggressive first); the
+ * overall gain is folded into the first section's numerator.
+ */
+Sections ZpkToSos(const Zpk& digital);
+
+} // namespace wpi::math::filter::internal
diff --git a/wpimath/src/main/native/include/wpi/math/filter/BiquadFilter.hpp b/wpimath/src/main/native/include/wpi/math/filter/BiquadFilter.hpp
new file mode 100644
index 0000000000..3360f0de9b
--- /dev/null
+++ b/wpimath/src/main/native/include/wpi/math/filter/BiquadFilter.hpp
@@ -0,0 +1,399 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "wpi/math/util/MathShared.hpp"
+#include "wpi/units/frequency.hpp"
+#include "wpi/util/SymbolExports.hpp"
+
+namespace wpi::math {
+
+/**
+ * This class implements a cascade of second-order IIR filter sections (biquads)
+ * in Direct Form II Transposed. It is intended for running higher-order filters
+ * (Butterworth, Chebyshev, etc.) produced by a filter designer without the
+ * numerical instability that direct-form implementations of a single high-order
+ * polynomial exhibit.
+ *
+ * Each section implements:
+ * y[n] = b₀ x[n] + s₁[n-1]
+ * s₁[n] = b₁ x[n] - a₁ y[n] + s₂[n-1]
+ * s₂[n] = b₂ x[n] - a₂ y[n]
+ *
+ * Sections are normalized so that a₀ = 1 and are applied in series.
+ *
+ * For 1st-order IIR filters or simple FIR filters (moving averages, finite
+ * differences), prefer LinearFilter and its factory methods — they cover those
+ * cases more ergonomically. Use BiquadFilter for high-order IIR cascades.
+ *
+ * Note: Calculate() should be called by the user on a known, regular period.
+ * Like any digital filter, the coefficients are a function of the sample rate
+ * they were designed for.
+ */
+class WPILIB_DLLEXPORT BiquadFilter {
+ public:
+ /**
+ * A single biquad (second-order) section. a₀ is assumed normalized to 1.
+ */
+ struct Section {
+ double b0;
+ double b1;
+ double b2;
+ double a1;
+ double a2;
+ };
+
+ /**
+ * Frequency response shape for the classical IIR design factories.
+ * For BandPass/BandStop, two cutoff frequencies (f1, f2) are required.
+ */
+ enum class Kind { LowPass, HighPass, BandPass, BandStop };
+
+ /**
+ * Creates a biquad filter cascade from the given sections.
+ *
+ * @param sections The biquad sections, applied in series.
+ * @throws std::runtime_error if sections is empty.
+ */
+ constexpr explicit BiquadFilter(std::span sections)
+ : m_sections(sections.begin(), sections.end()),
+ m_state(sections.size(), {0.0, 0.0}) {
+ if (sections.empty()) {
+ throw std::runtime_error("BiquadFilter requires at least one section.");
+ }
+
+ if (!std::is_constant_evaluated()) {
+ ++instances;
+ wpi::math::MathSharedStore::ReportUsage("BiquadFilter",
+ std::to_string(instances));
+ }
+ }
+
+ /**
+ * Creates a biquad filter cascade from the given sections.
+ *
+ * @param sections The biquad sections, applied in series.
+ * @throws std::runtime_error if sections is empty.
+ */
+ constexpr BiquadFilter(std::initializer_list sections)
+ : BiquadFilter(
+ std::span{sections.begin(), sections.end()}) {}
+
+ /**
+ * Calculates the next value of the filter.
+ *
+ * @param input Current input value.
+ * @return The filtered value at this step.
+ */
+ constexpr double Calculate(double input) {
+ // Direct Form II Transposed biquad. Per section, with state (s₁, s₂):
+ //
+ // y[n] = b₀·x[n] + s₁[n-1]
+ // s₁[n] = b₁·x[n] - a₁·y[n] + s₂[n-1]
+ // s₂[n] = b₂·x[n] - a₂·y[n]
+ //
+ // Reference:
+ // https://ccrma.stanford.edu/~jos/fp/Transposed_Direct_Forms.html
+ double x = input;
+ for (size_t i = 0; i < m_sections.size(); ++i) {
+ const auto& sec = m_sections[i];
+ auto& s = m_state[i];
+
+ double y = sec.b0 * x + s[0];
+ s[0] = sec.b1 * x - sec.a1 * y + s[1];
+ s[1] = sec.b2 * x - sec.a2 * y;
+
+ x = y;
+ }
+ m_lastOutput = x;
+ return x;
+ }
+
+ /**
+ * Resets the filter state to zero.
+ */
+ constexpr void Reset() {
+ for (auto& s : m_state) {
+ s = {0.0, 0.0};
+ }
+ m_lastOutput = 0.0;
+ }
+
+ /**
+ * Resets the filter state so that subsequent calls to Calculate() with a
+ * constant input equal to {@code value} immediately return the filter's
+ * steady-state response to that input.
+ *
+ * @param value The constant input value to seed with.
+ */
+ constexpr void Reset(double value) {
+ // Steady-state seed: at constant input x, y[n] = y[n-1] = y, s₁[n] =
+ // s₁[n-1], and s₂[n] = s₂[n-1]. Substituting into the DF-II Transposed
+ // update equations gives the linear system:
+ //
+ // y = b₀·x + s₁
+ // s₁ = b₁·x - a₁·y + s₂
+ // s₂ = b₂·x - a₂·y
+ //
+ // Adding the s₁ and s₂ rows eliminates s₂:
+ //
+ // s₁ = (b₁ + b₂)·x - (a₁ + a₂)·y
+ //
+ // Substituting into the y row yields y = H(1)·x, where
+ //
+ // H(1) = (b₀ + b₁ + b₂) / (1 + a₁ + a₂)
+ //
+ // is the section's DC gain (the transfer function evaluated at z = 1). s₂
+ // then falls out of its row directly. For cascades, each section's
+ // steady-state y is fed as the next section's x.
+ //
+ // Reference:
+ // https://ccrma.stanford.edu/~jos/fp/Transposed_Direct_Forms.html
+ double x = value;
+ for (size_t i = 0; i < m_sections.size(); ++i) {
+ const auto& sec = m_sections[i];
+ double sumB = sec.b0 + sec.b1 + sec.b2;
+ double sumA = sec.a1 + sec.a2;
+ double y = sumB * x / (1.0 + sumA);
+
+ m_state[i][0] = (sec.b1 + sec.b2) * x - sumA * y;
+ m_state[i][1] = sec.b2 * x - sec.a2 * y;
+
+ x = y;
+ }
+ m_lastOutput = x;
+ }
+
+ /**
+ * Returns the last value calculated by the BiquadFilter.
+ *
+ * @return The last value.
+ */
+ constexpr double LastValue() const { return m_lastOutput; }
+
+ /**
+ * Returns the number of sections in the cascade.
+ *
+ * @return The number of sections.
+ */
+ constexpr size_t NumSections() const { return m_sections.size(); }
+
+ /**
+ * Returns a view over the cascade's sections, in application order.
+ * Useful for inspection, logging, or serialization of designed filters.
+ *
+ * @return Span over the section list. Valid for the filter's lifetime.
+ */
+ std::span Sections() const { return m_sections; }
+
+ /**
+ * Designs a Butterworth IIR low-pass or high-pass filter (single cutoff).
+ *
+ * Coefficients match @c scipy.signal.butter(order, Wn, btype, fs,
+ * output='sos') to within ~1e-10.
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate. Must be positive.
+ * @param cutoff Cutoff frequency. Must satisfy
+ * 0 < cutoff < sampleRate/2.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is BandPass / BandStop.
+ */
+ static BiquadFilter Butterworth(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff);
+
+ /**
+ * Designs a Butterworth IIR band-pass or band-stop filter as a cascade of
+ * biquad sections.
+ *
+ * BandPass/BandStop outputs are numerically equivalent to scipy but may
+ * differ in section ordering / zero pairing; the product response matches.
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1). The resulting cascade has
+ * 2*order poles.
+ * @param sampleRate Sample rate. Must be positive.
+ * @param lowCutoff Low edge of the band. Must satisfy
+ * 0 < lowCutoff < highCutoff < sampleRate/2.
+ * @param highCutoff High edge of the band.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is LowPass / HighPass.
+ */
+ static BiquadFilter Butterworth(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff);
+
+ /**
+ * Designs a Chebyshev type-I IIR filter as a cascade of biquad sections.
+ * Equiripple in the passband, monotonic in the stopband. Coefficients match
+ * @c scipy.signal.cheby1(order, rp, Wn, btype, fs, output='sos').
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1). The cascade has 2*order poles.
+ * @param sampleRate Sample rate. Must be positive.
+ * @param lowCutoff Low edge of the band. Must satisfy
+ * 0 < lowCutoff < highCutoff < sampleRate/2.
+ * @param highCutoff High edge of the band.
+ * @param rippleDb Peak-to-peak passband ripple in dB. Must be > 0; values
+ * from ~0.1 to ~3 dB are typical.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is LowPass / HighPass.
+ */
+ static BiquadFilter ChebyshevI(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff,
+ double rippleDb);
+
+ /**
+ * Designs a Chebyshev type-I IIR low-pass or high-pass filter (single
+ * cutoff). The cutoff is the frequency at which the response first drops
+ * to -rippleDb dB.
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate. Must be positive.
+ * @param cutoff Cutoff frequency. Must satisfy
+ * 0 < cutoff < sampleRate/2.
+ * @param rippleDb Peak-to-peak passband ripple in dB. Must be > 0.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is BandPass / BandStop.
+ */
+ static BiquadFilter ChebyshevI(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff, double rippleDb);
+
+ /**
+ * Designs a Chebyshev type-II (inverse Chebyshev) IIR filter as a cascade of
+ * biquad sections. Monotonic in the passband, equiripple in the stopband.
+ * Coefficients match @c scipy.signal.cheby2(order, rs, Wn, btype, fs,
+ * output='sos').
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Prototype order (>= 1). The cascade has 2*order poles.
+ * @param sampleRate Sample rate. Must be positive.
+ * @param lowCutoff Low edge of the stop band. Must satisfy
+ * 0 < lowCutoff < highCutoff < sampleRate/2.
+ * @param highCutoff High edge of the stop band.
+ * @param stopAttenDb Stopband attenuation in dB. Must be > 0; values from
+ * ~20 to ~80 dB are typical.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is LowPass / HighPass.
+ */
+ static BiquadFilter ChebyshevII(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff,
+ double stopAttenDb);
+
+ /**
+ * Designs a Chebyshev type-II IIR low-pass or high-pass filter (single
+ * cutoff). The cutoff is the frequency at which the response first reaches
+ * @a stopAttenDb of attenuation.
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Prototype order (>= 1).
+ * @param sampleRate Sample rate. Must be positive.
+ * @param cutoff Stopband-edge frequency. Must satisfy
+ * 0 < cutoff < sampleRate/2.
+ * @param stopAttenDb Stopband attenuation in dB. Must be > 0.
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is BandPass / BandStop.
+ */
+ static BiquadFilter ChebyshevII(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff,
+ double stopAttenDb);
+
+ /**
+ * Designs an elliptic (Cauer) IIR filter as a cascade of biquad sections.
+ * Equiripple in both passband and stopband — the steepest transition for a
+ * given order, at the cost of ripple everywhere. Coefficients match
+ * @c scipy.signal.ellip(order, rp, rs, Wn, btype, fs, output='sos').
+ *
+ * @param kind Must be BandPass or BandStop.
+ * @param order Filter order (>= 1).
+ * @param sampleRate Sample rate. Must be positive.
+ * @param lowCutoff Low edge of the band. Must satisfy
+ * 0 < lowCutoff < highCutoff < sampleRate/2.
+ * @param highCutoff High edge of the band.
+ * @param rippleDb Passband ripple in dB (> 0).
+ * @param stopAttenDb Stopband attenuation in dB (must exceed @a rippleDb).
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is LowPass / HighPass.
+ */
+ static BiquadFilter Elliptic(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t lowCutoff,
+ wpi::units::hertz_t highCutoff, double rippleDb,
+ double stopAttenDb);
+
+ /**
+ * Designs an elliptic (Cauer) IIR low-pass or high-pass filter (single
+ * cutoff). The cutoff is the frequency at which the response first drops
+ * to -rippleDb dB.
+ *
+ * @param kind Must be LowPass or HighPass.
+ * @param order Filter order (>= 1).
+ * @param sampleRate Sample rate. Must be positive.
+ * @param cutoff Cutoff frequency. Must satisfy
+ * 0 < cutoff < sampleRate/2.
+ * @param rippleDb Passband ripple in dB (> 0).
+ * @param stopAttenDb Stopband attenuation in dB (must exceed @a rippleDb).
+ * @throws std::invalid_argument if any argument is out of range or @a kind
+ * is BandPass / BandStop.
+ */
+ static BiquadFilter Elliptic(Kind kind, int order,
+ wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t cutoff, double rippleDb,
+ double stopAttenDb);
+
+ /**
+ * Designs a second-order IIR notch at the given center frequency with the
+ * given quality factor. Matches @c scipy.signal.iirnotch.
+ *
+ * @param sampleRate Sample rate. Must be positive.
+ * @param centerFrequency Notch center frequency. Must satisfy
+ * 0 < centerFrequency < sampleRate/2.
+ * @param qualityFactor Quality factor (Q). Higher values give a narrower
+ * notch. Must be positive.
+ * @throws std::invalid_argument if any argument is out of range.
+ */
+ static BiquadFilter Notch(wpi::units::hertz_t sampleRate,
+ wpi::units::hertz_t centerFrequency,
+ double qualityFactor);
+
+ /**
+ * Designs an N-tap moving-average filter as a cascade of FIR biquads.
+ *
+ * Each section has a1 = a2 = 0 (all poles at the origin). The total gain
+ * 1/taps is folded into the first section's numerator so the DC gain of
+ * the cascade is 1.
+ *
+ * @param taps Length of the moving-average window. Must be >= 1.
+ * @throws std::invalid_argument if taps < 1.
+ */
+ static BiquadFilter MovingAverage(int taps);
+
+ private:
+ std::vector m_sections;
+ std::vector> m_state;
+ double m_lastOutput = 0.0;
+
+ inline static int instances = 0;
+};
+
+} // namespace wpi::math
diff --git a/wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp b/wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
index c62bf203aa..fc170fd507 100644
--- a/wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
+++ b/wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
@@ -61,6 +61,12 @@ namespace wpi::math {
* https://en.wikipedia.org/wiki/Iir_filter
* https://en.wikipedia.org/wiki/Fir_filter
*
+ * For IIR filters of order 4 or higher, prefer BiquadFilter — it represents
+ * the filter as a cascade of 2nd-order sections (Direct Form II Transposed),
+ * which avoids the numerical instability that high-order direct-form
+ * polynomials exhibit. Use LinearFilter for low-order IIR (SinglePoleIIR,
+ * HighPass) and FIR filters (MovingAverage, FiniteDifference).
+ *
* Note 1: Calculate() should be called by the user on a known, regular period.
* You can use a Notifier for this or do it "inline" with code in a
* periodic function.
diff --git a/wpimath/src/main/python/pyproject.toml b/wpimath/src/main/python/pyproject.toml
index 6066d3c4fe..85008e77e4 100644
--- a/wpimath/src/main/python/pyproject.toml
+++ b/wpimath/src/main/python/pyproject.toml
@@ -1495,6 +1495,7 @@ SwerveDrivePoseEstimator3d = "wpi/math/estimator/SwerveDrivePoseEstimator3d.hpp"
# UnscentedTransform = "wpi/math/estimator/UnscentedTransform.hpp"
# wpi/math/filter
+BiquadFilter = "wpi/math/filter/BiquadFilter.hpp"
Debouncer = "wpi/math/filter/Debouncer.hpp"
EdgeCounterFilter = "wpi/math/filter/EdgeCounterFilter.hpp"
LinearFilter = "wpi/math/filter/LinearFilter.hpp"
diff --git a/wpimath/src/main/python/semiwrap/BiquadFilter.yml b/wpimath/src/main/python/semiwrap/BiquadFilter.yml
new file mode 100644
index 0000000000..e3a62581b4
--- /dev/null
+++ b/wpimath/src/main/python/semiwrap/BiquadFilter.yml
@@ -0,0 +1,52 @@
+classes:
+ wpi::math::BiquadFilter:
+ enums:
+ Kind:
+ methods:
+ BiquadFilter:
+ overloads:
+ std::span:
+ std::initializer_list:
+ ignore: true
+ Butterworth:
+ overloads:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, wpi::units::hertz_t:
+ ChebyshevI:
+ overloads:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, wpi::units::hertz_t, double:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, double:
+ ChebyshevII:
+ overloads:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, wpi::units::hertz_t, double:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, double:
+ Elliptic:
+ overloads:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, wpi::units::hertz_t, double, double:
+ Kind, int, wpi::units::hertz_t, wpi::units::hertz_t, double, double:
+ Notch:
+ MovingAverage:
+ Calculate:
+ Reset:
+ overloads:
+ '':
+ double:
+ LastValue:
+ NumSections:
+ Sections:
+ wpi::math::BiquadFilter::Section:
+ attributes:
+ b0:
+ b1:
+ b2:
+ a1:
+ a2:
+ inline_code: |
+ .def(py::init([](double b0, double b1, double b2, double a1, double a2) {
+ return wpi::math::BiquadFilter::Section{b0, b1, b2, a1, a2};
+ }), py::arg("b0"), py::arg("b1"), py::arg("b2"), py::arg("a1"), py::arg("a2"))
+
+ .def("__repr__", [](const wpi::math::BiquadFilter::Section &self) {
+ return py::str("BiquadFilter.Section(b0={}, b1={}, b2={}, a1={}, a2={})").format(
+ self.b0, self.b1, self.b2, self.a1, self.a2);
+ })
diff --git a/wpimath/src/main/python/wpimath/__init__.py b/wpimath/src/main/python/wpimath/__init__.py
index 9396430432..db7293029c 100644
--- a/wpimath/src/main/python/wpimath/__init__.py
+++ b/wpimath/src/main/python/wpimath/__init__.py
@@ -5,6 +5,7 @@ from ._wpimath import (
AntiTipping,
ArmFeedforward,
BangBangController,
+ BiquadFilter,
CentripetalAccelerationConstraint,
ChassisAccelerations,
ChassisVelocities,
@@ -199,6 +200,7 @@ __all__ = [
"AntiTipping",
"ArmFeedforward",
"BangBangController",
+ "BiquadFilter",
"CentripetalAccelerationConstraint",
"ChassisAccelerations",
"ChassisVelocities",
diff --git a/wpimath/src/test/generate_biquad_fixtures.py b/wpimath/src/test/generate_biquad_fixtures.py
new file mode 100755
index 0000000000..3b43687e8d
--- /dev/null
+++ b/wpimath/src/test/generate_biquad_fixtures.py
@@ -0,0 +1,221 @@
+#!/usr/bin/env python3
+"""Reproducibility script for the BiquadFilter test golden values.
+
+Run this script (requires scipy + numpy) to print every scipy-derived
+constant that's hard-coded into the BiquadFilter test suites:
+
+ wpimath/src/test/native/cpp/filter/BiquadFilter*Test.cpp
+ wpimath/src/test/java/org/wpilib/math/filter/BiquadFilter*Test.java
+ wpimath/src/test/python/test_biquad_filter.py
+
+The hard-coded test literals were generated against the scipy version
+printed at the top of this script's output. If scipy ever changes the
+underlying algorithm or default precision behavior the literals may need
+to be regenerated. A lower-bound check is enforced below to catch the
+known-incompatible historical versions.
+"""
+
+import math
+import sys
+
+import numpy as np
+import scipy
+import scipy.signal as sig
+
+_MIN_SCIPY = (1, 11)
+_scipy_version = tuple(int(p) for p in scipy.__version__.split(".")[:2])
+if _scipy_version < _MIN_SCIPY:
+ sys.exit(
+ f"scipy >= {'.'.join(str(p) for p in _MIN_SCIPY)} required "
+ f"(found {scipy.__version__})"
+ )
+
+print(f"# scipy {scipy.__version__}, numpy {np.__version__}")
+print()
+
+
+def _format_section(row):
+ """scipy SOS row is [b0, b1, b2, 1.0, a1, a2]; tests store (b0,b1,b2,a1,a2)."""
+ b0, b1, b2, _, a1, a2 = (float(v) for v in row)
+ return f"({b0!r}, {b1!r}, {b2!r}, {a1!r}, {a2!r})"
+
+
+def _print_sos(label, sos):
+ print(f"# {label}")
+ for i, row in enumerate(sos):
+ print(f" sections[{i}] = {_format_section(row)}")
+ print()
+
+
+# ---------- Butterworth -----------------------------------------------------
+
+# Used by:
+# BiquadFilterTest.Butterworth4thOrderLowPass (cpp / java)
+# BiquadFilterTest.ResetZerosState (cpp / java)
+# BiquadFilterTest.ResetToSteadyState (cpp / java)
+# BiquadFilterTest.DCGainConverges (cpp / java)
+# BiquadFilterDesignTest.ButterworthLowPass4thOrderMatchesScipy
+# test_biquad_filter.test_butterworth_4th_order_low_pass
+# test_biquad_filter.test_butterworth_factory_matches_scipy
+# test_biquad_filter.test_reset_zeros_state / test_reset_to_steady_state
+sos_butter4_lp = sig.butter(4, 50.0, btype="low", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')", sos_butter4_lp
+)
+
+# Impulse response, first 30 samples, used in:
+# BiquadFilterTest.Butterworth4thOrderLowPass (cpp / java / python)
+print("# Butterworth4thOrderLowPass impulse response, first 30 samples")
+print("# (sosfilt of a unit impulse — matches inline values exactly)")
+x = np.zeros(30)
+x[0] = 1.0
+y = sig.sosfilt(sos_butter4_lp, x)
+for i, v in enumerate(y):
+ print(f" y[{i:2d}] = {float(v)!r}")
+print()
+
+# Used by:
+# BiquadFilterTest.Order8ButterworthMatchesScipy (cpp / java)
+# BiquadFilterDesignTest.ButterworthLowPass8thOrderMatchesScipy
+# test_biquad_filter.test_order_8_butterworth_matches_scipy
+sos_butter8_lp = sig.butter(8, 100.0, btype="low", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')",
+ sos_butter8_lp,
+)
+
+# Used by:
+# BiquadFilterDesignTest.ButterworthBandPass4thOrderMatchesScipy (cpp / java)
+# test_biquad_filter.test_butterworth_bandpass_factory_matches_scipy
+sos_butter4_bp = sig.butter(4, [80.0, 120.0], btype="bandpass", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.butter(4, [80.0, 120.0], btype='bandpass', fs=1000.0, "
+ "output='sos')",
+ sos_butter4_bp,
+)
+
+# Chirp 1→200 Hz over 500 samples at 1 kHz, spot samples for the order-8
+# filter.
+print("# Order8ButterworthMatchesScipy chirp spot samples")
+print("# (cos chirp 1→200 Hz, N=500, fs=1000, then sosfilt)")
+N = 500
+fs = 1000.0
+f0 = 1.0
+f1 = 200.0
+t1 = (N - 1) / fs
+k = (f1 - f0) / t1
+n = np.arange(N)
+t = n / fs
+phase = 2.0 * np.pi * (f0 * t + 0.5 * k * t * t)
+x = np.cos(phase)
+y = sig.sosfilt(sos_butter8_lp, x)
+for idx in [10, 50, 100, 250, 499]:
+ print(f" y[{idx:3d}] = {float(y[idx])!r}")
+print()
+
+
+# ---------- Notch -----------------------------------------------------------
+
+# Used by:
+# BiquadFilterTest.Notch60Hz (cpp / java)
+# BiquadFilterDesignTest.Notch60HzMatchesScipy
+# test_biquad_filter.test_notch_60hz / test_notch_factory_matches_scipy
+b, a = sig.iirnotch(60.0, Q=10.0, fs=1000.0)
+sos_notch = sig.tf2sos(b, a)
+_print_sos("scipy.signal.iirnotch(60.0, Q=10.0, fs=1000.0) via tf2sos", sos_notch)
+
+# Notch time-series spot samples. NOTE: ULP-drift from inline values.
+print("# Notch60Hz time-series spot samples")
+print("# (sin(2π·10·t) + sin(2π·60·t), N=1000, fs=1000, then sosfilt)")
+N = 1000
+fs = 1000.0
+n = np.arange(N)
+t = n / fs
+x = np.sin(2.0 * np.pi * 10.0 * t) + np.sin(2.0 * np.pi * 60.0 * t)
+y = sig.sosfilt(sos_notch, x)
+print(f" y[500] = {float(y[500])!r}")
+print(f" y[999] = {float(y[999])!r}")
+print()
+
+
+# ---------- Chebyshev type I ------------------------------------------------
+
+# Used by:
+# BiquadFilterChebyshevTest.Cheby1LowPass4thOrderMatchesScipy (cpp / java)
+# test_biquad_filter.test_chebyshev1_factory_matches_scipy
+sos_cheby1_lp = sig.cheby1(4, 1.0, 50.0, btype="low", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.cheby1(4, 1.0, 50.0, btype='low', fs=1000.0, output='sos')",
+ sos_cheby1_lp,
+)
+
+# Used by:
+# BiquadFilterChebyshevTest.Cheby1HighPass4thOrderMatchesScipy (cpp / java)
+sos_cheby1_hp = sig.cheby1(4, 1.0, 100.0, btype="high", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.cheby1(4, 1.0, 100.0, btype='high', fs=1000.0, output='sos')",
+ sos_cheby1_hp,
+)
+
+# Used by:
+# BiquadFilterChebyshevTest.Cheby1BandPass4thOrderMatchesScipy (cpp / java)
+sos_cheby1_bp = sig.cheby1(
+ 4, 1.0, [80.0, 120.0], btype="bandpass", fs=1000.0, output="sos"
+)
+_print_sos(
+ "scipy.signal.cheby1(4, 1.0, [80.0, 120.0], btype='bandpass', "
+ "fs=1000.0, output='sos')",
+ sos_cheby1_bp,
+)
+
+
+# ---------- Chebyshev type II -----------------------------------------------
+
+# Used by:
+# BiquadFilterChebyshevTest.Cheby2LowPass4thOrderMatchesScipy (cpp / java)
+# test_biquad_filter.test_chebyshev2_factory_matches_scipy
+sos_cheby2_lp = sig.cheby2(4, 40.0, 50.0, btype="low", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.cheby2(4, 40.0, 50.0, btype='low', fs=1000.0, output='sos')",
+ sos_cheby2_lp,
+)
+
+
+# ---------- Elliptic --------------------------------------------------------
+
+# Used by:
+# BiquadFilterEllipticTest.LowPass4thOrderMatchesScipy (cpp / java)
+# test_biquad_filter.test_elliptic_factory_matches_scipy
+sos_ellip_lp = sig.ellip(4, 1.0, 40.0, 50.0, btype="low", fs=1000.0, output="sos")
+_print_sos(
+ "scipy.signal.ellip(4, 1.0, 40.0, 50.0, btype='low', fs=1000.0, output='sos')",
+ sos_ellip_lp,
+)
+
+# Used by:
+# BiquadFilterEllipticTest.BandPass4thOrderMatchesScipy (cpp / java)
+# test_biquad_filter.test_elliptic_bandpass_factory_matches_scipy
+sos_ellip_bp = sig.ellip(
+ 4, 1.0, 40.0, [80.0, 120.0], btype="bandpass", fs=1000.0, output="sos"
+)
+_print_sos(
+ "scipy.signal.ellip(4, 1.0, 40.0, [80.0, 120.0], btype='bandpass', "
+ "fs=1000.0, output='sos')",
+ sos_ellip_bp,
+)
+
+
+# ---------- First-order-as-biquad cross-check -------------------------------
+
+# Used by:
+# BiquadFilterTest.FirstOrderMatchesSinglePoleIIR (cpp / java)
+# Not a scipy value — just the analytic conversion of SinglePoleIIR's pole
+# coefficient. Documented here so the conversion is in one place.
+print("# FirstOrderMatchesSinglePoleIIR: SinglePoleIIR(T=0.015915, dt=0.005)")
+print("# y[n] = (1-g) x[n] + g y[n-1], g = exp(-dt/T)")
+print("# Equivalent biquad section: (1-g, 0, 0, -g, 0)")
+T = 0.015915
+dt = 0.005
+g = math.exp(-dt / T)
+print(f" g = {g!r}")
+print(f" section = ({1.0 - g!r}, 0.0, 0.0, {-g!r}, 0.0)")
diff --git a/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterChebyshevTest.java b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterChebyshevTest.java
new file mode 100644
index 0000000000..8f56786f4a
--- /dev/null
+++ b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterChebyshevTest.java
@@ -0,0 +1,276 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class BiquadFilterChebyshevTest {
+ private static double cascadeMagnitude(BiquadFilter.Section[] sos, double f, double fs) {
+ double w = 2.0 * Math.PI * f / fs;
+ double cos1 = Math.cos(w);
+ double sin1 = -Math.sin(w);
+ double cos2 = Math.cos(2.0 * w);
+ double sin2 = -Math.sin(2.0 * w);
+ double hRe = 1.0;
+ double hIm = 0.0;
+ for (BiquadFilter.Section s : sos) {
+ double numRe = s.b0 + s.b1 * cos1 + s.b2 * cos2;
+ double numIm = s.b1 * sin1 + s.b2 * sin2;
+ double denRe = 1.0 + s.a1 * cos1 + s.a2 * cos2;
+ double denIm = s.a1 * sin1 + s.a2 * sin2;
+ double denMag = denRe * denRe + denIm * denIm;
+ double tmpRe = (numRe * denRe + numIm * denIm) / denMag;
+ double tmpIm = (numIm * denRe - numRe * denIm) / denMag;
+ double newRe = hRe * tmpRe - hIm * tmpIm;
+ double newIm = hRe * tmpIm + hIm * tmpRe;
+ hRe = newRe;
+ hIm = newIm;
+ }
+ return Math.hypot(hRe, hIm);
+ }
+
+ private static void expectSectionNear(
+ BiquadFilter.Section got, BiquadFilter.Section want, double tol) {
+ assertEquals(want.b0, got.b0, tol, "b0");
+ assertEquals(want.b1, got.b1, tol, "b1");
+ assertEquals(want.b2, got.b2, tol, "b2");
+ assertEquals(want.a1, got.a1, tol, "a1");
+ assertEquals(want.a2, got.a2, tol, "a2");
+ }
+
+ // ----- Chebyshev type I --------------------------------------------------
+
+ @Test
+ void cheby1LowPass4thOrderMatchesScipy() {
+ // scipy.signal.cheby1(4, 1.0, 50.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter = BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 1.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(2, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.00012984963538691335,
+ 0.0002596992707738267,
+ 0.00012984963538691335,
+ -1.7831991339963722,
+ 0.8083720161261031),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.8246970351326663, 0.917300614770565),
+ 1e-10);
+ }
+
+ @Test
+ void cheby1HighPass4thOrderMatchesScipy() {
+ // scipy.signal.cheby1(4, 1.0, 100.0, btype='high', fs=1000.0, output='sos')
+ BiquadFilter filter =
+ BiquadFilter.chebyshevI(BiquadFilter.Kind.HighPass, 4, 1000.0, 100.0, 1.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(2, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.3439348735216468,
+ -0.6878697470432936,
+ 0.3439348735216468,
+ -0.5756927885601547,
+ 0.2749869650540311),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, -2.0, 1.0, -1.4896289697923346, 0.8466697013585162),
+ 1e-10);
+ }
+
+ @Test
+ void cheby1BandPass4thOrderMatchesScipy() {
+ // scipy.signal.cheby1(4, 1.0, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ BiquadFilter filter =
+ BiquadFilter.chebyshevI(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0, 1.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(4, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 5.463638752463053e-05,
+ 0.00010927277504926106,
+ 5.463638752463053e-05,
+ -1.4985467271298947,
+ 0.9129301418072939),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.6224939133759921, 0.9242414431352561),
+ 1e-10);
+ expectSectionNear(
+ sections[2],
+ new BiquadFilter.Section(1.0, -2.0, 1.0, -1.4320495577056345, 0.9601480937923097),
+ 1e-10);
+ expectSectionNear(
+ sections[3],
+ new BiquadFilter.Section(1.0, -2.0, 1.0, -1.7261705273848356, 0.9716328706093393),
+ 1e-10);
+ }
+
+ @Test
+ void cheby1LowPassPassbandStaysWithinRipple() {
+ final double rippleDb = 1.0;
+ BiquadFilter filter =
+ BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, rippleDb);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ // For even order, |H(0)| = 1/sqrt(1+eps^2) — i.e. -ripple dB at DC.
+ double dcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 0.0, 1000.0));
+ assertEquals(-rippleDb, dcDb, 0.01);
+
+ // |H(fc)| = 1/sqrt(1+eps^2) too (ripple boundary).
+ double fcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 50.0, 1000.0));
+ assertEquals(-rippleDb, fcDb, 0.01);
+
+ // Strong attenuation past the cutoff.
+ double stopDb = 20.0 * Math.log10(cascadeMagnitude(sections, 200.0, 1000.0));
+ assertTrue(stopDb < -40.0, "stopband " + stopDb);
+ }
+
+ @Test
+ void cheby1OddOrderHasUnityDcGain() {
+ BiquadFilter filter = BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 5, 1000.0, 50.0, 1.0);
+ double gainDc = cascadeMagnitude(filter.sections(), 0.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-9);
+ }
+
+ @Test
+ void cheby1RejectsInvalidArgs() {
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 0, 1000.0, 50.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 0.0, 50.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 0.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 600.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.BandPass, 4, 1000.0, 120.0, 80.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 0.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, -1.0));
+ }
+
+ // ----- Chebyshev type II -------------------------------------------------
+
+ @Test
+ void cheby2LowPass4thOrderMatchesScipy() {
+ // scipy.signal.cheby2(4, 40.0, 50.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter =
+ BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 40.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(2, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.009735570656077937,
+ -0.01377605024474192,
+ 0.009735570656077937,
+ -1.6993957730842835,
+ 0.7262535657383176),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(
+ 1.0, -1.8857977835164716, 1.0, -1.87354703561714, 0.8977631739778823),
+ 1e-10);
+ }
+
+ @Test
+ void cheby2HighPassResponse() {
+ // For HP/BS, zero pairings can differ from scipy without affecting the
+ // cascade response. Verify the response at points that uniquely
+ // characterize the filter rather than per-section coefficients.
+ final double attenDb = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.chebyshevII(BiquadFilter.Kind.HighPass, 4, 1000.0, 100.0, attenDb);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ double gainHigh = cascadeMagnitude(sections, 400.0, 1000.0);
+ assertEquals(1.0, gainHigh, 1e-3);
+
+ double fcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 100.0, 1000.0));
+ assertTrue(fcDb < -attenDb + 0.01, "fc " + fcDb);
+
+ double dcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 0.0, 1000.0));
+ assertTrue(dcDb < -attenDb + 0.5, "DC " + dcDb);
+ }
+
+ @Test
+ void cheby2BandStopResponse() {
+ final double attenDb = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.chebyshevII(BiquadFilter.Kind.BandStop, 4, 1000.0, 80.0, 120.0, attenDb);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ assertEquals(1.0, cascadeMagnitude(sections, 0.0, 1000.0), 1e-3);
+ assertEquals(1.0, cascadeMagnitude(sections, 400.0, 1000.0), 1e-3);
+
+ double lowEdgeDb = 20.0 * Math.log10(cascadeMagnitude(sections, 80.0, 1000.0));
+ double highEdgeDb = 20.0 * Math.log10(cascadeMagnitude(sections, 120.0, 1000.0));
+ assertTrue(lowEdgeDb < -attenDb + 0.01, "low edge " + lowEdgeDb);
+ assertTrue(highEdgeDb < -attenDb + 0.01, "high edge " + highEdgeDb);
+ }
+
+ @Test
+ void cheby2LowPassFlatPassbandRipplesInStopband() {
+ final double attenDb = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, attenDb);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ // Cheby2 has |H(0)| = 1 always (no DC ripple).
+ assertEquals(1.0, cascadeMagnitude(sections, 0.0, 1000.0), 1e-6);
+
+ double fcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 50.0, 1000.0));
+ assertTrue(fcDb < -attenDb + 0.01, "fc " + fcDb);
+
+ double deepDb = 20.0 * Math.log10(cascadeMagnitude(sections, 100.0, 1000.0));
+ assertTrue(deepDb < -attenDb + 0.5, "deep " + deepDb);
+ }
+
+ @Test
+ void cheby2RejectsInvalidArgs() {
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 0, 1000.0, 50.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 0.0, 50.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 0.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 600.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.BandPass, 4, 1000.0, 120.0, 80.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 0.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, -10.0));
+ }
+}
diff --git a/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterDesignTest.java b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterDesignTest.java
new file mode 100644
index 0000000000..ed25b8440a
--- /dev/null
+++ b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterDesignTest.java
@@ -0,0 +1,300 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class BiquadFilterDesignTest {
+ // |H(e^{j·2π·f/fs})| across a cascade of biquad sections, computed as a real
+ // double via the standard digital-filter cascade-magnitude formula.
+ private static double cascadeMagnitude(BiquadFilter.Section[] sos, double f, double fs) {
+ double w = 2.0 * Math.PI * f / fs;
+ double cos1 = Math.cos(w);
+ double sin1 = -Math.sin(w);
+ double cos2 = Math.cos(2.0 * w);
+ double sin2 = -Math.sin(2.0 * w);
+ double hRe = 1.0;
+ double hIm = 0.0;
+ for (BiquadFilter.Section s : sos) {
+ double numRe = s.b0 + s.b1 * cos1 + s.b2 * cos2;
+ double numIm = s.b1 * sin1 + s.b2 * sin2;
+ double denRe = 1.0 + s.a1 * cos1 + s.a2 * cos2;
+ double denIm = s.a1 * sin1 + s.a2 * sin2;
+ double denMag = denRe * denRe + denIm * denIm;
+ double tmpRe = (numRe * denRe + numIm * denIm) / denMag;
+ double tmpIm = (numIm * denRe - numRe * denIm) / denMag;
+ double newRe = hRe * tmpRe - hIm * tmpIm;
+ double newIm = hRe * tmpIm + hIm * tmpRe;
+ hRe = newRe;
+ hIm = newIm;
+ }
+ return Math.hypot(hRe, hIm);
+ }
+
+ private static void expectSectionNear(
+ BiquadFilter.Section got, BiquadFilter.Section want, double tol) {
+ assertEquals(want.b0, got.b0, tol, "b0");
+ assertEquals(want.b1, got.b1, tol, "b1");
+ assertEquals(want.b2, got.b2, tol, "b2");
+ assertEquals(want.a1, got.a1, tol, "a1");
+ assertEquals(want.a2, got.a2, tol, "a2");
+ }
+
+ @Test
+ void butterworthLowPass4thOrderMatchesScipy() {
+ // scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter = BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(2, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979),
+ 1e-10);
+ }
+
+ @Test
+ void butterworthLowPass8thOrderMatchesScipy() {
+ // scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter = BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 8, 1000.0, 100.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(4, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 2.3959644103776166e-05,
+ 4.791928820755233e-05,
+ 2.3959644103776166e-05,
+ -1.0263514742610553,
+ 0.26864019099379005),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.0868584613628944, 0.343430940165366),
+ 1e-10);
+ expectSectionNear(
+ sections[2],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.2197253651240232, 0.5076634651740437),
+ 1e-10);
+ expectSectionNear(
+ sections[3],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.4515795942478362, 0.794251053241888),
+ 1e-10);
+ }
+
+ @Test
+ void butterworthLowPassCutoffIsMinusThreeDb() {
+ BiquadFilter filter = BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainFc = cascadeMagnitude(sections, 50.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-10);
+ assertEquals(-3.0, 20.0 * Math.log10(gainFc), 0.05);
+ }
+
+ @Test
+ void butterworthHighPassResponse() {
+ BiquadFilter filter = BiquadFilter.butterworth(BiquadFilter.Kind.HighPass, 4, 1000.0, 100.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainFc = cascadeMagnitude(sections, 100.0, 1000.0);
+ double gainHigh = cascadeMagnitude(sections, 400.0, 1000.0);
+ assertEquals(0.0, gainDc, 1e-10);
+ assertEquals(-3.0, 20.0 * Math.log10(gainFc), 0.05);
+ assertEquals(1.0, gainHigh, 1e-3);
+ }
+
+ @Test
+ void butterworthBandPass4thOrderMatchesScipy() {
+ // scipy.signal.butter(4, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ BiquadFilter filter =
+ BiquadFilter.butterworth(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(4, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.0001832160233696091,
+ 0.0003664320467392182,
+ 0.0001832160233696091,
+ -1.395944592254935,
+ 0.7785762494967928),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.5194742571654707, 0.8044610397041421),
+ 1e-10);
+ expectSectionNear(
+ sections[2],
+ new BiquadFilter.Section(1.0, -2.0, 1.0, -1.395095159020637, 0.8950130915917338),
+ 1e-10);
+ expectSectionNear(
+ sections[3],
+ new BiquadFilter.Section(1.0, -2.0, 1.0, -1.678184355447092, 0.9231164780821922),
+ 1e-10);
+ }
+
+ @Test
+ void butterworthBandPassResponse() {
+ BiquadFilter filter =
+ BiquadFilter.butterworth(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainCenter = cascadeMagnitude(sections, 100.0, 1000.0);
+ double gainNyquist = cascadeMagnitude(sections, 499.0, 1000.0);
+ assertTrue(gainDc < 1e-6, "DC gain " + gainDc);
+ assertTrue(gainNyquist < 1e-6, "Nyquist gain " + gainNyquist);
+ assertTrue(gainCenter > 0.8, "center gain " + gainCenter);
+ }
+
+ @Test
+ void butterworthBandStopResponse() {
+ BiquadFilter filter =
+ BiquadFilter.butterworth(BiquadFilter.Kind.BandStop, 4, 1000.0, 80.0, 120.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainCenter = cascadeMagnitude(sections, Math.sqrt(80.0 * 120.0), 1000.0);
+ double gainNyquist = cascadeMagnitude(sections, 499.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-3);
+ assertEquals(1.0, gainNyquist, 1e-3);
+ assertTrue(gainCenter < 1e-6, "center gain " + gainCenter);
+ }
+
+ @Test
+ void butterworthRejectsInvalidArgs() {
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 0, 1000.0, 50.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 0.0, 50.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 0.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 500.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 600.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.BandPass, 4, 1000.0, 120.0, 80.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 80.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.butterworth(BiquadFilter.Kind.BandStop, 4, 1000.0, 80.0, 500.0));
+ }
+
+ @Test
+ void notch60HzMatchesScipy() {
+ // scipy.signal.iirnotch(60.0, Q=10.0, fs=1000.0)
+ BiquadFilter filter = BiquadFilter.notch(1000.0, 60.0, 10.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(1, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.9814970254751076,
+ -1.8251457105120343,
+ 0.9814970254751076,
+ -1.8251457105120341,
+ 0.9629940509502151),
+ 1e-12);
+ }
+
+ @Test
+ void notchAttenuatesAtCenter() {
+ BiquadFilter filter = BiquadFilter.notch(1000.0, 60.0, 10.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainNotch = cascadeMagnitude(sections, 60.0, 1000.0);
+ double gainFar = cascadeMagnitude(sections, 200.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-6);
+ assertTrue(gainNotch < 1e-6, "notch gain " + gainNotch);
+ assertEquals(1.0, gainFar, 0.05);
+ }
+
+ @Test
+ void notchRejectsInvalidArgs() {
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(0.0, 60.0, 10.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(-1000.0, 60.0, 10.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(1000.0, 0.0, 10.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(1000.0, 500.0, 10.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(1000.0, 600.0, 10.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(1000.0, 60.0, 0.0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.notch(1000.0, 60.0, -1.0));
+ }
+
+ @Test
+ void movingAverageSingleTapIsPassThrough() {
+ BiquadFilter filter = BiquadFilter.movingAverage(1);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(1, sections.length);
+ expectSectionNear(sections[0], new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0), 0.0);
+ }
+
+ @Test
+ void movingAverageEvenLengthHasUnitDcGainAndNyquistNull() {
+ BiquadFilter filter = BiquadFilter.movingAverage(4);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, 1000.0);
+ double gainNyquist = cascadeMagnitude(sections, 500.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-12);
+ assertTrue(gainNyquist < 1e-12, "Nyquist gain " + gainNyquist);
+ }
+
+ @Test
+ void movingAverageOddLengthNullsAtFsOverN() {
+ final double fs = 1000.0;
+ final int n = 5;
+ BiquadFilter filter = BiquadFilter.movingAverage(n);
+ BiquadFilter.Section[] sections = filter.sections();
+ double gainDc = cascadeMagnitude(sections, 0.0, fs);
+ double gainNull = cascadeMagnitude(sections, fs / n, fs);
+ double gainHalfNull = cascadeMagnitude(sections, fs / (2.0 * n), fs);
+ assertEquals(1.0, gainDc, 1e-12);
+ assertTrue(gainNull < 1e-10, "null gain " + gainNull);
+ assertTrue(gainHalfNull > 0.1, "half-null gain " + gainHalfNull);
+ }
+
+ @Test
+ void movingAverageMatchesSumAverageImpulseResponse() {
+ final int n = 7;
+ BiquadFilter filter = BiquadFilter.movingAverage(n);
+
+ double[] out = new double[n + 3];
+ for (int i = 0; i < out.length; i++) {
+ double x = i == 0 ? 1.0 : 0.0;
+ out[i] = filter.calculate(x);
+ }
+ for (int i = 0; i < n; i++) {
+ assertEquals(1.0 / n, out[i], 1e-12, "tap " + i);
+ }
+ for (int i = n; i < out.length; i++) {
+ assertEquals(0.0, out[i], 1e-12, "post-window " + i);
+ }
+ }
+
+ @Test
+ void movingAverageRejectsInvalidArgs() {
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.movingAverage(0));
+ assertThrows(IllegalArgumentException.class, () -> BiquadFilter.movingAverage(-1));
+ }
+}
diff --git a/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterEllipticTest.java b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterEllipticTest.java
new file mode 100644
index 0000000000..c32836d20a
--- /dev/null
+++ b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterEllipticTest.java
@@ -0,0 +1,209 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import org.junit.jupiter.api.Test;
+
+class BiquadFilterEllipticTest {
+ private static double cascadeMagnitude(BiquadFilter.Section[] sos, double f, double fs) {
+ double w = 2.0 * Math.PI * f / fs;
+ double cos1 = Math.cos(w);
+ double sin1 = -Math.sin(w);
+ double cos2 = Math.cos(2.0 * w);
+ double sin2 = -Math.sin(2.0 * w);
+ double hRe = 1.0;
+ double hIm = 0.0;
+ for (BiquadFilter.Section s : sos) {
+ double numRe = s.b0 + s.b1 * cos1 + s.b2 * cos2;
+ double numIm = s.b1 * sin1 + s.b2 * sin2;
+ double denRe = 1.0 + s.a1 * cos1 + s.a2 * cos2;
+ double denIm = s.a1 * sin1 + s.a2 * sin2;
+ double denMag = denRe * denRe + denIm * denIm;
+ double tmpRe = (numRe * denRe + numIm * denIm) / denMag;
+ double tmpIm = (numIm * denRe - numRe * denIm) / denMag;
+ double newRe = hRe * tmpRe - hIm * tmpIm;
+ double newIm = hRe * tmpIm + hIm * tmpRe;
+ hRe = newRe;
+ hIm = newIm;
+ }
+ return Math.hypot(hRe, hIm);
+ }
+
+ private static void expectSectionNear(
+ BiquadFilter.Section got, BiquadFilter.Section want, double tol) {
+ assertEquals(want.b0, got.b0, tol, "b0");
+ assertEquals(want.b1, got.b1, tol, "b1");
+ assertEquals(want.b2, got.b2, tol, "b2");
+ assertEquals(want.a1, got.a1, tol, "a1");
+ assertEquals(want.a2, got.a2, tol, "a2");
+ }
+
+ @Test
+ void lowPass4thOrderMatchesScipy() {
+ // scipy.signal.ellip(4, 1.0, 40.0, 50.0, btype='low', fs=1000.0)
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 1.0, 40.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(2, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.011738158079014929,
+ -0.01231742214386518,
+ 0.011738158079014929,
+ -1.7624726990429698,
+ 0.7947551993829407),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(
+ 1.0, -1.7559103274197139, 1.0, -1.8423125689214854, 0.9369806105943849),
+ 1e-10);
+ }
+
+ @Test
+ void lowPassOddOrder5HasFirstOrderSection() {
+ // Odd order means one first-order-as-biquad section in the cascade (a2 = 0
+ // and b2 = 0 for that section). scipy emits 3 sections — verify count and
+ // shape rather than coefficient-by-coefficient, because section ordering
+ // and zero pairing have the same scipy-vs-ours freedom as Butterworth BP.
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 5, 1000.0, 50.0, 1.0, 40.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(3, sections.length);
+
+ int firstOrder = 0;
+ for (BiquadFilter.Section s : sections) {
+ if (s.a2 == 0.0 && s.b2 == 0.0) {
+ firstOrder++;
+ }
+ }
+ assertEquals(1, firstOrder);
+ }
+
+ @Test
+ void lowPassEquirippleInPassbandAndStopband() {
+ final double ripple = 1.0;
+ final double atten = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, ripple, atten);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ double dcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 0.0, 1000.0));
+ double fcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 50.0, 1000.0));
+ assertEquals(-ripple, dcDb, 0.01);
+ assertEquals(-ripple, fcDb, 0.01);
+
+ double stopDb = 20.0 * Math.log10(cascadeMagnitude(sections, 100.0, 1000.0));
+ assertTrue(stopDb < -atten + 0.5, "stop " + stopDb);
+ }
+
+ @Test
+ void oddOrderHasUnityDcGain() {
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 5, 1000.0, 50.0, 1.0, 40.0);
+ double gainDc = cascadeMagnitude(filter.sections(), 0.0, 1000.0);
+ assertEquals(1.0, gainDc, 1e-9);
+ }
+
+ @Test
+ void highPassResponse() {
+ final double ripple = 1.0;
+ final double atten = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.HighPass, 4, 1000.0, 100.0, ripple, atten);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ double passbandDb = 20.0 * Math.log10(cascadeMagnitude(sections, 400.0, 1000.0));
+ assertEquals(0.0, passbandDb, ripple + 0.01);
+
+ double cutoffDb = 20.0 * Math.log10(cascadeMagnitude(sections, 100.0, 1000.0));
+ assertEquals(-ripple, cutoffDb, 0.01);
+
+ double dcDb = 20.0 * Math.log10(cascadeMagnitude(sections, 0.0, 1000.0));
+ assertTrue(dcDb < -atten + 0.5, "DC " + dcDb);
+ }
+
+ @Test
+ void bandPass4thOrderMatchesScipy() {
+ // scipy.signal.ellip(4, 1.0, 40.0, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0, 1.0, 40.0);
+ BiquadFilter.Section[] sections = filter.sections();
+ assertEquals(4, sections.length);
+ expectSectionNear(
+ sections[0],
+ new BiquadFilter.Section(
+ 0.010903156756394984,
+ -0.008920205787636758,
+ 0.010903156756394982,
+ -1.4809043488404827,
+ 0.9052184223450329),
+ 1e-10);
+ expectSectionNear(
+ sections[1],
+ new BiquadFilter.Section(
+ 1.0, -1.9038045463534676, 0.9999999999999999, -1.62699499510272, 0.9194678402475894),
+ 1e-10);
+ expectSectionNear(
+ sections[2],
+ new BiquadFilter.Section(
+ 1.0, -1.3265553048553793, 1.0, -1.4370735618061194, 0.9697500844409095),
+ 1e-10);
+ expectSectionNear(
+ sections[3],
+ new BiquadFilter.Section(
+ 1.0, -1.8057300347135379, 0.9999999999999998, -1.733243724674222, 0.978571861817194),
+ 1e-10);
+ }
+
+ @Test
+ void bandPassResponse() {
+ final double ripple = 1.0;
+ final double atten = 40.0;
+ BiquadFilter filter =
+ BiquadFilter.elliptic(BiquadFilter.Kind.BandPass, 3, 1000.0, 80.0, 120.0, ripple, atten);
+ BiquadFilter.Section[] sections = filter.sections();
+
+ double centerDb = 20.0 * Math.log10(cascadeMagnitude(sections, 100.0, 1000.0));
+ assertEquals(0.0, centerDb, ripple + 0.01);
+
+ double belowDb = 20.0 * Math.log10(cascadeMagnitude(sections, 0.0, 1000.0));
+ double aboveDb = 20.0 * Math.log10(cascadeMagnitude(sections, 400.0, 1000.0));
+ assertTrue(belowDb < -atten + 1.0, "below " + belowDb);
+ assertTrue(aboveDb < -atten + 1.0, "above " + aboveDb);
+ }
+
+ @Test
+ void rejectsInvalidArgs() {
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 0, 1000.0, 50.0, 1.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 0.0, 50.0, 1.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 0.0, 40.0));
+ // Stopband must be deeper than passband ripple.
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 40.0, 1.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 5.0, 5.0));
+ // Frequencies out of range / inverted.
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.LowPass, 4, 1000.0, 600.0, 1.0, 40.0));
+ assertThrows(
+ IllegalArgumentException.class,
+ () -> BiquadFilter.elliptic(BiquadFilter.Kind.BandPass, 4, 1000.0, 120.0, 80.0, 1.0, 40.0));
+ }
+}
diff --git a/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterTest.java b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterTest.java
new file mode 100644
index 0000000000..cb6ec189c7
--- /dev/null
+++ b/wpimath/src/test/java/org/wpilib/math/filter/BiquadFilterTest.java
@@ -0,0 +1,282 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.wpilib.math.filter;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+import static org.junit.jupiter.api.Assertions.assertNotEquals;
+import static org.junit.jupiter.api.Assertions.assertThrows;
+import static org.junit.jupiter.api.Assertions.assertTrue;
+
+import java.util.Random;
+import org.junit.jupiter.api.Test;
+
+class BiquadFilterTest {
+ @Test
+ void passThroughTest() {
+ BiquadFilter filter = new BiquadFilter(new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0));
+ Random rng = new Random(42);
+ for (int i = 0; i < 200; i++) {
+ double x = (rng.nextDouble() - 0.5) * 200.0;
+ assertEquals(x, filter.calculate(x), 0.0);
+ }
+ }
+
+ @Test
+ void firstOrderMatchesSinglePoleIIRTest() {
+ // SinglePoleIIR: y[n] = (1-g) x[n] + g y[n-1], g = exp(-dt/T)
+ // As biquad: {1-g, 0, 0, -g, 0}
+ final double timeConstant = 0.015915;
+ final double period = 0.005;
+ final double g = Math.exp(-period / timeConstant);
+
+ LinearFilter linear = LinearFilter.singlePoleIIR(timeConstant, period);
+ BiquadFilter biquad = new BiquadFilter(new BiquadFilter.Section(1.0 - g, 0.0, 0.0, -g, 0.0));
+
+ Random rng = new Random(7);
+ for (int i = 0; i < 1000; i++) {
+ double x = (rng.nextDouble() - 0.5) * 100.0;
+ double yLin = linear.calculate(x);
+ double yBiq = biquad.calculate(x);
+ assertEquals(yLin, yBiq, 1e-12, "sample " + i);
+ }
+ }
+
+ @Test
+ void butterworth4thOrderLowPassTest() {
+ // scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979));
+
+ double[] expected = {
+ 0.00041659920440659937,
+ 0.0029914483065925663,
+ 0.010405740533503665,
+ 0.024092655231875183,
+ 0.04300386328531425,
+ 0.06442081415630327,
+ 0.08518000836484753,
+ 0.10245740377665029,
+ 0.1142030744642985,
+ 0.11931076610150239,
+ 0.11759868177262096,
+ 0.10966797135549569,
+ 0.09669445862739445,
+ 0.08019770689053446,
+ 0.06182021082200691,
+ 0.04313888252371942,
+ 0.025521549440937964,
+ 0.01003324447867498,
+ -0.002609160074363505,
+ -0.01203989315971688,
+ -0.018213508615082776,
+ -0.021350392922805498,
+ -0.02186860712506684,
+ -0.020311212598085788,
+ -0.01727668431352673,
+ -0.013358111475758645,
+ -0.009094876704322833,
+ -0.004938595712161598,
+ -0.0012334395430879353,
+ 0.0017903545884787877,
+ };
+
+ for (int i = 0; i < expected.length; i++) {
+ double x = (i == 0) ? 1.0 : 0.0;
+ double y = filter.calculate(x);
+ assertEquals(expected[i], y, 1e-10, "sample " + i);
+ }
+ }
+
+ @Test
+ void notch60HzTest() {
+ // scipy.signal.iirnotch(60, Q=10, fs=1000) via tf2sos
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 0.9814970254751076,
+ -1.8251457105120343,
+ 0.9814970254751076,
+ -1.8251457105120341,
+ 0.9629940509502151));
+
+ final double fs = 1000.0;
+ final int samples = 1000;
+ double[] output = new double[samples];
+ double[] input = new double[samples];
+ for (int n = 0; n < samples; n++) {
+ double t = n / fs;
+ input[n] = Math.sin(2.0 * Math.PI * 10.0 * t) + Math.sin(2.0 * Math.PI * 60.0 * t);
+ output[n] = filter.calculate(input[n]);
+ }
+
+ assertEquals(-0.017355123579818322, output[500], 1e-10);
+ assertEquals(-0.08007594066581347, output[999], 1e-10);
+
+ // Basic DFT at 10 Hz and 60 Hz over steady-state window.
+ final int window = 512;
+ double atten60 = attenuationDb(output, input, 60.0, fs, samples - window, window);
+ double atten10 = attenuationDb(output, input, 10.0, fs, samples - window, window);
+
+ assertTrue(atten60 < -40.0, "60 Hz attenuation too weak: " + atten60);
+ assertTrue(atten10 > -0.5, "10 Hz passband loss too large: " + atten10);
+ }
+
+ @Test
+ void order8ButterworthMatchesScipyTest() {
+ // scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 2.3959644103776166e-05,
+ 4.791928820755233e-05,
+ 2.3959644103776166e-05,
+ -1.0263514742610553,
+ 0.26864019099379005),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.0868584613628944, 0.343430940165366),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.2197253651240232, 0.5076634651740437),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.4515795942478362, 0.794251053241888));
+
+ final int N = 500;
+ final double fs = 1000.0;
+ final double f0 = 1.0;
+ final double f1 = 200.0;
+ final double t1 = (N - 1) / fs;
+ final double k = (f1 - f0) / t1;
+
+ int[] spotIdx = {10, 50, 100, 250, 499};
+ double[] expected = {
+ 0.8950675041062186,
+ -0.7902247252134351,
+ 0.1716891991372734,
+ 0.05240058121316523,
+ -0.0016952227415119995,
+ };
+ double[] got = new double[spotIdx.length];
+
+ int j = 0;
+ for (int n = 0; n < N; n++) {
+ double t = n / fs;
+ double phase = 2.0 * Math.PI * (f0 * t + 0.5 * k * t * t);
+ double x = Math.cos(phase);
+ double y = filter.calculate(x);
+ if (j < spotIdx.length && n == spotIdx[j]) {
+ got[j++] = y;
+ }
+ }
+
+ for (int i = 0; i < expected.length; i++) {
+ assertEquals(expected[i], got[i], 1e-10, "sample index " + spotIdx[i]);
+ }
+ }
+
+ @Test
+ void resetZerosStateTest() {
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979));
+
+ for (int i = 0; i < 50; i++) {
+ filter.calculate(1.0);
+ }
+ assertNotEquals(0.0, filter.lastValue());
+
+ filter.reset();
+ assertEquals(0.0, filter.lastValue(), 0.0);
+
+ double y = filter.calculate(1.0);
+ assertEquals(0.00041659920440659937, y, 1e-12);
+ }
+
+ @Test
+ void resetToSteadyStateTest() {
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979));
+
+ final double input = 3.0;
+ filter.reset(input);
+
+ assertEquals(input, filter.lastValue(), 1e-12);
+ assertEquals(input, filter.calculate(input), 1e-12);
+ for (int i = 0; i < 20; i++) {
+ assertEquals(input, filter.calculate(input), 1e-12);
+ }
+ }
+
+ @Test
+ void dcGainConvergesTest() {
+ BiquadFilter filter =
+ new BiquadFilter(
+ new BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889),
+ new BiquadFilter.Section(1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979));
+
+ final double input = 2.5;
+ double y = 0.0;
+ for (int i = 0; i < 500; i++) {
+ y = filter.calculate(input);
+ }
+ assertEquals(input, y, 1e-6);
+ }
+
+ @Test
+ void numSectionsTest() {
+ BiquadFilter one = new BiquadFilter(new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0));
+ assertEquals(1, one.numSections());
+
+ BiquadFilter three =
+ new BiquadFilter(
+ new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0),
+ new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0),
+ new BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0));
+ assertEquals(3, three.numSections());
+ }
+
+ @Test
+ void emptyCascadeThrowsTest() {
+ assertThrows(IllegalArgumentException.class, () -> new BiquadFilter());
+ }
+
+ private static double attenuationDb(
+ double[] out, double[] in, double freq, double fs, int start, int window) {
+ double magOut = dftMag(out, freq, fs, start, window);
+ double magIn = dftMag(in, freq, fs, start, window);
+ return 20.0 * Math.log10(magOut / magIn);
+ }
+
+ private static double dftMag(double[] sig, double freq, double fs, int start, int window) {
+ double re = 0.0;
+ double im = 0.0;
+ for (int n = 0; n < window; n++) {
+ double phase = 2.0 * Math.PI * freq * n / fs;
+ re += sig[start + n] * Math.cos(phase);
+ im -= sig[start + n] * Math.sin(phase);
+ }
+ return Math.hypot(re, im);
+ }
+}
diff --git a/wpimath/src/test/native/cpp/filter/BiquadFilterChebyshevTest.cpp b/wpimath/src/test/native/cpp/filter/BiquadFilterChebyshevTest.cpp
new file mode 100644
index 0000000000..4493c668a2
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/BiquadFilterChebyshevTest.cpp
@@ -0,0 +1,257 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "wpi/math/filter/BiquadFilter.hpp"
+#include "wpi/units/frequency.hpp"
+
+namespace {
+
+using wpi::math::BiquadFilter;
+using Section = BiquadFilter::Section;
+using Kind = BiquadFilter::Kind;
+
+double CascadeMagnitude(std::span sos, double f, double fs) {
+ const double w = 2.0 * std::numbers::pi * f / fs;
+ const std::complex z1 = std::polar(1.0, -w);
+ const std::complex z2 = std::polar(1.0, -2.0 * w);
+ std::complex h{1.0, 0.0};
+ for (const auto& s : sos) {
+ std::complex num = s.b0 + s.b1 * z1 + s.b2 * z2;
+ std::complex den = 1.0 + s.a1 * z1 + s.a2 * z2;
+ h *= num / den;
+ }
+ return std::abs(h);
+}
+
+void ExpectSectionNear(const Section& got, const Section& want, double tol) {
+ EXPECT_NEAR(got.b0, want.b0, tol);
+ EXPECT_NEAR(got.b1, want.b1, tol);
+ EXPECT_NEAR(got.b2, want.b2, tol);
+ EXPECT_NEAR(got.a1, want.a1, tol);
+ EXPECT_NEAR(got.a2, want.a2, tol);
+}
+
+// ----- Chebyshev type I ----------------------------------------------------
+
+TEST(BiquadFilterChebyshevTest, Cheby1LowPass4thOrderMatchesScipy) {
+ // scipy.signal.cheby1(4, 1.0, 50.0, btype='low', fs=1000.0, output='sos')
+ auto filter =
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 1.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 2u);
+ ExpectSectionNear(
+ sections[0],
+ {0.00012984963538691335, 0.0002596992707738267, 0.00012984963538691335,
+ -1.7831991339963722, 0.8083720161261031},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, 2.0, 1.0, -1.8246970351326663, 0.917300614770565},
+ 1e-10);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby1HighPass4thOrderMatchesScipy) {
+ // scipy.signal.cheby1(4, 1.0, 100.0, btype='high', fs=1000.0, output='sos')
+ auto filter =
+ BiquadFilter::ChebyshevI(Kind::HighPass, 4, 1000.0_Hz, 100.0_Hz, 1.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 2u);
+ ExpectSectionNear(
+ sections[0],
+ {0.3439348735216468, -0.6878697470432936, 0.3439348735216468,
+ -0.5756927885601547, 0.2749869650540311},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, -2.0, 1.0, -1.4896289697923346, 0.8466697013585162},
+ 1e-10);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby1BandPass4thOrderMatchesScipy) {
+ // scipy.signal.cheby1(4, 1.0, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ auto filter = BiquadFilter::ChebyshevI(Kind::BandPass, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz, 1.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 4u);
+ ExpectSectionNear(
+ sections[0],
+ {5.463638752463053e-05, 0.00010927277504926106, 5.463638752463053e-05,
+ -1.4985467271298947, 0.9129301418072939},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, 2.0, 1.0, -1.6224939133759921, 0.9242414431352561},
+ 1e-10);
+ ExpectSectionNear(sections[2],
+ {1.0, -2.0, 1.0, -1.4320495577056345, 0.9601480937923097},
+ 1e-10);
+ ExpectSectionNear(sections[3],
+ {1.0, -2.0, 1.0, -1.7261705273848356, 0.9716328706093393},
+ 1e-10);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby1LowPassPassbandStaysWithinRipple) {
+ constexpr double kRippleDb = 1.0;
+ auto filter =
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, kRippleDb);
+ auto sections = filter.Sections();
+
+ // For even order, |H(0)| = 1/sqrt(1+eps^2) — i.e. -ripple dB at DC.
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double dcDb = 20.0 * std::log10(gainDc);
+ EXPECT_NEAR(dcDb, -kRippleDb, 0.01);
+
+ // |H(fc)| = 1/sqrt(1+eps^2) too (ripple boundary).
+ double gainFc = CascadeMagnitude(sections, 50.0, 1000.0);
+ double fcDb = 20.0 * std::log10(gainFc);
+ EXPECT_NEAR(fcDb, -kRippleDb, 0.01);
+
+ // Strong attenuation past the cutoff.
+ double gainStop = CascadeMagnitude(sections, 200.0, 1000.0);
+ EXPECT_LT(20.0 * std::log10(gainStop), -40.0);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby1OddOrderHasUnityDcGain) {
+ // For odd order the ripple boundary touches |H(0)| = 1 exactly.
+ auto filter =
+ BiquadFilter::ChebyshevI(Kind::LowPass, 5, 1000.0_Hz, 50.0_Hz, 1.0);
+ double gainDc = CascadeMagnitude(filter.Sections(), 0.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-9);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby1RejectsInvalidArgs) {
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevI(Kind::LowPass, 0, 1000.0_Hz, 50.0_Hz, 1.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::ChebyshevI(Kind::LowPass, 4, 0.0_Hz, 50.0_Hz, 1.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 0.0_Hz, 1.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 600.0_Hz, 1.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::ChebyshevI(Kind::BandPass, 4, 1000.0_Hz, 120.0_Hz,
+ 80.0_Hz, 1.0),
+ std::invalid_argument);
+ // Ripple must be strictly positive.
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 0.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevI(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, -1.0),
+ std::invalid_argument);
+}
+
+// ----- Chebyshev type II ---------------------------------------------------
+
+TEST(BiquadFilterChebyshevTest, Cheby2LowPass4thOrderMatchesScipy) {
+ // scipy.signal.cheby2(4, 40.0, 50.0, btype='low', fs=1000.0, output='sos')
+ auto filter =
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 40.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 2u);
+ ExpectSectionNear(
+ sections[0],
+ {0.009735570656077937, -0.01377605024474192, 0.009735570656077937,
+ -1.6993957730842835, 0.7262535657383176},
+ 1e-10);
+ ExpectSectionNear(
+ sections[1],
+ {1.0, -1.8857977835164716, 1.0, -1.87354703561714, 0.8977631739778823},
+ 1e-10);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby2HighPassResponse) {
+ // For HP/BS, zero pairings can differ from scipy without affecting the
+ // cascade response (same caveat as Butterworth BP/BS). Verify the response
+ // at points that uniquely characterize the filter rather than per-section
+ // coefficients.
+ constexpr double kAttenDb = 40.0;
+ auto filter = BiquadFilter::ChebyshevII(Kind::HighPass, 4, 1000.0_Hz,
+ 100.0_Hz, kAttenDb);
+ auto sections = filter.Sections();
+
+ // Passband (high frequencies): unity gain.
+ double gainHigh = CascadeMagnitude(sections, 400.0, 1000.0);
+ EXPECT_NEAR(gainHigh, 1.0, 1e-3);
+
+ // Stopband edge fc=100: response reaches the stopband attenuation.
+ double gainFc = CascadeMagnitude(sections, 100.0, 1000.0);
+ EXPECT_LT(20.0 * std::log10(gainFc), -kAttenDb + 0.01);
+
+ // DC: deeply attenuated.
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ EXPECT_LT(20.0 * std::log10(gainDc), -kAttenDb + 0.5);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby2BandStopResponse) {
+ // BandStop: zero pairings differ from scipy in the same way as Butterworth
+ // BS — total response matches but per-section coefficients don't.
+ constexpr double kAttenDb = 40.0;
+ auto filter = BiquadFilter::ChebyshevII(Kind::BandStop, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz, kAttenDb);
+ auto sections = filter.Sections();
+
+ // Outside the stop band: unity gain.
+ EXPECT_NEAR(CascadeMagnitude(sections, 0.0, 1000.0), 1.0, 1e-3);
+ EXPECT_NEAR(CascadeMagnitude(sections, 400.0, 1000.0), 1.0, 1e-3);
+
+ // Stop-band edges hit the attenuation level; deeper inside the band is
+ // at least that attenuated.
+ EXPECT_LT(20.0 * std::log10(CascadeMagnitude(sections, 80.0, 1000.0)),
+ -kAttenDb + 0.01);
+ EXPECT_LT(20.0 * std::log10(CascadeMagnitude(sections, 120.0, 1000.0)),
+ -kAttenDb + 0.01);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby2LowPassFlatPassbandRipplesInStopband) {
+ constexpr double kAttenDb = 40.0;
+ auto filter =
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, kAttenDb);
+ auto sections = filter.Sections();
+
+ // Cheby2 has |H(0)| = 1 always (no DC ripple).
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-6);
+
+ // At the stopband edge fc=50, |H| reaches the stopband attenuation.
+ double gainFc = CascadeMagnitude(sections, 50.0, 1000.0);
+ EXPECT_LT(20.0 * std::log10(gainFc), -kAttenDb + 0.01);
+
+ // Stopband stays at or below kAttenDb attenuation.
+ double gainDeep = CascadeMagnitude(sections, 100.0, 1000.0);
+ EXPECT_LT(20.0 * std::log10(gainDeep), -kAttenDb + 0.5);
+}
+
+TEST(BiquadFilterChebyshevTest, Cheby2RejectsInvalidArgs) {
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 0, 1000.0_Hz, 50.0_Hz, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 0.0_Hz, 50.0_Hz, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 0.0_Hz, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 600.0_Hz, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::ChebyshevII(Kind::BandPass, 4, 1000.0_Hz, 120.0_Hz,
+ 80.0_Hz, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 0.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::ChebyshevII(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, -10.0),
+ std::invalid_argument);
+}
+
+} // namespace
diff --git a/wpimath/src/test/native/cpp/filter/BiquadFilterDesignTest.cpp b/wpimath/src/test/native/cpp/filter/BiquadFilterDesignTest.cpp
new file mode 100644
index 0000000000..6fc08428d0
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/BiquadFilterDesignTest.cpp
@@ -0,0 +1,266 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "wpi/math/filter/BiquadFilter.hpp"
+#include "wpi/units/frequency.hpp"
+
+namespace {
+
+using wpi::math::BiquadFilter;
+using Section = BiquadFilter::Section;
+using Kind = BiquadFilter::Kind;
+
+// |H(e^{j·2π·f/fs})| across a cascade of biquad sections.
+double CascadeMagnitude(std::span sos, double f, double fs) {
+ const double w = 2.0 * std::numbers::pi * f / fs;
+ const std::complex z1 = std::polar(1.0, -w);
+ const std::complex z2 = std::polar(1.0, -2.0 * w);
+ std::complex h{1.0, 0.0};
+ for (const auto& s : sos) {
+ std::complex num = s.b0 + s.b1 * z1 + s.b2 * z2;
+ std::complex den = 1.0 + s.a1 * z1 + s.a2 * z2;
+ h *= num / den;
+ }
+ return std::abs(h);
+}
+
+void ExpectSectionNear(const Section& got, const Section& want, double tol) {
+ EXPECT_NEAR(got.b0, want.b0, tol);
+ EXPECT_NEAR(got.b1, want.b1, tol);
+ EXPECT_NEAR(got.b2, want.b2, tol);
+ EXPECT_NEAR(got.a1, want.a1, tol);
+ EXPECT_NEAR(got.a2, want.a2, tol);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthLowPass4thOrderMatchesScipy) {
+ // scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ auto filter = BiquadFilter::Butterworth(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 2u);
+ ExpectSectionNear(
+ sections[0],
+ {0.00041659920440659937, 0.0008331984088131987, 0.00041659920440659937,
+ -1.4796742169311934, 0.5558215432824889},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979},
+ 1e-10);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthLowPass8thOrderMatchesScipy) {
+ // scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')
+ auto filter =
+ BiquadFilter::Butterworth(Kind::LowPass, 8, 1000.0_Hz, 100.0_Hz);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 4u);
+ ExpectSectionNear(
+ sections[0],
+ {2.3959644103776166e-05, 4.791928820755233e-05, 2.3959644103776166e-05,
+ -1.0263514742610553, 0.26864019099379005},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, 2.0, 1.0, -1.0868584613628944, 0.343430940165366},
+ 1e-10);
+ ExpectSectionNear(sections[2],
+ {1.0, 2.0, 1.0, -1.2197253651240232, 0.5076634651740437},
+ 1e-10);
+ ExpectSectionNear(sections[3],
+ {1.0, 2.0, 1.0, -1.4515795942478362, 0.794251053241888},
+ 1e-10);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthLowPassCutoffIsMinusThreeDb) {
+ auto filter = BiquadFilter::Butterworth(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double gainFc = CascadeMagnitude(sections, 50.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-10);
+ EXPECT_NEAR(20.0 * std::log10(gainFc), -3.0, 0.05);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthHighPassResponse) {
+ auto filter =
+ BiquadFilter::Butterworth(Kind::HighPass, 4, 1000.0_Hz, 100.0_Hz);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double gainFc = CascadeMagnitude(sections, 100.0, 1000.0);
+ double gainHigh = CascadeMagnitude(sections, 400.0, 1000.0);
+ EXPECT_NEAR(gainDc, 0.0, 1e-10);
+ EXPECT_NEAR(20.0 * std::log10(gainFc), -3.0, 0.05);
+ EXPECT_NEAR(gainHigh, 1.0, 1e-3);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthBandPass4thOrderMatchesScipy) {
+ // scipy.signal.butter(4, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ auto filter = BiquadFilter::Butterworth(Kind::BandPass, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 4u);
+ ExpectSectionNear(
+ sections[0],
+ {0.0001832160233696091, 0.0003664320467392182, 0.0001832160233696091,
+ -1.395944592254935, 0.7785762494967928},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, 2.0, 1.0, -1.5194742571654707, 0.8044610397041421},
+ 1e-10);
+ ExpectSectionNear(sections[2],
+ {1.0, -2.0, 1.0, -1.395095159020637, 0.8950130915917338},
+ 1e-10);
+ ExpectSectionNear(sections[3],
+ {1.0, -2.0, 1.0, -1.678184355447092, 0.9231164780821922},
+ 1e-10);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthBandPassResponse) {
+ auto filter = BiquadFilter::Butterworth(Kind::BandPass, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double gainCenter = CascadeMagnitude(sections, 100.0, 1000.0);
+ double gainNyquist = CascadeMagnitude(sections, 499.0, 1000.0);
+ EXPECT_LT(gainDc, 1e-6);
+ EXPECT_LT(gainNyquist, 1e-6);
+ EXPECT_GT(gainCenter, 0.8);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthBandStopResponse) {
+ auto filter = BiquadFilter::Butterworth(Kind::BandStop, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ // Geometric mean is the analog-prototype zero; digital zero is nearby.
+ double gainCenter =
+ CascadeMagnitude(sections, std::sqrt(80.0 * 120.0), 1000.0);
+ double gainNyquist = CascadeMagnitude(sections, 499.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-3);
+ EXPECT_NEAR(gainNyquist, 1.0, 1e-3);
+ EXPECT_LT(gainCenter, 1e-6);
+}
+
+TEST(BiquadFilterDesignTest, ButterworthRejectsInvalidArgs) {
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::LowPass, 0, 1000.0_Hz, 50.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::LowPass, 4, 0.0_Hz, 50.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::LowPass, 4, 1000.0_Hz, 0.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::LowPass, 4, 1000.0_Hz, 500.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::LowPass, 4, 1000.0_Hz, 600.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::BandPass, 4, 1000.0_Hz, 120.0_Hz,
+ 80.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::Butterworth(Kind::BandPass, 4, 1000.0_Hz, 80.0_Hz, 80.0_Hz),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Butterworth(Kind::BandStop, 4, 1000.0_Hz, 80.0_Hz,
+ 500.0_Hz),
+ std::invalid_argument);
+}
+
+TEST(BiquadFilterDesignTest, Notch60HzMatchesScipy) {
+ // scipy.signal.iirnotch(60.0, Q=10.0, fs=1000.0)
+ auto filter = BiquadFilter::Notch(1000.0_Hz, 60.0_Hz, 10.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 1u);
+ ExpectSectionNear(
+ sections[0],
+ {0.9814970254751076, -1.8251457105120343, 0.9814970254751076,
+ -1.8251457105120341, 0.9629940509502151},
+ 1e-12);
+}
+
+TEST(BiquadFilterDesignTest, NotchAttenuatesAtCenter) {
+ auto filter = BiquadFilter::Notch(1000.0_Hz, 60.0_Hz, 10.0);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double gainNotch = CascadeMagnitude(sections, 60.0, 1000.0);
+ double gainFar = CascadeMagnitude(sections, 200.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-6);
+ EXPECT_LT(gainNotch, 1e-6);
+ EXPECT_NEAR(gainFar, 1.0, 0.05);
+}
+
+TEST(BiquadFilterDesignTest, NotchRejectsInvalidArgs) {
+ EXPECT_THROW(BiquadFilter::Notch(0.0_Hz, 60.0_Hz, 10.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(-1000.0_Hz, 60.0_Hz, 10.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(1000.0_Hz, 0.0_Hz, 10.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(1000.0_Hz, 500.0_Hz, 10.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(1000.0_Hz, 600.0_Hz, 10.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(1000.0_Hz, 60.0_Hz, 0.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Notch(1000.0_Hz, 60.0_Hz, -1.0),
+ std::invalid_argument);
+}
+
+TEST(BiquadFilterDesignTest, MovingAverageSingleTapIsPassThrough) {
+ auto filter = BiquadFilter::MovingAverage(1);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 1u);
+ ExpectSectionNear(sections[0], {1.0, 0.0, 0.0, 0.0, 0.0}, 0.0);
+}
+
+TEST(BiquadFilterDesignTest,
+ MovingAverageEvenLengthHasUnitDcGainAndNyquistNull) {
+ auto filter = BiquadFilter::MovingAverage(4);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, 1000.0);
+ double gainNyquist = CascadeMagnitude(sections, 500.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-12);
+ EXPECT_LT(gainNyquist, 1e-12);
+}
+
+TEST(BiquadFilterDesignTest, MovingAverageOddLengthNullsAtFsOverN) {
+ constexpr double kFs = 1000.0;
+ constexpr int kN = 5;
+ auto filter = BiquadFilter::MovingAverage(kN);
+ auto sections = filter.Sections();
+ double gainDc = CascadeMagnitude(sections, 0.0, kFs);
+ double gainNull = CascadeMagnitude(sections, kFs / kN, kFs);
+ double gainHalfNull = CascadeMagnitude(sections, kFs / (2.0 * kN), kFs);
+ EXPECT_NEAR(gainDc, 1.0, 1e-12);
+ EXPECT_LT(gainNull, 1e-10);
+ EXPECT_GT(gainHalfNull, 0.1);
+}
+
+TEST(BiquadFilterDesignTest, MovingAverageMatchesSumAverageImpulseResponse) {
+ // Verify impulse response of the cascade equals 1/N for N samples, then 0.
+ constexpr int kN = 7;
+ auto filter = BiquadFilter::MovingAverage(kN);
+
+ std::array out{};
+ for (size_t i = 0; i < out.size(); ++i) {
+ double x = (i == 0) ? 1.0 : 0.0;
+ out[i] = filter.Calculate(x);
+ }
+ for (int i = 0; i < kN; ++i) {
+ EXPECT_NEAR(out[i], 1.0 / kN, 1e-12) << "tap " << i;
+ }
+ for (size_t i = kN; i < out.size(); ++i) {
+ EXPECT_NEAR(out[i], 0.0, 1e-12) << "post-window " << i;
+ }
+}
+
+TEST(BiquadFilterDesignTest, MovingAverageRejectsInvalidArgs) {
+ EXPECT_THROW(BiquadFilter::MovingAverage(0), std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::MovingAverage(-1), std::invalid_argument);
+}
+
+} // namespace
diff --git a/wpimath/src/test/native/cpp/filter/BiquadFilterEllipticTest.cpp b/wpimath/src/test/native/cpp/filter/BiquadFilterEllipticTest.cpp
new file mode 100644
index 0000000000..dedea58fe3
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/BiquadFilterEllipticTest.cpp
@@ -0,0 +1,197 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "wpi/math/filter/BiquadFilter.hpp"
+#include "wpi/units/frequency.hpp"
+
+namespace {
+
+using wpi::math::BiquadFilter;
+using Section = BiquadFilter::Section;
+using Kind = BiquadFilter::Kind;
+
+double CascadeMagnitude(std::span sos, double f, double fs) {
+ const double w = 2.0 * std::numbers::pi * f / fs;
+ const std::complex z1 = std::polar(1.0, -w);
+ const std::complex z2 = std::polar(1.0, -2.0 * w);
+ std::complex h{1.0, 0.0};
+ for (const auto& s : sos) {
+ std::complex num = s.b0 + s.b1 * z1 + s.b2 * z2;
+ std::complex den = 1.0 + s.a1 * z1 + s.a2 * z2;
+ h *= num / den;
+ }
+ return std::abs(h);
+}
+
+void ExpectSectionNear(const Section& got, const Section& want, double tol) {
+ EXPECT_NEAR(got.b0, want.b0, tol);
+ EXPECT_NEAR(got.b1, want.b1, tol);
+ EXPECT_NEAR(got.b2, want.b2, tol);
+ EXPECT_NEAR(got.a1, want.a1, tol);
+ EXPECT_NEAR(got.a2, want.a2, tol);
+}
+
+TEST(BiquadFilterEllipticTest, LowPass4thOrderMatchesScipy) {
+ // scipy.signal.ellip(4, 1.0, 40.0, 50.0, btype='low', fs=1000.0)
+ auto filter =
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 1.0, 40.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 2u);
+ ExpectSectionNear(
+ sections[0],
+ {0.011738158079014929, -0.01231742214386518, 0.011738158079014929,
+ -1.7624726990429698, 0.7947551993829407},
+ 1e-10);
+ ExpectSectionNear(
+ sections[1],
+ {1.0, -1.7559103274197139, 1.0, -1.8423125689214854, 0.9369806105943849},
+ 1e-10);
+}
+
+TEST(BiquadFilterEllipticTest, LowPassOddOrder5HasFirstOrderSection) {
+ // Odd order means one first-order-as-biquad section in the cascade (a2 = 0
+ // and b2 = 0 for that section). scipy emits 3 sections — verify count and
+ // shape rather than coefficient-by-coefficient, because section ordering
+ // and zero pairing have the same scipy-vs-ours freedom as Butterworth BP.
+ auto filter =
+ BiquadFilter::Elliptic(Kind::LowPass, 5, 1000.0_Hz, 50.0_Hz, 1.0, 40.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 3u);
+
+ int firstOrderSections = 0;
+ for (const auto& s : sections) {
+ if (s.a2 == 0.0 && s.b2 == 0.0) {
+ ++firstOrderSections;
+ }
+ }
+ EXPECT_EQ(firstOrderSections, 1);
+}
+
+TEST(BiquadFilterEllipticTest, LowPassEquirippleInPassbandAndStopband) {
+ // Even order: |H(0)| = -rippleDb at DC; odd order: |H(0)| = 0 dB.
+ // Both share a stopband floor at -stopAttenDb.
+ constexpr double kRipple = 1.0;
+ constexpr double kAtten = 40.0;
+ auto filter = BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz,
+ kRipple, kAtten);
+ auto sections = filter.Sections();
+
+ double dcDb = 20.0 * std::log10(CascadeMagnitude(sections, 0.0, 1000.0));
+ double fcDb = 20.0 * std::log10(CascadeMagnitude(sections, 50.0, 1000.0));
+ EXPECT_NEAR(dcDb, -kRipple, 0.01);
+ EXPECT_NEAR(fcDb, -kRipple, 0.01);
+
+ // Past the transition band the response stays at -rs floor.
+ double stopDb = 20.0 * std::log10(CascadeMagnitude(sections, 100.0, 1000.0));
+ EXPECT_LT(stopDb, -kAtten + 0.5);
+}
+
+TEST(BiquadFilterEllipticTest, OddOrderHasUnityDcGain) {
+ auto filter =
+ BiquadFilter::Elliptic(Kind::LowPass, 5, 1000.0_Hz, 50.0_Hz, 1.0, 40.0);
+ double gainDc = CascadeMagnitude(filter.Sections(), 0.0, 1000.0);
+ EXPECT_NEAR(gainDc, 1.0, 1e-9);
+}
+
+TEST(BiquadFilterEllipticTest, HighPassResponse) {
+ // Section ordering may differ from scipy for HP, like Cheby2 — verify the
+ // total response instead.
+ constexpr double kRipple = 1.0;
+ constexpr double kAtten = 40.0;
+ auto filter = BiquadFilter::Elliptic(Kind::HighPass, 4, 1000.0_Hz, 100.0_Hz,
+ kRipple, kAtten);
+ auto sections = filter.Sections();
+
+ double passbandDb =
+ 20.0 * std::log10(CascadeMagnitude(sections, 400.0, 1000.0));
+ EXPECT_NEAR(passbandDb, 0.0, kRipple + 0.01);
+
+ double cutoffDb =
+ 20.0 * std::log10(CascadeMagnitude(sections, 100.0, 1000.0));
+ EXPECT_NEAR(cutoffDb, -kRipple, 0.01);
+
+ double dcDb = 20.0 * std::log10(CascadeMagnitude(sections, 0.0, 1000.0));
+ EXPECT_LT(dcDb, -kAtten + 0.5);
+}
+
+TEST(BiquadFilterEllipticTest, BandPass4thOrderMatchesScipy) {
+ // scipy.signal.ellip(4, 1.0, 40.0, [80.0, 120.0], btype='bandpass',
+ // fs=1000.0)
+ auto filter = BiquadFilter::Elliptic(Kind::BandPass, 4, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz, 1.0, 40.0);
+ auto sections = filter.Sections();
+ ASSERT_EQ(sections.size(), 4u);
+ ExpectSectionNear(
+ sections[0],
+ {0.010903156756394984, -0.008920205787636758, 0.010903156756394982,
+ -1.4809043488404827, 0.9052184223450329},
+ 1e-10);
+ ExpectSectionNear(sections[1],
+ {1.0, -1.9038045463534676, 0.9999999999999999,
+ -1.62699499510272, 0.9194678402475894},
+ 1e-10);
+ ExpectSectionNear(
+ sections[2],
+ {1.0, -1.3265553048553793, 1.0, -1.4370735618061194, 0.9697500844409095},
+ 1e-10);
+ ExpectSectionNear(sections[3],
+ {1.0, -1.8057300347135379, 0.9999999999999998,
+ -1.733243724674222, 0.978571861817194},
+ 1e-10);
+}
+
+TEST(BiquadFilterEllipticTest, BandPassResponse) {
+ constexpr double kRipple = 1.0;
+ constexpr double kAtten = 40.0;
+ auto filter = BiquadFilter::Elliptic(Kind::BandPass, 3, 1000.0_Hz, 80.0_Hz,
+ 120.0_Hz, kRipple, kAtten);
+ auto sections = filter.Sections();
+
+ double centerDb =
+ 20.0 * std::log10(CascadeMagnitude(sections, 100.0, 1000.0));
+ EXPECT_NEAR(centerDb, 0.0, kRipple + 0.01);
+
+ // Outside the band: deeply attenuated.
+ EXPECT_LT(20.0 * std::log10(CascadeMagnitude(sections, 0.0, 1000.0)),
+ -kAtten + 1.0);
+ EXPECT_LT(20.0 * std::log10(CascadeMagnitude(sections, 400.0, 1000.0)),
+ -kAtten + 1.0);
+}
+
+TEST(BiquadFilterEllipticTest, RejectsInvalidArgs) {
+ // Order, fs, ripple, atten ranges.
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 0, 1000.0_Hz, 50.0_Hz, 1.0, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 0.0_Hz, 50.0_Hz, 1.0, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 0.0, 40.0),
+ std::invalid_argument);
+ // Stopband must be deeper than passband ripple.
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 40.0, 1.0),
+ std::invalid_argument);
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 50.0_Hz, 5.0, 5.0),
+ std::invalid_argument);
+ // Frequencies out of range / inverted.
+ EXPECT_THROW(
+ BiquadFilter::Elliptic(Kind::LowPass, 4, 1000.0_Hz, 600.0_Hz, 1.0, 40.0),
+ std::invalid_argument);
+ EXPECT_THROW(BiquadFilter::Elliptic(Kind::BandPass, 4, 1000.0_Hz, 120.0_Hz,
+ 80.0_Hz, 1.0, 40.0),
+ std::invalid_argument);
+}
+
+} // namespace
diff --git a/wpimath/src/test/native/cpp/filter/BiquadFilterTest.cpp b/wpimath/src/test/native/cpp/filter/BiquadFilterTest.cpp
new file mode 100644
index 0000000000..17b8b7460c
--- /dev/null
+++ b/wpimath/src/test/native/cpp/filter/BiquadFilterTest.cpp
@@ -0,0 +1,264 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "wpi/math/filter/BiquadFilter.hpp"
+
+#include
+#include
+#include
+#include
+#include
+
+#include
+
+#include "wpi/math/filter/LinearFilter.hpp"
+#include "wpi/units/time.hpp"
+
+using wpi::math::BiquadFilter;
+
+TEST(BiquadFilterTest, PassThrough) {
+ BiquadFilter filter({{1.0, 0.0, 0.0, 0.0, 0.0}});
+
+ std::mt19937 rng(42);
+ std::uniform_real_distribution dist(-100.0, 100.0);
+ for (int i = 0; i < 200; ++i) {
+ double x = dist(rng);
+ EXPECT_DOUBLE_EQ(filter.Calculate(x), x);
+ }
+}
+
+TEST(BiquadFilterTest, FirstOrderMatchesSinglePoleIIR) {
+ // SinglePoleIIR: y[n] = (1-g) x[n] + g y[n-1], g = exp(-dt/T)
+ // As biquad: {1-g, 0, 0, -g, 0}
+ constexpr double kTimeConstant = 0.015915;
+ constexpr double kPeriod = 0.005;
+ double g = std::exp(-kPeriod / kTimeConstant);
+
+ auto linear =
+ wpi::math::LinearFilter::SinglePoleIIR(kTimeConstant, 5_ms);
+ BiquadFilter biquad({{1.0 - g, 0.0, 0.0, -g, 0.0}});
+
+ std::mt19937 rng(7);
+ std::uniform_real_distribution dist(-50.0, 50.0);
+ for (int i = 0; i < 1000; ++i) {
+ double x = dist(rng);
+ double y_lin = linear.Calculate(x);
+ double y_biq = biquad.Calculate(x);
+ EXPECT_NEAR(y_lin, y_biq, 1e-12);
+ }
+}
+
+TEST(BiquadFilterTest, Butterworth4thOrderLowPass) {
+ // scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter({
+ {0.00041659920440659937, 0.0008331984088131987, 0.00041659920440659937,
+ -1.4796742169311934, 0.5558215432824889},
+ {1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979},
+ });
+
+ // Impulse response, first 30 samples, from scipy.signal.sosfilt.
+ constexpr std::array kExpected = {
+ 0.00041659920440659937, 0.0029914483065925663, 0.010405740533503665,
+ 0.024092655231875183, 0.04300386328531425, 0.06442081415630327,
+ 0.08518000836484753, 0.10245740377665029, 0.1142030744642985,
+ 0.11931076610150239, 0.11759868177262096, 0.10966797135549569,
+ 0.09669445862739445, 0.08019770689053446, 0.06182021082200691,
+ 0.04313888252371942, 0.025521549440937964, 0.01003324447867498,
+ -0.002609160074363505, -0.01203989315971688, -0.018213508615082776,
+ -0.021350392922805498, -0.02186860712506684, -0.020311212598085788,
+ -0.01727668431352673, -0.013358111475758645, -0.009094876704322833,
+ -0.004938595712161598, -0.0012334395430879353, 0.0017903545884787877,
+ };
+
+ for (size_t i = 0; i < kExpected.size(); ++i) {
+ double x = (i == 0) ? 1.0 : 0.0;
+ double y = filter.Calculate(x);
+ EXPECT_NEAR(y, kExpected[i], 1e-10) << "sample " << i;
+ }
+}
+
+TEST(BiquadFilterTest, Notch60Hz) {
+ // scipy.signal.iirnotch(60.0, Q=10.0, fs=1000.0), converted via tf2sos
+ BiquadFilter filter({
+ {0.9814970254751076, -1.8251457105120343, 0.9814970254751076,
+ -1.8251457105120341, 0.9629940509502151},
+ });
+
+ constexpr double kFs = 1000.0;
+ constexpr int kSamples = 1000;
+ std::vector output(kSamples);
+ for (int n = 0; n < kSamples; ++n) {
+ double t = n / kFs;
+ double x = std::sin(2.0 * std::numbers::pi * 10.0 * t) +
+ std::sin(2.0 * std::numbers::pi * 60.0 * t);
+ output[n] = filter.Calculate(x);
+ }
+
+ // Spot-check against scipy.signal.sosfilt outputs
+ EXPECT_NEAR(output[500], -0.017355123579818322, 1e-10);
+ EXPECT_NEAR(output[999], -0.08007594066581347, 1e-10);
+
+ // Attenuation check via a basic DFT at 10 Hz and 60 Hz over the last 512
+ // samples (in steady state). 60 Hz should be strongly attenuated, 10 Hz
+ // should pass almost untouched.
+ constexpr int kWindow = 512;
+ auto bin = [&](const std::vector& sig, double freq) {
+ double re = 0.0;
+ double im = 0.0;
+ for (int n = 0; n < kWindow; ++n) {
+ double x = sig[kSamples - kWindow + n];
+ double phase = 2.0 * std::numbers::pi * freq * n / kFs;
+ re += x * std::cos(phase);
+ im -= x * std::sin(phase);
+ }
+ return std::hypot(re, im);
+ };
+
+ std::vector input(kSamples);
+ for (int n = 0; n < kSamples; ++n) {
+ double t = n / kFs;
+ input[n] = std::sin(2.0 * std::numbers::pi * 10.0 * t) +
+ std::sin(2.0 * std::numbers::pi * 60.0 * t);
+ }
+
+ double in10 = bin(input, 10.0);
+ double in60 = bin(input, 60.0);
+ double out10 = bin(output, 10.0);
+ double out60 = bin(output, 60.0);
+
+ double atten60_dB = 20.0 * std::log10(out60 / in60);
+ double atten10_dB = 20.0 * std::log10(out10 / in10);
+
+ EXPECT_LT(atten60_dB, -40.0) << "60 Hz not sufficiently attenuated";
+ EXPECT_GT(atten10_dB, -0.5) << "10 Hz passband loss too large";
+}
+
+TEST(BiquadFilterTest, Order8ButterworthMatchesScipy) {
+ // High-order filter = 4 biquads. This test exists to prove that the SOS
+ // (Direct Form II Transposed) implementation is numerically correct at the
+ // orders that a flattened-polynomial LinearFilter cannot reliably run.
+ //
+ // scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')
+ BiquadFilter filter({
+ {2.3959644103776166e-05, 4.791928820755233e-05, 2.3959644103776166e-05,
+ -1.0263514742610553, 0.26864019099379005},
+ {1.0, 2.0, 1.0, -1.0868584613628944, 0.343430940165366},
+ {1.0, 2.0, 1.0, -1.2197253651240232, 0.5076634651740437},
+ {1.0, 2.0, 1.0, -1.4515795942478362, 0.794251053241888},
+ });
+
+ // Linear chirp from 1 Hz to 200 Hz over 500 samples at 1 kHz.
+ // Matches scipy.signal.chirp(t, f0=1, f1=200, t1=t[-1], method='linear').
+ constexpr int kSamples = 500;
+ constexpr double kFs = 1000.0;
+ constexpr double kF0 = 1.0;
+ constexpr double kF1 = 200.0;
+ const double t1 = (kSamples - 1) / kFs;
+ const double k = (kF1 - kF0) / t1;
+
+ std::array spot_samples{};
+ constexpr std::array kSpotIndices{10, 50, 100, 250, 499};
+ constexpr std::array kExpected{
+ 0.8950675041062186, -0.7902247252134351, 0.1716891991372734,
+ 0.05240058121316523, -0.0016952227415119995,
+ };
+
+ size_t spot_idx = 0;
+ for (int n = 0; n < kSamples; ++n) {
+ double t = n / kFs;
+ double phase = 2.0 * std::numbers::pi * (kF0 * t + 0.5 * k * t * t);
+ double x = std::cos(phase);
+ double y = filter.Calculate(x);
+
+ if (spot_idx < kSpotIndices.size() && n == kSpotIndices[spot_idx]) {
+ spot_samples[spot_idx++] = y;
+ }
+ }
+
+ for (size_t i = 0; i < kExpected.size(); ++i) {
+ EXPECT_NEAR(spot_samples[i], kExpected[i], 1e-10)
+ << "sample index " << kSpotIndices[i];
+ }
+}
+
+TEST(BiquadFilterTest, ResetZerosState) {
+ BiquadFilter filter({
+ {0.00041659920440659937, 0.0008331984088131987, 0.00041659920440659937,
+ -1.4796742169311934, 0.5558215432824889},
+ {1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979},
+ });
+
+ for (int i = 0; i < 50; ++i) {
+ filter.Calculate(1.0);
+ }
+ EXPECT_NE(filter.LastValue(), 0.0);
+
+ filter.Reset();
+ EXPECT_DOUBLE_EQ(filter.LastValue(), 0.0);
+
+ // First call after Reset should behave like the filter starts fresh —
+ // matches the impulse-response first sample.
+ double y = filter.Calculate(1.0);
+ EXPECT_NEAR(y, 0.00041659920440659937, 1e-12);
+}
+
+TEST(BiquadFilterTest, ResetToSteadyState) {
+ // DC gain of each section is (b0+b1+b2)/(1+a1+a2). After Reset(value),
+ // Calculate(value) should immediately return value * cascade_DC_gain.
+ BiquadFilter filter({
+ {0.00041659920440659937, 0.0008331984088131987, 0.00041659920440659937,
+ -1.4796742169311934, 0.5558215432824889},
+ {1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979},
+ });
+
+ constexpr double kInput = 3.0;
+ filter.Reset(kInput);
+
+ // Cascade DC gain for a Butterworth LP is 1.0, so output should equal input.
+ EXPECT_NEAR(filter.LastValue(), kInput, 1e-12);
+ double y = filter.Calculate(kInput);
+ EXPECT_NEAR(y, kInput, 1e-12);
+
+ // And remain at steady state
+ for (int i = 0; i < 20; ++i) {
+ EXPECT_NEAR(filter.Calculate(kInput), kInput, 1e-12);
+ }
+}
+
+TEST(BiquadFilterTest, DCGainConverges) {
+ BiquadFilter filter({
+ {0.00041659920440659937, 0.0008331984088131987, 0.00041659920440659937,
+ -1.4796742169311934, 0.5558215432824889},
+ {1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979},
+ });
+
+ constexpr double kInput = 2.5;
+ double y = 0.0;
+ for (int i = 0; i < 500; ++i) {
+ y = filter.Calculate(kInput);
+ }
+ EXPECT_NEAR(y, kInput, 1e-6); // Butterworth LP has DC gain 1
+}
+
+TEST(BiquadFilterTest, NumSections) {
+ BiquadFilter one({{1.0, 0.0, 0.0, 0.0, 0.0}});
+ EXPECT_EQ(one.NumSections(), 1u);
+
+ BiquadFilter three({
+ {1.0, 0.0, 0.0, 0.0, 0.0},
+ {1.0, 0.0, 0.0, 0.0, 0.0},
+ {1.0, 0.0, 0.0, 0.0, 0.0},
+ });
+ EXPECT_EQ(three.NumSections(), 3u);
+}
+
+TEST(BiquadFilterTest, EmptyCascadeThrows) {
+ EXPECT_THROW(
+ {
+ std::vector sections;
+ std::span empty{sections};
+ BiquadFilter filter{empty};
+ },
+ std::runtime_error);
+}
diff --git a/wpimath/src/test/python/test_biquad_filter.py b/wpimath/src/test/python/test_biquad_filter.py
new file mode 100644
index 0000000000..b793954ce9
--- /dev/null
+++ b/wpimath/src/test/python/test_biquad_filter.py
@@ -0,0 +1,448 @@
+"""Tests for wpimath.BiquadFilter."""
+
+import math
+import random
+
+import pytest
+
+from wpimath import BiquadFilter
+
+
+def test_pass_through():
+ filter = BiquadFilter([BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0)])
+ rng = random.Random(42)
+ for _ in range(200):
+ x = rng.uniform(-100.0, 100.0)
+ assert filter.calculate(x) == x
+
+
+def test_butterworth_4th_order_low_pass():
+ # scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889,
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979
+ ),
+ ]
+ )
+ expected = [
+ 0.00041659920440659937,
+ 0.0029914483065925663,
+ 0.010405740533503665,
+ 0.024092655231875183,
+ 0.04300386328531425,
+ 0.06442081415630327,
+ 0.08518000836484753,
+ 0.10245740377665029,
+ 0.1142030744642985,
+ 0.11931076610150239,
+ 0.11759868177262096,
+ 0.10966797135549569,
+ 0.09669445862739445,
+ 0.08019770689053446,
+ 0.06182021082200691,
+ 0.04313888252371942,
+ 0.025521549440937964,
+ 0.01003324447867498,
+ -0.002609160074363505,
+ -0.01203989315971688,
+ -0.018213508615082776,
+ -0.021350392922805498,
+ -0.02186860712506684,
+ -0.020311212598085788,
+ -0.01727668431352673,
+ -0.013358111475758645,
+ -0.009094876704322833,
+ -0.004938595712161598,
+ -0.0012334395430879353,
+ 0.0017903545884787877,
+ ]
+ for i, e in enumerate(expected):
+ x = 1.0 if i == 0 else 0.0
+ y = filter.calculate(x)
+ assert math.isclose(y, e, rel_tol=0, abs_tol=1e-10), f"sample {i}"
+
+
+def test_notch_60hz():
+ # scipy.signal.iirnotch(60, Q=10, fs=1000) via tf2sos
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(
+ 0.9814970254751076,
+ -1.8251457105120343,
+ 0.9814970254751076,
+ -1.8251457105120341,
+ 0.9629940509502151,
+ )
+ ]
+ )
+ fs = 1000.0
+ samples = 1000
+ output = []
+ for n in range(samples):
+ t = n / fs
+ x = math.sin(2.0 * math.pi * 10.0 * t) + math.sin(2.0 * math.pi * 60.0 * t)
+ output.append(filter.calculate(x))
+
+ assert math.isclose(output[500], -0.017355123579818322, abs_tol=1e-10)
+ assert math.isclose(output[999], -0.08007594066581347, abs_tol=1e-10)
+
+
+def test_order_8_butterworth_matches_scipy():
+ # scipy.signal.butter(8, 100.0, btype='low', fs=1000.0, output='sos')
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(
+ 2.3959644103776166e-05,
+ 4.791928820755233e-05,
+ 2.3959644103776166e-05,
+ -1.0263514742610553,
+ 0.26864019099379005,
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.0868584613628944, 0.343430940165366
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.2197253651240232, 0.5076634651740437
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.4515795942478362, 0.794251053241888
+ ),
+ ]
+ )
+ N = 500
+ fs = 1000.0
+ f0 = 1.0
+ f1 = 200.0
+ t1 = (N - 1) / fs
+ k = (f1 - f0) / t1
+
+ spot_idx = [10, 50, 100, 250, 499]
+ expected = [
+ 0.8950675041062186,
+ -0.7902247252134351,
+ 0.1716891991372734,
+ 0.05240058121316523,
+ -0.0016952227415119995,
+ ]
+ got = []
+ j = 0
+ for n in range(N):
+ t = n / fs
+ phase = 2.0 * math.pi * (f0 * t + 0.5 * k * t * t)
+ x = math.cos(phase)
+ y = filter.calculate(x)
+ if j < len(spot_idx) and n == spot_idx[j]:
+ got.append(y)
+ j += 1
+
+ for i, (g, e) in enumerate(zip(got, expected)):
+ assert math.isclose(g, e, abs_tol=1e-10), f"index {spot_idx[i]}"
+
+
+def test_reset_zeros_state():
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889,
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979
+ ),
+ ]
+ )
+ for _ in range(50):
+ filter.calculate(1.0)
+ assert filter.lastValue() != 0.0
+
+ filter.reset()
+ assert filter.lastValue() == 0.0
+ y = filter.calculate(1.0)
+ assert math.isclose(y, 0.00041659920440659937, abs_tol=1e-12)
+
+
+def test_reset_to_steady_state():
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889,
+ ),
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979
+ ),
+ ]
+ )
+ input_val = 3.0
+ filter.reset(input_val)
+
+ assert math.isclose(filter.lastValue(), input_val, abs_tol=1e-12)
+ assert math.isclose(filter.calculate(input_val), input_val, abs_tol=1e-12)
+ for _ in range(20):
+ assert math.isclose(filter.calculate(input_val), input_val, abs_tol=1e-12)
+
+
+def test_num_sections():
+ filter = BiquadFilter(
+ [
+ BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0),
+ BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0),
+ BiquadFilter.Section(1.0, 0.0, 0.0, 0.0, 0.0),
+ ]
+ )
+ assert filter.numSections() == 3
+
+
+def test_section_repr():
+ s = BiquadFilter.Section(1.0, 2.0, 3.0, 4.0, 5.0)
+ assert "b0" in repr(s)
+ assert "1.0" in repr(s)
+
+
+def test_empty_cascade_throws():
+ with pytest.raises(RuntimeError):
+ BiquadFilter([])
+
+
+# ---------- Design factories ------------------------------------------------
+
+
+def _expect_section_near(got, want, tol):
+ assert got.b0 == pytest.approx(want.b0, abs=tol)
+ assert got.b1 == pytest.approx(want.b1, abs=tol)
+ assert got.b2 == pytest.approx(want.b2, abs=tol)
+ assert got.a1 == pytest.approx(want.a1, abs=tol)
+ assert got.a2 == pytest.approx(want.a2, abs=tol)
+
+
+def _cascade_magnitude(sections, f, fs):
+ import cmath
+
+ w = 2.0 * math.pi * f / fs
+ z1 = cmath.exp(-1j * w)
+ z2 = cmath.exp(-2j * w)
+ h = 1.0 + 0j
+ for s in sections:
+ num = s.b0 + s.b1 * z1 + s.b2 * z2
+ den = 1.0 + s.a1 * z1 + s.a2 * z2
+ h *= num / den
+ return abs(h)
+
+
+def test_butterworth_factory_matches_scipy():
+ # scipy.signal.butter(4, 50.0, btype='low', fs=1000.0, output='sos')
+ f = BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0)
+ sections = list(f.sections())
+ assert len(sections) == 2
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.00041659920440659937,
+ 0.0008331984088131987,
+ 0.00041659920440659937,
+ -1.4796742169311934,
+ 0.5558215432824889,
+ ),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[1],
+ BiquadFilter.Section(
+ 1.0, 2.0, 1.0, -1.7009643319435257, 0.7884997398152979
+ ),
+ 1e-10,
+ )
+
+
+def test_butterworth_bandpass_factory_matches_scipy():
+ # scipy.signal.butter(4, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ f = BiquadFilter.butterworth(BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0)
+ sections = list(f.sections())
+ assert len(sections) == 4
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.0001832160233696091,
+ 0.0003664320467392182,
+ 0.0001832160233696091,
+ -1.395944592254935,
+ 0.7785762494967928,
+ ),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[1],
+ BiquadFilter.Section(1.0, 2.0, 1.0, -1.5194742571654707, 0.8044610397041421),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[2],
+ BiquadFilter.Section(1.0, -2.0, 1.0, -1.395095159020637, 0.8950130915917338),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[3],
+ BiquadFilter.Section(1.0, -2.0, 1.0, -1.678184355447092, 0.9231164780821922),
+ 1e-10,
+ )
+
+
+def test_butterworth_factory_cutoff_at_minus_three_db():
+ f = BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0)
+ sections = list(f.sections())
+ assert _cascade_magnitude(sections, 0.0, 1000.0) == pytest.approx(1.0, abs=1e-10)
+ db_at_fc = 20.0 * math.log10(_cascade_magnitude(sections, 50.0, 1000.0))
+ assert db_at_fc == pytest.approx(-3.0, abs=0.05)
+
+
+def test_chebyshev1_factory_matches_scipy():
+ # scipy.signal.cheby1(4, 1.0, 50.0, btype='low', fs=1000.0, output='sos')
+ f = BiquadFilter.chebyshevI(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 1.0)
+ sections = list(f.sections())
+ assert len(sections) == 2
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.00012984963538691335,
+ 0.0002596992707738267,
+ 0.00012984963538691335,
+ -1.7831991339963722,
+ 0.8083720161261031,
+ ),
+ 1e-10,
+ )
+
+
+def test_chebyshev2_factory_matches_scipy():
+ # scipy.signal.cheby2(4, 40.0, 50.0, btype='low', fs=1000.0, output='sos')
+ f = BiquadFilter.chebyshevII(BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 40.0)
+ sections = list(f.sections())
+ assert len(sections) == 2
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.009735570656077937,
+ -0.01377605024474192,
+ 0.009735570656077937,
+ -1.6993957730842835,
+ 0.7262535657383176,
+ ),
+ 1e-10,
+ )
+
+
+def test_elliptic_factory_matches_scipy():
+ # scipy.signal.ellip(4, 1.0, 40.0, 50.0, btype='low', fs=1000.0)
+ f = BiquadFilter.elliptic(
+ BiquadFilter.Kind.LowPass, 4, 1000.0, 50.0, 1.0, 40.0
+ )
+ sections = list(f.sections())
+ assert len(sections) == 2
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.011738158079014929,
+ -0.01231742214386518,
+ 0.011738158079014929,
+ -1.7624726990429698,
+ 0.7947551993829407,
+ ),
+ 1e-10,
+ )
+
+
+def test_elliptic_bandpass_factory_matches_scipy():
+ # scipy.signal.ellip(4, 1.0, 40.0, [80.0, 120.0], btype='bandpass', fs=1000.0)
+ f = BiquadFilter.elliptic(
+ BiquadFilter.Kind.BandPass, 4, 1000.0, 80.0, 120.0, 1.0, 40.0
+ )
+ sections = list(f.sections())
+ assert len(sections) == 4
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.010903156756394984,
+ -0.008920205787636758,
+ 0.010903156756394982,
+ -1.4809043488404827,
+ 0.9052184223450329,
+ ),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[1],
+ BiquadFilter.Section(
+ 1.0,
+ -1.9038045463534676,
+ 0.9999999999999999,
+ -1.62699499510272,
+ 0.9194678402475894,
+ ),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[2],
+ BiquadFilter.Section(
+ 1.0, -1.3265553048553793, 1.0, -1.4370735618061194, 0.9697500844409095
+ ),
+ 1e-10,
+ )
+ _expect_section_near(
+ sections[3],
+ BiquadFilter.Section(
+ 1.0,
+ -1.8057300347135379,
+ 0.9999999999999998,
+ -1.733243724674222,
+ 0.978571861817194,
+ ),
+ 1e-10,
+ )
+
+
+def test_notch_factory_matches_scipy():
+ # scipy.signal.iirnotch(60.0, Q=10.0, fs=1000.0)
+ f = BiquadFilter.notch(1000.0, 60.0, 10.0)
+ sections = list(f.sections())
+ assert len(sections) == 1
+ _expect_section_near(
+ sections[0],
+ BiquadFilter.Section(
+ 0.9814970254751076,
+ -1.8251457105120343,
+ 0.9814970254751076,
+ -1.8251457105120341,
+ 0.9629940509502151,
+ ),
+ 1e-12,
+ )
+
+
+def test_moving_average_factory_dc_gain():
+ f = BiquadFilter.movingAverage(4)
+ sections = list(f.sections())
+ assert _cascade_magnitude(sections, 0.0, 1000.0) == pytest.approx(1.0, abs=1e-12)
+ assert _cascade_magnitude(sections, 500.0, 1000.0) < 1e-12
+
+
+def test_factory_invalid_args_throw():
+ with pytest.raises((ValueError, RuntimeError)):
+ BiquadFilter.butterworth(BiquadFilter.Kind.LowPass, 0, 1000.0, 50.0)
+ with pytest.raises((ValueError, RuntimeError)):
+ BiquadFilter.notch(1000.0, 60.0, 0.0)
+ with pytest.raises((ValueError, RuntimeError)):
+ BiquadFilter.movingAverage(0)