Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-07-04 03:11:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpimath] Add BiquadFilter class for Second Order Section filters (#8814)

Browse Source 
This adds a C++, Java and Python version of a Biquad filter class to
wpimath.

For testing, I took output from scipi functions and commented the inputs
I used above each test case.

---------

Co-authored-by: Drew Williams <drew.williams@capstanmedical.com>
This commit is contained in:
Drew Williams
2026-06-23 10:43:56 -07:00
committed by GitHub
parent ada1191265
commit 38f113ae4b
34 changed files with 6511 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

430
wpimath/src/main/java/org/wpilib/math/filter/BiquadFilter.java Normal file
View File

@@ -0,0 +1,430 @@
// 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 org.wpilib.math.filter.internal.BiquadFilterDesigner;
import org.wpilib.math.util.MathSharedStore;
/**
* 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.
*
* <p>Each section implements:
*
* <pre>
* 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]
* </pre>
*
* <p>Sections are normalized so that a₀ = 1 and are applied in series.
*
* <p>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.
*
* <p>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.
*/
public class BiquadFilter {
/** A single biquad (second-order) section. a₀ is assumed normalized to 1. */
public static final class Section {
/** The b0 feedforward coefficient. */
public final double b0;
/** The b1 feedforward coefficient. */
public final double b1;
/** The b2 feedforward coefficient. */
public final double b2;
/** The a1 feedback coefficient. */
public final double a1;
/** The a2 feedback coefficient. */
public final double a2;
/**
* Creates a biquad section.
*
* @param b0 The b0 feedforward coefficient.
* @param b1 The b1 feedforward coefficient.
* @param b2 The b2 feedforward coefficient.
* @param a1 The a1 feedback coefficient.
* @param a2 The a2 feedback coefficient.
*/
public Section(double b0, double b1, double b2, double a1, double a2) {
this.b0 = b0;
this.b1 = b1;
this.b2 = b2;
this.a1 = a1;
this.a2 = a2;
}
}
/**
* Frequency response shape for the classical IIR design factories. For BandPass/BandStop, two
* cutoff frequencies (f1, f2) are required.
*/
public enum Kind {
/** Low-pass filter (passes frequencies below the cutoff). */
LowPass,
/** High-pass filter (passes frequencies above the cutoff). */
HighPass,
/** Band-pass filter (passes a frequency band). */
BandPass,
/** Band-stop filter (rejects a frequency band). */
BandStop
}
private final Section[] m_sections;
private final double[][] m_state;
private double m_lastOutput;
private static int instances;
/**
* Creates a biquad filter cascade from the given sections.
*
* @param sections The biquad sections, applied in series.
* @throws IllegalArgumentException if sections is empty.
*/
public BiquadFilter(Section... sections) {
if (sections.length == 0) {
throw new IllegalArgumentException("BiquadFilter requires at least one section.");
}
m_sections = sections.clone();
m_state = new double[sections.length][2];
instances++;
MathSharedStore.reportUsage("BiquadFilter", String.valueOf(instances));
}
/**
* Calculates the next value of the filter.
*
* @param input Current input value.
* @return The filtered value at this step.
*/
public 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 (int i = 0; i < m_sections.length; i++) {
final Section sec = m_sections[i];
final double[] 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. */
public void reset() {
for (double[] s : m_state) {
s[0] = 0.0;
s[1] = 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.
*/
public 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 (int i = 0; i < m_sections.length; i++) {
final Section 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.
*/
public double lastValue() {
return m_lastOutput;
}
/**
* Returns the number of sections in the cascade.
*
* @return The number of sections.
*/
public int numSections() {
return m_sections.length;
}
/**
* Returns a copy of the cascade's sections, in application order. Useful for inspection, logging,
* or serialization of designed filters.
*
* @return A copy of the section array.
*/
public Section[] sections() {
return m_sections.clone();
}
// ---------- Design factories -------------------------------------------------
/**
* Designs a Butterworth IIR band-pass or band-stop filter as a cascade of biquad sections.
*
* <p>Coefficients match {@code scipy.signal.butter(order, Wn, btype, fs, output='sos')}.
* 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 (&gt;= 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 &lt; lowCutoff &lt; highCutoff &lt;
* sampleRate/2.
* @param highCutoff High edge of the band (Hz).
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is LowPass / HighPass.
*/
public static BiquadFilter butterworth(
Kind kind, int order, double sampleRate, double lowCutoff, double highCutoff) {
return new BiquadFilter(
BiquadFilterDesigner.butterworth(kind, order, sampleRate, lowCutoff, highCutoff));
}
/**
* Designs a Butterworth IIR low-pass or high-pass filter (single cutoff).
*
* <p>Coefficients match {@code scipy.signal.butter(order, Wn, btype, fs, output='sos')} to within
* ~1e-10.
*
* @param kind Must be LowPass or HighPass.
* @param order Prototype order (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is BandPass /
* BandStop.
*/
public static BiquadFilter butterworth(Kind kind, int order, double sampleRate, double cutoff) {
return new BiquadFilter(BiquadFilterDesigner.butterworth(kind, order, sampleRate, cutoff));
}
/**
* Designs a Chebyshev type-I IIR band-pass or band-stop filter as a cascade of biquad sections.
* Equiripple in the passband, monotonic in the stopband. Coefficients match {@code
* scipy.signal.cheby1(order, rp, Wn, btype, fs, output='sos')}.
*
* @param kind Must be BandPass or BandStop.
* @param order Prototype order (&gt;= 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 &lt; lowCutoff &lt; highCutoff &lt;
* sampleRate/2.
* @param highCutoff High edge of the band (Hz).
* @param rippleDb Peak-to-peak passband ripple in dB. Must be &gt; 0; values from ~0.1 to ~3 dB
* are typical.
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is LowPass / HighPass.
*/
public static BiquadFilter chebyshevI(
Kind kind,
int order,
double sampleRate,
double lowCutoff,
double highCutoff,
double rippleDb) {
return new BiquadFilter(
BiquadFilterDesigner.chebyshevI(kind, order, sampleRate, lowCutoff, highCutoff, 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param rippleDb Peak-to-peak passband ripple in dB. Must be &gt; 0.
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is BandPass /
* BandStop.
*/
public static BiquadFilter chebyshevI(
Kind kind, int order, double sampleRate, double cutoff, double rippleDb) {
return new BiquadFilter(
BiquadFilterDesigner.chebyshevI(kind, order, sampleRate, cutoff, rippleDb));
}
/**
* Designs a Chebyshev type-II (inverse Chebyshev) IIR band-pass or band-stop filter as a cascade
* of biquad sections. Monotonic in the passband, equiripple in the stopband. Coefficients match
* {@code scipy.signal.cheby2(order, rs, Wn, btype, fs, output='sos')}.
*
* @param kind Must be BandPass or BandStop.
* @param order Prototype order (&gt;= 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 &lt; lowCutoff &lt; highCutoff
* &lt; sampleRate/2.
* @param highCutoff High edge of the stop band (Hz).
* @param stopAttenDb Stopband attenuation in dB. Must be &gt; 0; values from ~20 to ~80 dB are
* typical.
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is LowPass / HighPass.
*/
public static BiquadFilter chebyshevII(
Kind kind,
int order,
double sampleRate,
double lowCutoff,
double highCutoff,
double stopAttenDb) {
return new BiquadFilter(
BiquadFilterDesigner.chebyshevII(
kind, order, sampleRate, lowCutoff, highCutoff, 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 {@code stopAttenDb} of attenuation.
*
* @param kind Must be LowPass or HighPass.
* @param order Prototype order (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Stopband-edge frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param stopAttenDb Stopband attenuation in dB. Must be &gt; 0.
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is BandPass /
* BandStop.
*/
public static BiquadFilter chebyshevII(
Kind kind, int order, double sampleRate, double cutoff, double stopAttenDb) {
return new BiquadFilter(
BiquadFilterDesigner.chebyshevII(kind, order, sampleRate, cutoff, stopAttenDb));
}
/**
* Designs an elliptic (Cauer) IIR band-pass or band-stop 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 {@code scipy.signal.ellip(order, rp, rs, Wn,
* btype, fs, output='sos')}.
*
* @param kind Must be BandPass or BandStop.
* @param order Filter order (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param lowCutoff Low edge of the band (Hz). Must satisfy 0 &lt; lowCutoff &lt; highCutoff &lt;
* sampleRate/2.
* @param highCutoff High edge of the band (Hz).
* @param rippleDb Passband ripple in dB (&gt; 0).
* @param stopAttenDb Stopband attenuation in dB (must exceed {@code rippleDb}).
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is LowPass / HighPass.
*/
public static BiquadFilter elliptic(
Kind kind,
int order,
double sampleRate,
double lowCutoff,
double highCutoff,
double rippleDb,
double stopAttenDb) {
return new BiquadFilter(
BiquadFilterDesigner.elliptic(
kind, order, sampleRate, lowCutoff, highCutoff, rippleDb, 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param rippleDb Passband ripple in dB (&gt; 0).
* @param stopAttenDb Stopband attenuation in dB (must exceed {@code rippleDb}).
* @return The designed filter.
* @throws IllegalArgumentException if any argument is out of range or kind is BandPass /
* BandStop.
*/
public static BiquadFilter elliptic(
Kind kind, int order, double sampleRate, double cutoff, double rippleDb, double stopAttenDb) {
return new BiquadFilter(
BiquadFilterDesigner.elliptic(kind, order, sampleRate, cutoff, rippleDb, stopAttenDb));
}
/**
* Designs a second-order IIR notch at the given center frequency with the given quality factor.
* Matches {@code scipy.signal.iirnotch}.
*
* @param sampleRate Sample rate (Hz). Must be positive.
* @param centerFrequency Notch center frequency (Hz). Must satisfy 0 &lt; centerFrequency &lt;
* sampleRate/2.
* @param qualityFactor Quality factor (Q). Higher values give a narrower notch. Must be positive.
* @return The designed filter (single-section cascade).
* @throws IllegalArgumentException if any argument is out of range.
*/
public static BiquadFilter notch(
double sampleRate, double centerFrequency, double qualityFactor) {
return new BiquadFilter(BiquadFilterDesigner.notch(sampleRate, centerFrequency, qualityFactor));
}
/**
* Designs an N-tap moving-average filter as a cascade of FIR biquads.
*
* <p>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 &gt;= 1.
* @return The designed filter.
* @throws IllegalArgumentException if {@code taps} &lt; 1.
*/
public static BiquadFilter movingAverage(int taps) {
return new BiquadFilter(BiquadFilterDesigner.movingAverage(taps));
}
}

6
wpimath/src/main/java/org/wpilib/math/filter/LinearFilter.java
View File

@@ -41,6 +41,12 @@ import org.wpilib.util.container.DoubleCircularBuffer;
* <a href="https://en.wikipedia.org/wiki/Fir_filter">https://en.wikipedia.org/wiki/Fir_filter</a>
* <br>
*
* <p>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 ({@code
* singlePoleIIR}, {@code highPass}) and FIR filters ({@code movingAverage}, {@code
* finiteDifference}).
*
* <p>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.
*

294
wpimath/src/main/java/org/wpilib/math/filter/internal/AnalogPrototypes.java Normal file
View File

@@ -0,0 +1,294 @@
// 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;
/**
* Analog low-pass prototype ZPK constructions for the four classical IIR families. Cutoff is
* normalized to 1 rad/s in every case.
*
* <p>Each prototype mirrors the corresponding {@code *_ap} helper in scipy.signal: {@code buttap},
* {@code cheb1ap}, {@code cheb2ap}, {@code 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.
*
* <p>References for the families themselves:
*
* <ul>
* <li>Butterworth poles on the unit circle:
* https://en.wikipedia.org/wiki/Butterworth_filter#Transfer_function
* <li>Chebyshev I/II pole/zero geometry: https://en.wikipedia.org/wiki/Chebyshev_filter
* <li>Elliptic (Cauer): Orfanidis, "Introduction to Signal Processing Second Edition (2023)",
* https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
* </ul>
*/
final class AnalogPrototypes {
private AnalogPrototypes() {}
private static final double COEF_EPS = 2e-16;
private static final double LN10 = 2.302585092994045684017991454684;
// 10^x - 1 evaluated as expm1(x · ln10) for accuracy when x is small.
private static double pow10m1(double x) {
return Math.expm1(LN10 * x);
}
// Java's Math has no asinh; poly-fill via the standard identity. The arguments
// we pass are in (~1, ~100) for typical filter ripple/atten settings, so the
// simple form is numerically safe.
private static double asinh(double x) {
return Math.log(x + Math.sqrt(x * x + 1.0));
}
/** Analog Butterworth low-pass prototype, cutoff 1 rad/s. */
static Zpk butterworthPrototype(int order) {
// Order-N Butterworth analog low-pass prototype. 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 = new Zpk();
p.gain = 1.0;
for (int k = 0; k < order; k++) {
double angle = Math.PI / 2.0 + Math.PI * (2.0 * k + 1.0) / (2.0 * order);
p.poles.add(Complex.polar(1.0, angle));
}
return p;
}
/**
* 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 &gt; 0).
*/
static 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 = new Zpk();
final double eps = Math.sqrt(Math.pow(10.0, 0.1 * rippleDb) - 1.0);
final double mu = asinh(1.0 / eps) / order;
Complex prodNegP = Complex.ONE;
for (int k = 0; k < order; k++) {
double m = -order + 1 + 2 * k;
double theta = Math.PI * m / (2.0 * order);
Complex pole = new Complex(mu, theta).sinh().negate();
out.poles.add(pole);
prodNegP = prodNegP.mul(pole.negate());
}
// 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 /= Math.sqrt(1.0 + eps * eps);
}
out.gain = k;
return out;
}
/**
* Analog Chebyshev type-II low-pass prototype, equiripple in stopband, stopband-edge frequency 1
* rad/s (the point at which the response first reaches {@code stopAttenDb} of attenuation).
*
* @param stopAttenDb Stopband attenuation in dB (must be &gt; 0).
*/
static 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 = new Zpk();
final double delta = 1.0 / Math.sqrt(Math.pow(10.0, 0.1 * stopAttenDb) - 1.0);
final double mu = asinh(1.0 / delta) / order;
Complex prodNegP = Complex.ONE;
for (int k = 0; k < order; k++) {
double m1 = -order + 1 + 2 * k;
double theta = Math.PI * m1 / (2.0 * order);
Complex pole = Complex.ONE.div(new Complex(mu, theta).sinh()).negate();
out.poles.add(pole);
prodNegP = prodNegP.mul(pole.negate());
}
// 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).
Complex prodNegZ = Complex.ONE;
for (int k = 0; k < order; k++) {
double m = -order + 1 + 2 * k;
if (m == 0.0) {
continue;
}
Complex zero = new Complex(0.0, 1.0 / Math.sin(Math.PI * m / (2.0 * order)));
out.zeros.add(zero);
prodNegZ = prodNegZ.mul(zero.negate());
}
out.gain = prodNegP.div(prodNegZ).real();
return out;
}
/**
* 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 (&gt;= 1).
* @param rippleDb Peak-to-peak passband ripple in dB (&gt; 0).
* @param stopAttenDb Stopband attenuation in dB (&gt; {@code rippleDb}).
*/
static 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 = new Zpk();
// 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 = Math.pow(10.0, -rippleDb / 20.0);
return out;
}
if (order == 1) {
double p = -Math.sqrt(1.0 / pow10m1(0.1 * rippleDb));
out.poles.add(new Complex(p, 0.0));
out.gain = -p;
return out;
}
final double epsSq = pow10m1(0.1 * rippleDb);
final double eps = Math.sqrt(epsSq);
final double ck1Sq = epsSq / pow10m1(0.1 * stopAttenDb);
if (ck1Sq <= 0.0) {
throw new IllegalArgumentException(
"Elliptic design: invalid ripple/atten combination (small modulus = 0)");
}
final double K1 = JacobiElliptic.ellipticK(ck1Sq);
final double m = JacobiElliptic.ellipticDegree(order, ck1Sq);
final double capK = JacobiElliptic.ellipticK(m);
final double sqrtM = Math.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).
List<Integer> jIdx = new ArrayList<>();
for (int j = 1 - (order % 2); j < order; j += 2) {
jIdx.add(j);
}
// Cache (sn, cn, dn) at each j·K/N — needed for both zeros and poles.
List<JacobiElliptic.JacobiResult> jacobi = new ArrayList<>(jIdx.size());
for (int j : jIdx) {
jacobi.add(JacobiElliptic.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.
List<Complex> zerosUpper = new ArrayList<>();
for (JacobiElliptic.JacobiResult jr : jacobi) {
if (Math.abs(jr.sn) <= COEF_EPS) {
continue;
}
Complex z = new Complex(0.0, 1.0 / (sqrtM * jr.sn));
zerosUpper.add(z);
}
for (Complex z : zerosUpper) {
out.zeros.add(z);
}
for (Complex z : zerosUpper) {
out.zeros.add(z.conj());
}
// Poles use an auxiliary point v0 found by inverting sc(·, 1-m) at 1/eps,
// then ellipj at v0 with the complementary modulus.
final double r = JacobiElliptic.inverseJacobiSc1(1.0 / eps, ck1Sq);
final double v0 = capK * r / (order * K1);
final JacobiElliptic.JacobiResult sv = JacobiElliptic.ellipj(v0, 1.0 - m);
List<Complex> 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;
}
}

127
wpimath/src/main/java/org/wpilib/math/filter/internal/BilinearTransform.java Normal file
View File

@@ -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.
*
* <p>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).
*
* <p>Background:
*
* <ul>
* <li>https://en.wikipedia.org/wiki/Bilinear_transform
* <li>Oppenheim &amp; Schafer, "Discrete-Time Signal Processing" §7.2.2
* </ul>
*
* <p>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).
*
* <p>Caller is responsible for validating inputs (positive fs, f1 in (0, fs/2), and for BP/BS, f1
* &lt; f2 &lt; fs/2). This helper does no validation itself.
*/
static List<BiquadFilter.Section> 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));
}
}

405
wpimath/src/main/java/org/wpilib/math/filter/internal/BiquadFilterDesigner.java Normal file
View File

@@ -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.
*
* <p>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.
*
* <p>Each classical-IIR factory (Butterworth, Chebyshev I/II, Elliptic) drives the same three-step
* pipeline that {@code scipy.signal.iirfilter} does:
*
* <ol>
* <li>{@link AnalogPrototypes} — analog LP prototype, cutoff 1 rad/s
* <li>{@link BilinearTransform#designFromAnalogLp}: kind-specific frequency transform via {@link
* Zpk}, bilinear analog→digital at sample rate fs, then ZPK→SOS biquad pairing
* </ol>
*
* <p>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}).
*
* <p>{@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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param lowCutoff Low edge of the band (Hz). Must satisfy 0 &lt; lowCutoff &lt; highCutoff &lt;
* 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; 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 (&gt;= 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 &lt; lowCutoff &lt; highCutoff &lt;
* sampleRate/2.
* @param highCutoff High edge of the band (Hz).
* @param rippleDb Peak-to-peak passband ripple in dB. Must be &gt; 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param rippleDb Peak-to-peak passband ripple in dB. Must be &gt; 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 (&gt;= 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 &lt; lowCutoff &lt; highCutoff
* &lt; sampleRate/2.
* @param highCutoff High edge of the stop band (Hz).
* @param stopAttenDb Stopband attenuation in dB. Must be &gt; 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Stopband-edge frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param stopAttenDb Stopband attenuation in dB. Must be &gt; 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param lowCutoff Low edge of the band (Hz). Must satisfy 0 &lt; lowCutoff &lt; highCutoff &lt;
* sampleRate/2.
* @param highCutoff High edge of the band (Hz).
* @param rippleDb Passband ripple in dB (&gt; 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 (&gt;= 1).
* @param sampleRate Sample rate (Hz). Must be positive.
* @param cutoff Cutoff frequency (Hz). Must satisfy 0 &lt; cutoff &lt; sampleRate/2.
* @param rippleDb Passband ripple in dB (&gt; 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 &lt; centerFrequency &lt;
* 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 &gt;= 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<Section> 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<Section> list) {
return list.toArray(new Section[0]);
}
}

123
wpimath/src/main/java/org/wpilib/math/filter/internal/Complex.java Normal file
View File

@@ -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<double>} 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<double> — 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);
}
}

232
wpimath/src/main/java/org/wpilib/math/filter/internal/JacobiElliptic.java Normal file
View File

@@ -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.
*
* <p>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.
*
* <p>All four routines below ({@code ellipticK}, {@code ellipj}, {@code inverseJacobiSn}, {@code
* ellipticDegree}) follow the derivations and equation numbers in:
*
* <p>Orfanidis, "Introduction to Signal Processing Second Edition (2023)",
* https://rutgers.app.box.com/s/92is8ajwe2b0liokflkqx1ul2fqqqa7l
*
* <p>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<Double> aSeq = new ArrayList<>();
List<Double> 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.
*
* <p>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<Double> 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<Complex> 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.
*
* <pre>
* N · K(m) / K(1-m) = K(m1) / K(1-m1)
* </pre>
*
* <p>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;
}
}

328
wpimath/src/main/java/org/wpilib/math/filter/internal/Zpk.java Normal file
View File

@@ -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.
*
* <pre>
* H(s) = gain · ∏(s - z_i) / ∏(s - p_j) (analog)
* H(z) = gain · ∏(z - z_i) / ∏(z - p_j) (digital)
* </pre>
*
* <p>Complex roots must appear in conjugate pairs; that invariant is preserved by every transform
* below.
*
* <p>The four {@code analogLpTo*} helpers are the standard frequency-domain spectral
* transformations (Oppenheim &amp; 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:
*
* <ul>
* <li>{@code analogLpToLp} ↔ {@code scipy.signal.lp2lp_zpk}
* <li>{@code analogLpToHp} ↔ {@code scipy.signal.lp2hp_zpk}
* <li>{@code analogLpToBp} ↔ {@code scipy.signal.lp2bp_zpk}
* <li>{@code analogLpToBs} ↔ {@code scipy.signal.lp2bs_zpk}
* </ul>
*
* <p>Source for all four: https://github.com/scipy/scipy/blob/main/scipy/signal/_filter_design.py
*
* <p>{@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<Complex> zeros = new ArrayList<>();
final List<Complex> 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<Complex> complexPairs = new ArrayList<>();
final List<Double> 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<Complex> 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<BiquadFilter.Section> 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<BiquadFilter.Section> 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;
}
}

300
wpimath/src/main/native/cpp/filter/BiquadFilterDesign.cpp Normal file
View File

@@ -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 <cmath>
#include <numbers>
#include <stdexcept>
#include <string>
#include <vector>
#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<Section> 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<double>(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

282
wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.cpp Normal file
View File

@@ -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 <cmath>
#include <complex>
#include <numbers>
#include <stdexcept>
#include <vector>
#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<double>(-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<double>(-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<double>(-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<int> 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<JacobiResult> 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<cplx> 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<cplx> 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

42
wpimath/src/main/native/cpp/filter/internal/AnalogPrototypes.hpp Normal file
View File

@@ -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

107
wpimath/src/main/native/cpp/filter/internal/BilinearTransform.cpp Normal file
View File

@@ -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 <cmath>
#include <numbers>
#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<int>(p.poles.size()) - static_cast<int>(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

38
wpimath/src/main/native/cpp/filter/internal/BilinearTransform.hpp Normal file
View File

@@ -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

182
wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.cpp Normal file
View File

@@ -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 <cmath>
#include <complex>
#include <limits>
#include <numbers>
#include <vector>
// 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<double>::quiet_NaN();
}
if (m == 1.0) {
return std::numeric_limits<double>::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<double>::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<double> a;
std::vector<double> 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<double>::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<double>::quiet_NaN(),
std::numeric_limits<double>::quiet_NaN()};
}
if (k == 1.0) {
// sn(z, 1) = tanh(z), so the inverse is atanh(w).
return std::atanh(w);
}
std::vector<double> 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<cplx> 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<double>(i) * (i + 1));
}
double den = 1.0;
for (int i = 1; i <= kMmax + 1; ++i) {
den += 2.0 * std::pow(q, static_cast<double>(i) * i);
}
double ratio = num / den;
return 16.0 * q * ratio * ratio * ratio * ratio;
}
} // namespace wpi::math::filter::internal

60
wpimath/src/main/native/cpp/filter/internal/JacobiElliptic.hpp Normal file
View File

@@ -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 <complex>
namespace wpi::math::filter::internal {
using cplx = std::complex<double>;
/**
* 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

305
wpimath/src/main/native/cpp/filter/internal/Zpk.cpp Normal file
View File

@@ -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 <algorithm>
#include <cmath>
#include <stdexcept>
#include <utility>
#include <vector>
// 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<cplx> complexPairs; // one representative per conjugate pair
std::vector<double> realRoots;
};
Partitioned Partition(const std::vector<cplx>& roots) {
Partitioned out;
std::vector<bool> 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<int>(p.poles.size()) - static_cast<int>(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<cplx, cplx> 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<int>(polePart.complexPairs.size());
std::vector<cplx> cplxZeroForPole(numCplxPoles, {0.0, 0.0});
std::vector<bool> 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

58
wpimath/src/main/native/cpp/filter/internal/Zpk.hpp Normal file
View File

@@ -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 <complex>
#include <vector>
#include "wpi/math/filter/BiquadFilter.hpp"
namespace wpi::math::filter::internal {
using cplx = std::complex<double>;
using Section = wpi::math::BiquadFilter::Section;
using Sections = std::vector<Section>;
/**
* 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<cplx> zeros;
std::vector<cplx> 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

399
wpimath/src/main/native/include/wpi/math/filter/BiquadFilter.hpp Normal file
View File

@@ -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 <array>
#include <initializer_list>
#include <span>
#include <stdexcept>
#include <string>
#include <type_traits>
#include <vector>
#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:<br>
* y[n] = b₀ x[n] + s₁[n-1]<br>
* s₁[n] = b₁ x[n] - a₁ y[n] + s₂[n-1]<br>
* 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<const Section> 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<Section> sections)
: BiquadFilter(
std::span<const Section>{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<const Section> 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<Section> m_sections;
std::vector<std::array<double, 2>> m_state;
double m_lastOutput = 0.0;
inline static int instances = 0;
};
} // namespace wpi::math

6
wpimath/src/main/native/include/wpi/math/filter/LinearFilter.hpp
View File

@@ -61,6 +61,12 @@ namespace wpi::math {
* https://en.wikipedia.org/wiki/Iir_filter<br>
* https://en.wikipedia.org/wiki/Fir_filter<br>
*
* 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.

1
wpimath/src/main/python/pyproject.toml
View File

@@ -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"

52
wpimath/src/main/python/semiwrap/BiquadFilter.yml Normal file
View File

@@ -0,0 +1,52 @@
classes:
wpi::math::BiquadFilter:
enums:
Kind:
methods:
BiquadFilter:
overloads:
std::span<const Section>:
std::initializer_list<Section>:
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);
})

2
wpimath/src/main/python/wpimath/__init__.py
View File

@@ -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",