IIRFilter.hpp

Go to the documentation of this file.

#pragma once
 
#include <AH/Containers/Array.hpp>
#include <AH/STL/type_traits>
#include <Filters/TransferFunction.hpp>
 
 
template <uint8_t NB, uint8_t NA, class T>
class NonNormalizingIIRFilter {
  public:
    NonNormalizingIIRFilter(const AH::Array<T, NB> &b_coefficients,
                            const AH::Array<T, NA> &a_coefficients)
        : a0(a_coefficients[0]) {
        for (uint8_t i = 0; i < 2 * NB - 1; ++i)
            this->b_coefficients[i] = b_coefficients[(2 * NB - 1 - i) % NB];
        for (uint8_t i = 0; i < 2 * MA - 1; ++i)
            this->a_coefficients[i] = a_coefficients[(2 * MA - 2 - i) % MA + 1];
    }
 
    NonNormalizingIIRFilter(const TransferFunction<NB, NA, T> &tf)
        : NonNormalizingIIRFilter{tf.b, tf.a} {}
 
    T operator()(T input) {
        // Save the new input to the ring buffer.
        x[index_b] = input;
 
        // Calculate the offset to the shifted coefficients.
        T *b_coeff_shift = b_coefficients.end() - NB - index_b;
        T *a_coeff_shift = a_coefficients.end() - MA - index_a;
 
        // Multiply and accumulate the inputs and their respective coefficients.
        T acc = {};
        for (uint8_t i = 0; i < NB; i++)
            acc += x[i] * b_coeff_shift[i];
 
        // Multiply and accumulate the inputs and their respective coefficients.
        for (uint8_t i = 0; i < MA; i++)
            acc -= y[i] * a_coeff_shift[i];
 
        // Save the current output
        acc /= a0;
        y[index_a] = acc;
 
        // Increment and wrap around the index of the ring buffer.
        index_b++;
        if (index_b == NB)
            index_b = 0;
 
        index_a++;
        if (index_a == MA)
            index_a = 0;
 
        return acc;
    }
 
  private:
    constexpr static uint8_t MA = NA - 1;
    uint8_t index_b = 0, index_a = 0;
    AH::Array<T, NB> x = {};                 
    AH::Array<T, MA> y = {};                 
    AH::Array<T, 2 * NB - 1> b_coefficients; 
    AH::Array<T, 2 * MA - 1> a_coefficients; 
    T a0;
};
 
template <uint8_t NB, uint8_t NA, class T>
class NormalizingIIRFilter {
  public:
    NormalizingIIRFilter(const AH::Array<T, NB> &b_coefficients,
                         const AH::Array<T, NA> &a_coefficients) {
        T a0 = a_coefficients[0];
        for (uint8_t i = 0; i < 2 * NB - 1; ++i)
            this->b_coefficients[i] =
                b_coefficients[(2 * NB - 1 - i) % NB] / a0;
        for (uint8_t i = 0; i < 2 * MA - 1; ++i)
            this->a_coefficients[i] =
                a_coefficients[(2 * MA - 2 - i) % MA + 1] / a0;
    }
 
    NormalizingIIRFilter(const TransferFunction<NB, NA, T> &tf)
        : NormalizingIIRFilter{tf.b, tf.a} {}
 
    T operator()(T input) {
        // Save the new input to the ring buffer.
        x[index_b] = input;
 
        // Calculate the offset to the shifted coefficients.
        T *b_coeff_shift = b_coefficients.end() - NB - index_b;
        T *a_coeff_shift = a_coefficients.end() - MA - index_a;
 
        // Multiply and accumulate the inputs and their respective coefficients.
        T acc = {};
        for (uint8_t i = 0; i < NB; i++)
            acc += x[i] * b_coeff_shift[i];
 
        // Multiply and accumulate the inputs and their respective coefficients.
        for (uint8_t i = 0; i < MA; i++)
            acc -= y[i] * a_coeff_shift[i];
 
        // Save the current output
        y[index_a] = acc;
 
        // Increment and wrap around the index of the ring buffer.
        index_b++;
        if (index_b == NB)
            index_b = 0;
 
        index_a++;
        if (index_a == MA)
            index_a = 0;
 
        return acc;
    }
 
  private:
    constexpr static uint8_t MA = NA - 1;
    uint8_t index_b = 0, index_a = 0;
    AH::Array<T, NB> x = {};
    AH::Array<T, MA> y = {};
    AH::Array<T, 2 * NB - 1> b_coefficients;
    AH::Array<T, 2 * MA - 1> a_coefficients;
};
 
 
template <uint8_t NB, uint8_t NA, class T>
using IIRImplementation =
    typename std::conditional<std::is_floating_point<T>::value,
                              NormalizingIIRFilter<NB, NA, T>,
                              NonNormalizingIIRFilter<NB, NA, T>>::type;
 
 
template <uint8_t NB, uint8_t NA = NB, class T = float>
class IIRFilter : public IIRImplementation<NB, NA, T> {
  public:
    IIRFilter(const AH::Array<T, NB> &b_coefficients,
              const AH::Array<T, NA> &a_coefficients)
        : IIRImplementation<NB, NA, T>{b_coefficients, a_coefficients} {}
 
    IIRFilter(const TransferFunction<NB, NA, T> &tf)
        : IIRImplementation<NB, NA, T>{tf} {}
 
    T operator()(T input) {
        return IIRImplementation<NB, NA, T>::operator()(input);
    }
};
 
template <size_t NB, size_t NA, class T = float>
IIRFilter<NB, NA, T> makeIIRFilter(const TransferFunction<NB, NA, T> &tf) {
    return tf;
}
 
template <size_t NB, size_t NA, class T = float>
IIRFilter<NB, NA, T> makeIIRFilter(const AH::Array<T, NB> &b_coefficients,
                                   const AH::Array<T, NA> &a_coefficients) {
    return {b_coefficients, a_coefficients};
}