butterworth.hpp

Go to the documentation of this file.

/*****************************************************************************/
/********** !!! WARNING: CODE GENERATED BY TAPROOT. DO NOT EDIT !!! **********/
/*****************************************************************************/
 
/*
 * Copyright (c) 2024-2025 Advanced Robotics at the University of Washington <robomstr@uw.edu>
 *
 * This file is part of Taproot.
 *
 * Taproot is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 *
 * Taproot is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with Taproot.  If not, see <https://www.gnu.org/licenses/>.
 */
 
#ifndef TAPROOT_BUTTERWORTH_HPP_
#define TAPROOT_BUTTERWORTH_HPP_
#include <array>
#include <cmath>
#include <complex>
#include <cstdint>
 
#include "modm/math/geometry/angle.hpp"
 
namespace tap
{
namespace algorithms
{
enum FilterType : uint8_t
{
    LOWPASS = 0b00,
    HIGHPASS = 0b01,
    BANDPASS = 0b10,
    BANDSTOP = 0b11,
};
 
constexpr std::complex<double> s2z(std::complex<double> s, double Ts)
{
    return (1.0 + (Ts / 2) * s) / (1.0 - (Ts / 2) * s);
}
 
template <uint8_t ORDER>
constexpr std::array<double, ORDER + 1> expandPolynomial(
    std::array<std::complex<double>, ORDER> zeros)
{
    std::array<std::complex<double>, ORDER + 1> coefficients{
        std::complex<double>(1.0, 0.0)};  // Start with 1
 
    for (size_t i = 0; i < ORDER; i++)
    {
        // Multiply current polynomial by (x - zero)
        std::array<std::complex<double>, ORDER + 1> newCoefficients{std::complex<double>(0.0, 0.0)};
        for (size_t j = 0; j < i + 1; j++)
        {
            newCoefficients[j] -=
                coefficients[j] * zeros[i];             // Multiply current coefficient by zero
            newCoefficients[j + 1] += coefficients[j];  // Shift current coefficient
        }
        coefficients.swap(newCoefficients);
    }
    std::array<double, ORDER + 1> stripped_coefficients{};
    // Extract the real part of each coefficient, the imaginary appears to be an
    // artifact of double
    for (size_t i = 0; i < ORDER + 1; i++)
    {
        stripped_coefficients[i] = coefficients[i].real();
    }
 
    return stripped_coefficients;
}
 
template <uint8_t ORDER>
constexpr std::complex<double> evaluateFrequencyResponse(
    const std::array<double, ORDER + 1> &b,
    const std::array<double, ORDER + 1> &a,
    double w,
    double Ts)
{
    std::complex<double> j(0, 1);  // imaginary unit i = sqrt(-1)
    std::complex<double> numerator(0, 0);
    std::complex<double> denominator(0, 0);
 
    double omega = w * Ts;  // omega is in terms of rad/sample
 
    // Evaluate numerator: b0 + b1 * e^{-jω} + b2 * e^{-j2ω} + ...
    for (size_t k = 0; k < b.size(); ++k)
    {
        numerator += b[k] * (std::cos(omega * static_cast<double>(k)) -
                             j * std::sin(omega * static_cast<double>(k)));
    }
 
    // Evaluate denominator: a0 + a1 * e^{-jω} + a2 * e^{-j2ω} + ...
    for (size_t k = 0; k < a.size(); ++k)
    {
        denominator += a[k] * (std::cos(omega * static_cast<double>(k)) -
                               j * std::sin(omega * static_cast<double>(k)));
    }
 
    return numerator / denominator;
}
constexpr double complex_abs(std::complex<double> z)
{
    return std::sqrt(z.real() * z.real() + z.imag() * z.imag());
}
 
constexpr std::complex<double> complex_sqrt(std::complex<double> z)
{
    double r = std::sqrt(complex_abs(z));
    double theta = std::atan2(z.imag(), z.real()) * 0.5;
    return {r * std::cos(theta), r * std::sin(theta)};
}
 
template <uint8_t ORDER, FilterType Type = LOWPASS, typename T = float>
class Butterworth
{
public:
    constexpr Butterworth(double wc, double Ts, double wh = 0.0)
        : naturalResponseCoefficients(),
          forcedResponseCoefficients()
    {
        const int n = ORDER;
 
        // For band filters we treat wc as ωl
        double wl = wc;
        double whp = wh;
        std::array<std::complex<double>, 2 * ORDER> bandpass_stop_poles;
 
        // pre-warp all edges for bilinear transform
        wl = (2.0 / Ts) * std::tan(wl * (Ts / 2.0));
        whp = (2.0 / Ts) * std::tan(whp * (Ts / 2.0));
 
        // generate N prototype poles on unit circle
        std::array<std::complex<double>, COEFFICIENTS - 1> poles;
        for (int k = 0; k < n; ++k)
        {
            double theta = M_PI * (2.0 * k + 1) / (2.0 * n) + M_PI / 2.0;
            poles[k] = std::complex<double>(std::cos(theta), std::sin(theta));
        }
 
        std::array<std::complex<double>, COEFFICIENTS - 1> zPoles;
 
        // apply the appropriate s-domain transform to each pole
        switch (Type)
        {
            case LOWPASS:
            {
                // poles are multiplied by wl to scale them to the desired cutoff frequency
                for (int j = 0; j < n; ++j) poles[j] = poles[j] * wl;
 
                // now map each analog pole into the z-plane
                for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts);
                break;
            }
            case HIGHPASS:
            {
                // applys the inverse transform of the butterworth lowpass filter
                for (int j = 0; j < n; ++j)
                {
                    poles[j] = wl / poles[j];
                }
                // now map each analog pole into the z-plane
                for (int i = 0; i < COEFFICIENTS - 1; ++i) zPoles[i] = s2z(poles[i], Ts);
                break;
            }
            // In the case of a bandpass or a bandstop the amount of poles doubles.
            case BANDPASS:
            {
                // check for validity of the filter edges
                if (whp <= wl)
                {
                    throw std::invalid_argument("wh must be > wl for BANDPASS");
                }
                /*
                 *  transform in the form of s → (s² + Ω₀²) / (B · s)
                 *  where:
                 *      Ω₀ = √(Ω_low * Ω_high)      // Center frequency (rad/sec)
                 *      B  = Ω_high - Ω_low         // Bandwidth (rad/sec)
                 */
                double B = whp - wl;
                double W0sq = whp * wl;
 
                for (int j = 0; j < ORDER; ++j)
                {
                    std::complex<double> p = poles[j];
 
                    std::complex<double> discriminant = (p * B) * (p * B) - 4.0 * W0sq;
                    std::complex<double> root = complex_sqrt(discriminant);
 
                    bandpass_stop_poles[2 * j] = (p * B + root) * 0.5;
                    bandpass_stop_poles[2 * j + 1] = (p * B - root) * 0.5;
                }
 
                // now map each analog pole into the z-plane
                for (int i = 0; i < COEFFICIENTS - 1; ++i)
                    zPoles[i] = s2z(bandpass_stop_poles[i], Ts);
 
                break;
            }
 
            case BANDSTOP:
            {
                // check for validity of the filter edges
                if (whp <= wl)
                {
                    throw std::invalid_argument("wh must be > wl for BANDSTOP");
                }
                /*
                 *  transform in the form of  s → B · s / (s² + Ω₀²)
                 *  where:
                 *      Ω₀ = √(Ω_low * Ω_high)      // Center frequency (rad/sec)
                 *      B  = Ω_high - Ω_low         // Bandwidth (rad/sec)
                 */
                double B = whp - wl;
                double W0sq = whp * wl;
                for (int j = 0; j < n; ++j)
                {
                    std::complex<double> p = poles[j];
 
                    std::complex<double> discriminant = B * B - (4.0 * -p * W0sq);
                    std::complex<double> root = complex_sqrt(discriminant);
 
                    bandpass_stop_poles[2 * j] = (B + root) / (2.0 * p);
                    bandpass_stop_poles[2 * j + 1] = (B - root) / (2.0 * p);
                }
 
                // now map each analog pole into the z-plane
                for (int i = 0; i < COEFFICIENTS - 1; ++i)
                    zPoles[i] = s2z(bandpass_stop_poles[i], Ts);
 
                break;
            }
            default:
                throw std::invalid_argument("Unknown filter type");
        }
 
        std::array<std::complex<double>, COEFFICIENTS - 1> zZeros;
 
        switch (Type)
        {
            case LOWPASS:
                // zeros: for Butterworth lowpass all z-zeros at z = –1
                zZeros.fill(std::complex<double>(-1, 0));
                break;
            case HIGHPASS:
                // zeros: for butterworth highpass all z-zeros are at z = 1
                zZeros.fill(std::complex<double>(1, 0));
                break;
            case BANDPASS:
                // zeros: for butterworth bandpass all z-zeros are at z = ±1
                for (int i = 0; i < COEFFICIENTS - 1; ++i)
                {
                    zZeros[i] =
                        (i % 2 == 0) ? std::complex<double>(1, 0) : std::complex<double>(-1, 0);
                }
                break;
            case BANDSTOP:
            {
                /* zeros: for butterworth bandstop are in a circle with radius 1 at the
                 * center of the z-domain with an angle in radians per sample of the
                 * center frequency. The zeros are complex conjugates of each other.
                 */
 
                /* the notch (center) frequency in radians/sample */
                double omega0 = std::sqrt(wl * whp) * Ts;
 
                double realPart = std::cos(omega0);
                double imagPart = std::sin(omega0);
 
                std::complex<double> zeroPlus(realPart, imagPart);    // e^(+jω0)
                std::complex<double> zeroMinus(realPart, -imagPart);  // e^(-jω0)
 
                for (int i = 0; i < COEFFICIENTS - 1; i += 2)
                {
                    zZeros[i] = zeroPlus;                                 // first of pair
                    if (i + 1 < COEFFICIENTS) zZeros[i + 1] = zeroMinus;  // second of pair
                }
            }
            break;
        }
 
        // get expanded polynomials
        auto b = expandPolynomial<COEFFICIENTS - 1>(zZeros);
        auto a = expandPolynomial<COEFFICIENTS - 1>(zPoles);
 
        // scale the gain properly
        switch (Type)
        {
            case LOWPASS:
            {
                // Eval at DC
                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, 0, Ts);
                auto mag = complex_abs(freqResp);
                double scale = 1 / mag;
                for (auto &coef : b) coef *= scale;
                break;
            }
            case HIGHPASS:
            {
                // Eval at niquest
                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, M_PI / Ts, Ts);
                auto mag = complex_abs(freqResp);
                double scale = 1 / mag;
                for (auto &coef : b) coef *= scale;
                break;
            }
            case BANDPASS:
            {
                // Eval at center frequency
                auto freqResp =
                    evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, std::sqrt(wl * whp), Ts);
                auto mag = complex_abs(freqResp);
                double scale = 1 / mag;
                for (auto &coef : b) coef *= scale;
                break;
            }
            case BANDSTOP:
            {
                // Eval at dc gain
                auto freqResp = evaluateFrequencyResponse<COEFFICIENTS - 1>(b, a, 0, Ts);
                auto mag = complex_abs(freqResp);
                double scale = 1 / mag;
                for (auto &coef : b) coef *= scale;
                break;
            }
        }
 
        // store in member arrays (reverse order)
        for (size_t i = 0; i < COEFFICIENTS; ++i)
        {
            naturalResponseCoefficients[COEFFICIENTS - i - 1] = a[i];
            forcedResponseCoefficients[COEFFICIENTS - i - 1] = b[i];
        }
    }
 
    static constexpr int COEFFICIENTS = (1 + ((Type & 0b10) != 0)) * ORDER + 1;
 
    std::array<T, COEFFICIENTS> getNaturalResponseCoefficients() const
    {
        return naturalResponseCoefficients;
    }
 
    std::array<T, COEFFICIENTS> getForcedResponseCoefficients() const
    {
        return forcedResponseCoefficients;
    }
 
private:
    std::array<T, COEFFICIENTS> naturalResponseCoefficients;
    std::array<T, COEFFICIENTS> forcedResponseCoefficients;
};
 
}  // namespace algorithms
 
}  // namespace tap
 
#endif  // TAPROOT_BUTTERWORTH_HPP_