Browse Source

Add IIRFilter.

tags/v1.1.6
Andrew Belt 3 years ago
parent
commit
0a930b0ba0
2 changed files with 180 additions and 113 deletions
  1. +158
    -113
      include/dsp/filter.hpp
  2. +22
    -0
      include/simd/functions.hpp

+ 158
- 113
include/dsp/filter.hpp View File

@@ -182,20 +182,108 @@ struct TExponentialSlewLimiter {
typedef TExponentialSlewLimiter<> ExponentialSlewLimiter;


template <typename T = float>
struct TBiquadFilter {
/** input state */
T x[2];
/** Digital IIR filter processor.
https://en.wikipedia.org/wiki/Infinite_impulse_response
*/
template <int B_ORDER, int A_ORDER, typename T = float>
struct IIRFilter {
/** transfer function numerator coefficients: b_0, b_1, etc.
*/
T b[B_ORDER] = {};
/** transfer function denominator coefficients: a_1, a_2, etc.
a_0 is fixed to 1 and omitted from the `a` array, so its indices are shifted down by 1.
*/
T a[A_ORDER - 1] = {};
/** input state
x[0] = x_{n-1}
x[1] = x_{n-2}
etc.
*/
T x[B_ORDER - 1];
/** output state */
T y[2];
T y[A_ORDER - 1];

IIRFilter() {
reset();
}

/** transfer function numerator coefficients: b_0, b_1, b_2 */
float b[3];
/** transfer function denominator coefficients: a_1, a_2
a_0 is fixed to 1.
void reset() {
for (int i = 1; i < B_ORDER; i++) {
x[i - 1] = 0.f;
}
for (int i = 1; i < A_ORDER; i++) {
y[i - 1] = 0.f;
}
}

void setCoefficients(const T* b, const T* a) {
for (int i = 0; i < B_ORDER; i++) {
this->b[i] = b[i];
}
for (int i = 1; i < A_ORDER; i++) {
this->a[i - 1] = a[i - 1];
}
}

T process(T in) {
T out = 0.f;
// Add x state
if (0 < B_ORDER) {
out = b[0] * in;
}
for (int i = 1; i < B_ORDER; i++) {
out += b[i] * x[i - 1];
}
// Subtract y state
for (int i = 1; i < A_ORDER; i++) {
out -= a[i - 1] * y[i - 1];
}
// Shift x state
for (int i = B_ORDER - 1; i >= 2; i--) {
x[i - 1] = x[i - 2];
}
x[0] = in;
// Shift y state
for (int i = A_ORDER - 1; i >= 2; i--) {
y[i - 1] = y[i - 2];
}
y[0] = out;
return out;
}

/** Computes the complex transfer function $H(s)$ at a particular frequency
s: normalized angular frequency equal to $2 \pi f / f_{sr}$ ($\pi$ is the Nyquist frequency)
*/
float a[2];
std::complex<T> getTransferFunction(T s) {
// Compute sum(a_k z^-k) / sum(b_k z^-k) where z = e^(i s)
std::complex<T> bSum(b[0], 0);
std::complex<T> aSum(1, 0);
for (int i = 1; i < std::max(B_ORDER, A_ORDER); i++) {
T p = -i * s;
std::complex<T> z(simd::cos(p), simd::sin(p));
if (i < B_ORDER)
bSum += b[i] * z;
if (i < A_ORDER)
aSum += a[i - 1] * z;
}
return bSum / aSum;
}

T getFrequencyResponse(T f) {
// T hReal, hImag;
// getTransferFunction(2 * M_PI * f, &hReal, &hImag);
// return simd::hypot(hReal, hImag);
return simd::abs(getTransferFunction(2 * M_PI * f));
}

T getFrequencyPhase(T f) {
return simd::arg(getTransferFunction(2 * M_PI * f));
}
};


template <typename T = float>
struct TBiquadFilter : IIRFilter<3, 3, T> {
enum Type {
LOWPASS_1POLE,
HIGHPASS_1POLE,
@@ -210,28 +298,9 @@ struct TBiquadFilter {
};

TBiquadFilter() {
reset();
setParameters(LOWPASS, 0.f, 0.f, 1.f);
}

void reset() {
std::memset(x, 0, sizeof(x));
std::memset(y, 0, sizeof(y));
}

T process(T in) {
// Advance IIR
T out = b[0] * in + b[1] * x[0] + b[2] * x[1]
- a[0] * y[0] - a[1] * y[1];
// Push input
x[1] = x[0];
x[0] = in;
// Push output
y[1] = y[0];
y[0] = out;
return out;
}

/** Calculates and sets the biquad transfer function coefficients.
f: normalized frequency (cutoff frequency / sample rate), must be less than 0.5
Q: quality factor
@@ -241,37 +310,37 @@ struct TBiquadFilter {
float K = std::tan(M_PI * f);
switch (type) {
case LOWPASS_1POLE: {
a[0] = -std::exp(-2.f * M_PI * f);
a[1] = 0.f;
b[0] = 1.f + a[0];
b[1] = 0.f;
b[2] = 0.f;
this->a[0] = -std::exp(-2.f * M_PI * f);
this->a[1] = 0.f;
this->b[0] = 1.f + this->a[0];
this->b[1] = 0.f;
this->b[2] = 0.f;
} break;

case HIGHPASS_1POLE: {
a[0] = std::exp(-2.f * M_PI * (0.5f - f));
a[1] = 0.f;
b[0] = 1.f - a[0];
b[1] = 0.f;
b[2] = 0.f;
this->a[0] = std::exp(-2.f * M_PI * (0.5f - f));
this->a[1] = 0.f;
this->b[0] = 1.f - this->a[0];
this->b[1] = 0.f;
this->b[2] = 0.f;
} break;

case LOWPASS: {
float norm = 1.f / (1.f + K / Q + K * K);
b[0] = K * K * norm;
b[1] = 2.f * b[0];
b[2] = b[0];
a[0] = 2.f * (K * K - 1.f) * norm;
a[1] = (1.f - K / Q + K * K) * norm;
this->b[0] = K * K * norm;
this->b[1] = 2.f * this->b[0];
this->b[2] = this->b[0];
this->a[0] = 2.f * (K * K - 1.f) * norm;
this->a[1] = (1.f - K / Q + K * K) * norm;
} break;

case HIGHPASS: {
float norm = 1.f / (1.f + K / Q + K * K);
b[0] = norm;
b[1] = -2.f * b[0];
b[2] = b[0];
a[0] = 2.f * (K * K - 1.f) * norm;
a[1] = (1.f - K / Q + K * K) * norm;
this->b[0] = norm;
this->b[1] = -2.f * this->b[0];
this->b[2] = this->b[0];
this->a[0] = 2.f * (K * K - 1.f) * norm;
this->a[1] = (1.f - K / Q + K * K) * norm;

} break;

@@ -279,19 +348,19 @@ struct TBiquadFilter {
float sqrtV = std::sqrt(V);
if (V >= 1.f) {
float norm = 1.f / (1.f + M_SQRT2 * K + K * K);
b[0] = (1.f + M_SQRT2 * sqrtV * K + V * K * K) * norm;
b[1] = 2.f * (V * K * K - 1.f) * norm;
b[2] = (1.f - M_SQRT2 * sqrtV * K + V * K * K) * norm;
a[0] = 2.f * (K * K - 1.f) * norm;
a[1] = (1.f - M_SQRT2 * K + K * K) * norm;
this->b[0] = (1.f + M_SQRT2 * sqrtV * K + V * K * K) * norm;
this->b[1] = 2.f * (V * K * K - 1.f) * norm;
this->b[2] = (1.f - M_SQRT2 * sqrtV * K + V * K * K) * norm;
this->a[0] = 2.f * (K * K - 1.f) * norm;
this->a[1] = (1.f - M_SQRT2 * K + K * K) * norm;
}
else {
float norm = 1.f / (1.f + M_SQRT2 / sqrtV * K + K * K / V);
b[0] = (1.f + M_SQRT2 * K + K * K) * norm;
b[1] = 2.f * (K * K - 1) * norm;
b[2] = (1.f - M_SQRT2 * K + K * K) * norm;
a[0] = 2.f * (K * K / V - 1.f) * norm;
a[1] = (1.f - M_SQRT2 / sqrtV * K + K * K / V) * norm;
this->b[0] = (1.f + M_SQRT2 * K + K * K) * norm;
this->b[1] = 2.f * (K * K - 1) * norm;
this->b[2] = (1.f - M_SQRT2 * K + K * K) * norm;
this->a[0] = 2.f * (K * K / V - 1.f) * norm;
this->a[1] = (1.f - M_SQRT2 / sqrtV * K + K * K / V) * norm;
}
} break;

@@ -299,86 +368,62 @@ struct TBiquadFilter {
float sqrtV = std::sqrt(V);
if (V >= 1.f) {
float norm = 1.f / (1.f + M_SQRT2 * K + K * K);
b[0] = (V + M_SQRT2 * sqrtV * K + K * K) * norm;
b[1] = 2.f * (K * K - V) * norm;
b[2] = (V - M_SQRT2 * sqrtV * K + K * K) * norm;
a[0] = 2.f * (K * K - 1.f) * norm;
a[1] = (1.f - M_SQRT2 * K + K * K) * norm;
this->b[0] = (V + M_SQRT2 * sqrtV * K + K * K) * norm;
this->b[1] = 2.f * (K * K - V) * norm;
this->b[2] = (V - M_SQRT2 * sqrtV * K + K * K) * norm;
this->a[0] = 2.f * (K * K - 1.f) * norm;
this->a[1] = (1.f - M_SQRT2 * K + K * K) * norm;
}
else {
float norm = 1.f / (1.f / V + M_SQRT2 / sqrtV * K + K * K);
b[0] = (1.f + M_SQRT2 * K + K * K) * norm;
b[1] = 2.f * (K * K - 1.f) * norm;
b[2] = (1.f - M_SQRT2 * K + K * K) * norm;
a[0] = 2.f * (K * K - 1.f / V) * norm;
a[1] = (1.f / V - M_SQRT2 / sqrtV * K + K * K) * norm;
this->b[0] = (1.f + M_SQRT2 * K + K * K) * norm;
this->b[1] = 2.f * (K * K - 1.f) * norm;
this->b[2] = (1.f - M_SQRT2 * K + K * K) * norm;
this->a[0] = 2.f * (K * K - 1.f / V) * norm;
this->a[1] = (1.f / V - M_SQRT2 / sqrtV * K + K * K) * norm;
}
} break;

case BANDPASS: {
float norm = 1.f / (1.f + K / Q + K * K);
b[0] = K / Q * norm;
b[1] = 0.f;
b[2] = -b[0];
a[0] = 2.f * (K * K - 1.f) * norm;
a[1] = (1.f - K / Q + K * K) * norm;
this->b[0] = K / Q * norm;
this->b[1] = 0.f;
this->b[2] = -this->b[0];
this->a[0] = 2.f * (K * K - 1.f) * norm;
this->a[1] = (1.f - K / Q + K * K) * norm;
} break;

case PEAK: {
if (V >= 1.f) {
float norm = 1.f / (1.f + K / Q + K * K);
b[0] = (1.f + K / Q * V + K * K) * norm;
b[1] = 2.f * (K * K - 1.f) * norm;
b[2] = (1.f - K / Q * V + K * K) * norm;
a[0] = b[1];
a[1] = (1.f - K / Q + K * K) * norm;
this->b[0] = (1.f + K / Q * V + K * K) * norm;
this->b[1] = 2.f * (K * K - 1.f) * norm;
this->b[2] = (1.f - K / Q * V + K * K) * norm;
this->a[0] = this->b[1];
this->a[1] = (1.f - K / Q + K * K) * norm;
}
else {
float norm = 1.f / (1.f + K / Q / V + K * K);
b[0] = (1.f + K / Q + K * K) * norm;
b[1] = 2.f * (K * K - 1.f) * norm;
b[2] = (1.f - K / Q + K * K) * norm;
a[0] = b[1];
a[1] = (1.f - K / Q / V + K * K) * norm;
this->b[0] = (1.f + K / Q + K * K) * norm;
this->b[1] = 2.f * (K * K - 1.f) * norm;
this->b[2] = (1.f - K / Q + K * K) * norm;
this->a[0] = this->b[1];
this->a[1] = (1.f - K / Q / V + K * K) * norm;
}
} break;

case NOTCH: {
float norm = 1.f / (1.f + K / Q + K * K);
b[0] = (1.f + K * K) * norm;
b[1] = 2.f * (K * K - 1.f) * norm;
b[2] = b[0];
a[0] = b[1];
a[1] = (1.f - K / Q + K * K) * norm;
this->b[0] = (1.f + K * K) * norm;
this->b[1] = 2.f * (K * K - 1.f) * norm;
this->b[2] = this->b[0];
this->a[0] = this->b[1];
this->a[1] = (1.f - K / Q + K * K) * norm;
} break;

default: break;
}
}

void copyParameters(const TBiquadFilter<T>& from) {
b[0] = from.b[0];
b[1] = from.b[1];
b[2] = from.b[2];
a[0] = from.a[0];
a[1] = from.a[1];
}

/** Computes the gain of a particular frequency
f: normalized frequency
*/
float getFrequencyResponse(float f) {
// Compute sum(a_k e^(-i k f))
std::complex<float> bsum = b[0];
std::complex<float> asum = 1.f;
for (int i = 1; i < 3; i++) {
float p = 2 * M_PI * -i * f;
std::complex<float> e(std::cos(p), std::sin(p));
bsum += b[i] * e;
asum += a[i - 1] * e;
}
return std::abs(bsum / asum);
}
};

typedef TBiquadFilter<> BiquadFilter;


+ 22
- 0
include/simd/functions.hpp View File

@@ -168,6 +168,12 @@ inline float_4 fmod(float_4 a, float_4 b) {
return a - trunc(a / b) * b;
}

using std::hypot;

inline float_4 hypot(float_4 a, float_4 b) {
return sqrt(a * a + b * b);
}

using std::fabs;

inline float_4 fabs(float_4 a) {
@@ -176,6 +182,22 @@ inline float_4 fabs(float_4 a) {
return a & float_4::cast(mask);
}

using std::abs;

inline float_4 abs(float_4 a) {
return fabs(a);
}

inline float_4 abs(std::complex<float_4> a) {
return hypot(a.real(), a.imag());
}

using std::arg;

inline float_4 arg(std::complex<float_4> a) {
return atan2(a.imag(), a.real());
}

using std::pow;

inline float_4 pow(float_4 a, float_4 b) {


Loading…
Cancel
Save