#include <math.h>
|
#include "const.h"
|
#include "IIRFilter.h"
|
|
void IIRFilter::call_peaking_eq_with_lowfreq(double fc, double gain, double q, int fs
|
, double* coef_b, double* coef_a)
|
{
|
double A, H, k;
|
double a, d;
|
double f2, f1;
|
double ftemp = 0.0;
|
double bw;
|
|
ftemp = sqrt(1 + 1 / (4 * q * q));
|
f1 = fc * (ftemp - 0.5 / q);
|
f2 = fc * (ftemp + 0.5 / q);
|
if (f2 > 22000)
|
f2 = 22000;
|
|
bw = f2 - f1;
|
if (bw < 0.1)
|
bw = 0.1;
|
|
|
A = pow(10, (gain * 0.05));
|
k = tan(pi * bw / fs);
|
|
if (A < 1.0)
|
a = (k - A) / (k + A);
|
else
|
a = (k - 1) / (k + 1);
|
|
H = (A - 1) / 2;
|
d = -cos(2 * pi * fc / fs);
|
|
coef_b[0] = 1 + (1 + a) * H;
|
coef_b[1] = d * (1 - a);
|
coef_b[2] = (-a - (coef_b[0] - 1)); //( - a - (1 + a) * H / 2);
|
|
coef_a[0] = 1;
|
coef_a[1] = coef_b[1];
|
coef_a[2] = -a;
|
|
}
|
|
void IIRFilter::call_peaking_eq(double fc, double gain, double q, int fs, double* coef_b, double* coef_a)
|
{
|
double bw;
|
double deltaw;
|
double g0, g, gb;
|
double beta;
|
double w0;
|
double f2, f1;
|
double ftemp;
|
|
if (gain<0.1 && gain>-0.1) {
|
coef_b[0] = coef_a[0] = 1;
|
coef_b[1] = coef_b[2] = coef_a[1] = coef_a[2] = 0;
|
return;
|
}
|
|
if (fc < 100) return call_peaking_eq_with_lowfreq(fc, gain, q, fs, coef_b, coef_a);
|
//bw = fc / q;
|
//if (bw >= fs / 2) bw = fs / 2 - 100;
|
ftemp = sqrt(1 + 1 / (4 * q * q));
|
f1 = fc * (ftemp - 0.5 / q);
|
f2 = fc * (ftemp + 0.5 / q);
|
if (f2 >= fs / 2.0f) f2 = fs / 2.0f - 100.0f;
|
|
bw = f2 - f1;
|
w0 = 2 * pi * fc / fs;
|
deltaw = 2 * pi * bw / fs;
|
|
g0 = 1.0;
|
g = pow(10, (gain / 20)) * g0;
|
|
if (fabs(gain) <= 6) {
|
gb = pow(10, gain / 40);
|
}
|
else if (gain > 0) {
|
gb = pow(10, (gain - 3) / 20); //·åÖµÒÔÏÂ-3dB´¦´ø¿í
|
}
|
else {
|
gb = pow(10, (gain + 3) / 20); //·åÖµÒÔÉÏ3dB´¦´ø¿í
|
}
|
|
beta = sqrt((gb * gb - g0 * g0) / (g * g - gb * gb)) * tan(deltaw / 2);
|
|
coef_a[0] = 1;
|
coef_a[1] = -2 * (cos(w0) / (1 + beta));
|
coef_a[2] = (1 - beta) / (1 + beta);
|
|
coef_b[0] = (g0 + g * beta) / (1 + beta);
|
coef_b[1] = -2 * (g0 * cos(w0) / (1 + beta));
|
coef_b[2] = (g0 - g * beta) / (1 + beta);
|
}
|
|
int IIRFilter::eq(IIRBANDType type, int f0, double g, double q, int fs, double(*sos)[6])
|
{
|
double A = sqrt(pow(10, g * 0.05));
|
double w0 = 2 * pi * f0 / fs;
|
double alpha = sin(w0) / (2 * q);
|
double* b = sos[0];
|
double* a = sos[0] + 3;
|
|
switch (type)
|
{
|
case lowpass:
|
A = pow(10, g * 0.05);
|
|
b[0] = A * (1 - cos(w0)) / 2;
|
b[1] = A * (1 - cos(w0));
|
b[2] = A * (1 - cos(w0)) / 2;
|
|
a[0] = 1 + alpha;
|
a[1] = -2 * cos(w0);
|
a[2] = 1 - alpha;
|
break;
|
case highpass:
|
A = pow(10, g * 0.05);
|
b[0] = A * (1 + cos(w0)) / 2;
|
b[1] = -A * (1 + cos(w0));
|
b[2] = A * (1 + cos(w0)) / 2;
|
|
a[0] = 1 + alpha;
|
a[1] = -2 * cos(w0);
|
a[2] = 1 - alpha;
|
break;
|
case bpf_pq:
|
break;
|
case bpf_zero:
|
break;
|
case notch: {
|
double w0 = 2 * pi * f0 / fs;
|
double alpha = sin(w0) / (2 * q);
|
double A = pow(10.0, g / 40.0); // gain/40 ¶ø²»ÊÇ gain/20
|
|
// ·Ö×ÓϵÊý
|
b[0] = 1.0 + alpha * A;
|
b[1] = -2.0 * cos(w0);
|
b[2] = 1.0 - alpha * A;
|
|
// ·ÖĸϵÊý
|
a[0] = 1.0 + alpha / A;
|
a[1] = -2.0 * cos(w0);
|
a[2] = 1.0 - alpha / A;
|
}
|
break;
|
case apf:
|
A = pow(10, g * 0.05);
|
b[0] = A * (1 - alpha);
|
b[1] = A * -2 * cos(w0);
|
b[2] = A * (1 + alpha);
|
|
a[0] = 1 + alpha;
|
a[1] = -2 * cos(w0);
|
a[2] = 1 - alpha;
|
break;
|
case peaking_eq:
|
call_peaking_eq(f0, g, q, fs, b, a);
|
break;
|
case lowshelf: {
|
double cosw, sqrta;
|
|
A = pow(10, g * 0.025);
|
cosw = cos(w0);
|
sqrta = 2 * sqrt(A) * alpha;
|
|
b[0] = A * ((A + 1) - (A - 1) * cosw + sqrta);
|
b[1] = 2 * A * ((A - 1) - (A + 1) * cosw);
|
b[2] = A * ((A + 1) - (A - 1) * cosw - sqrta);
|
a[0] = (A + 1) + (A - 1) * cosw + sqrta;
|
a[1] = -2 * ((A - 1) + (A + 1) * cosw);
|
a[2] = (A + 1) + (A - 1) * cosw - sqrta;
|
}
|
break;
|
case highshelf: {
|
double cosw, sqrta;
|
|
A = pow(10, g * 0.025);
|
cosw = cos(w0);
|
sqrta = 2 * sqrt(A) * alpha;
|
|
b[0] = A * ((A + 1) + (A - 1) * cosw + sqrta);
|
b[1] = -2 * A * ((A - 1) + (A + 1) * cosw);
|
b[2] = A * ((A + 1) + (A - 1) * cosw - sqrta);
|
a[0] = (A + 1) - (A - 1) * cosw + sqrta;
|
a[1] = 2 * ((A - 1) - (A + 1) * cosw);
|
a[2] = (A + 1) - (A - 1) * cosw - sqrta;
|
}
|
break;
|
default:
|
return ret_param_error;
|
break;
|
}
|
|
return ret_success;
|
}
|
int IIRFilter::butters(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
|
{
|
double wc = tan(2 * pi * fc / fs / 2);
|
double w;
|
double re_p[8], im_p[8];
|
int i, nsection;
|
double a1;
|
double re_m, re_k, im_m, im_k;
|
|
nsection = (n + 1) / 2;
|
|
for (i = 0; i < n; i++)
|
{
|
w = (pi * (n + 2 * i + 1) / (2 * n));
|
re_p[i] = wc * cos(w);
|
im_p[i] = wc * sin(w);
|
}
|
|
if (type == IIRBANDType::lowpass)
|
{
|
for (i = 0; i < nsection; i++)
|
{
|
if (i == n - i - 1)
|
{
|
a1 = 1 - re_p[i];
|
sos[i][0] = wc / a1;
|
sos[i][1] = wc / a1;
|
sos[i][2] = 0;
|
sos[i][3] = 1;
|
sos[i][4] = (-re_p[i] - 1) / a1;
|
sos[i][5] = 0;
|
|
}
|
else
|
{
|
re_m = re_p[i] * re_p[n - i - 1] - im_p[i] * im_p[n - i - 1];
|
im_m = re_p[i] * im_p[n - i - 1] + im_p[i] * re_p[n - i - 1];
|
|
re_k = re_p[i] + re_p[n - i - 1];
|
im_k = im_p[i] + im_p[n - i - 1];
|
|
a1 = 1 - re_k + re_m;
|
|
sos[i][0] = wc * wc / a1;
|
sos[i][1] = 2 * wc * wc / a1;
|
sos[i][2] = wc * wc / a1;
|
sos[i][3] = 1;
|
sos[i][4] = (2 * re_m - 2) / a1;
|
sos[i][5] = (re_m + re_k + 1) / a1;
|
}
|
}
|
}
|
else if (type == IIRBANDType::highpass)
|
{
|
for (i = 0; i < nsection; i++)
|
{
|
if (i == n - i - 1)
|
{
|
a1 = wc * wc - re_p[i];
|
sos[i][0] = wc / a1;
|
sos[i][1] = -wc / a1;
|
sos[i][2] = 0;
|
sos[i][3] = 1;
|
sos[i][4] = (wc * wc + re_p[i]) / a1;
|
sos[i][5] = 0;
|
|
}
|
else
|
{
|
re_m = re_p[i] * re_p[n - i - 1] - im_p[i] * im_p[n - i - 1];
|
im_m = re_p[i] * im_p[n - i - 1] + im_p[i] * re_p[n - i - 1];
|
|
re_k = re_p[i] + re_p[n - i - 1];
|
im_k = im_p[i] + im_p[n - i - 1];
|
|
a1 = wc * wc * wc * wc - re_k * wc * wc + re_m;
|
|
sos[i][0] = wc * wc / a1;
|
sos[i][1] = -2 * wc * wc / a1;
|
sos[i][2] = wc * wc / a1;
|
sos[i][3] = 1;
|
sos[i][4] = (2 * wc * wc * wc * wc - 2 * re_m) / a1;
|
sos[i][5] = (wc * wc * wc * wc + re_k * wc * wc + re_m) / a1;
|
}
|
}
|
}
|
|
g = pow(10.0, g / 20.0);
|
sos[0][0] *= g;
|
sos[0][1] *= g;
|
sos[0][2] *= g;
|
|
return nsection;
|
}
|
|
int IIRFilter::linkwitzs(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
|
{
|
double q[5];
|
double wc = tan(2 * pi * fc / fs / 2);
|
int i;
|
int nsection = n / 2;
|
double a1;
|
|
switch (n)
|
{
|
case 2:
|
q[0] = 1 / 0.5;
|
break;
|
case 4:
|
q[0] = 1 / 0.71;
|
q[1] = 1 / 0.71;
|
break;
|
case 6:
|
q[0] = 1 / 0.5;
|
q[1] = 1.0;
|
q[2] = 1.0;
|
break;
|
case 8:
|
q[0] = 1 / 0.54;
|
q[1] = 1 / 1.34;
|
q[2] = 1 / 0.54;
|
q[3] = 1 / 1.34;
|
break;
|
case 10:
|
q[0] = 1 / 0.5;
|
q[1] = 1 / 0.62;
|
q[2] = 1 / 1.62;
|
q[3] = 1 / 0.62;
|
q[4] = 1 / 1.62;
|
break;
|
}
|
|
if (type == IIRBANDType::lowpass)
|
{
|
for (i = 0; i < nsection; i++)
|
{
|
a1 = 1 + q[i] * wc + wc * wc;
|
sos[i][0] = wc * wc / a1;
|
sos[i][1] = 2 * wc * wc / a1;
|
sos[i][2] = wc * wc / a1;
|
sos[i][3] = 1;
|
sos[i][4] = (2 * wc * wc - 2) / a1;
|
sos[i][5] = (wc * wc - q[i] * wc + 1) / a1;
|
|
}
|
}
|
else if (type == IIRBANDType::highpass)
|
{
|
for (i = 0; i < nsection; i++)
|
{
|
a1 = 1 + q[i] * wc + wc * wc;
|
sos[i][0] = 1 / a1;
|
sos[i][1] = -2 / a1;
|
sos[i][2] = 1 / a1;
|
sos[i][3] = 1;
|
sos[i][4] = (2 * wc * wc - 2) / a1;
|
sos[i][5] = (wc * wc - q[i] * wc + 1) / a1;
|
|
}
|
}
|
g = pow(10.0, g / 20.0);
|
sos[0][0] *= g;
|
sos[0][1] *= g;
|
sos[0][2] *= g;
|
|
return nsection;
|
}
|
int IIRFilter::bessels(IIRBANDType type, int n, int fc, int fs, double g, double(*sos)[6])
|
{
|
int nsection = (n + 1) / 2;
|
double wc = tan(2 * pi * fc / fs / 2);
|
double re_p[8],im_p[8];
|
int i;
|
double a1;
|
double constant[] = { 1.0, 1.272, 1.413, 1.533, 1.613, 1.735, 1.804, 1.956 };
|
int k = 0;
|
|
|
if (n == 1)
|
{
|
re_p[0] = -1; im_p[0] = 0;
|
}
|
else if (n == 2)
|
{
|
re_p[0] = -.8660254037844386467637229; im_p[0] = .4999999999999999999999996;
|
re_p[1] = -.8660254037844386467637229; im_p[1] = -.4999999999999999999999996;
|
}
|
else if (n == 3)
|
{
|
re_p[0] = -.9416000265332067855971980; im_p[0] = 0;
|
re_p[1] = -.7456403858480766441810907; im_p[1] = -.7113666249728352680992154;
|
re_p[2] = -.7456403858480766441810907; im_p[2] = .7113666249728352680992154;
|
}
|
else if (n == 4)
|
{
|
re_p[0] = -.6572111716718829545787781; im_p[0] = -.8301614350048733772399715;
|
re_p[1] = -.6572111716718829545787788; im_p[1] = .8301614350048733772399715;
|
re_p[2] = -.9047587967882449459642637; im_p[2] = -.2709187330038746636700923;
|
re_p[3] = -.9047587967882449459642624; im_p[3] = .2709187330038746636700926;
|
}
|
else if (n == 5)
|
{
|
re_p[0] = -.9264420773877602247196260; im_p[0] = 0;
|
re_p[1] = -.8515536193688395541722677; im_p[1] = -.4427174639443327209850002;
|
re_p[2] = -.8515536193688395541722677; im_p[2] = .4427174639443327209850002;
|
re_p[3] = -.5905759446119191779319432; im_p[3] = -.9072067564574549539291747;
|
re_p[4] = -.5905759446119191779319432; im_p[4] = .9072067564574549539291747;
|
}
|
else if (n == 6)
|
{
|
re_p[0] = -.9093906830472271808050953; im_p[0] = -.1856964396793046769246397;
|
re_p[1] = -.9093906830472271808050953; im_p[1] = .1856964396793046769246397;
|
re_p[2] = -.7996541858328288520243325; im_p[2] = -.5621717346937317988594118;
|
re_p[3] = -.7996541858328288520243325; im_p[3] = .5621717346937317988594118;
|
re_p[4] = -.5385526816693109683073792; im_p[4] = -.9616876881954277199245657;
|
re_p[5] = -.5385526816693109683073792; im_p[5] = .9616876881954277199245657;
|
}
|
else if (n == 7)
|
{
|
re_p[0] = -.9194871556490290014311619; ; im_p[0] = 0;
|
re_p[1] = -.8800029341523374639772340; im_p[1] = -.3216652762307739398381830;
|
re_p[2] = -.8800029341523374639772340; im_p[2] = .3216652762307739398381830;
|
re_p[3] = -.7527355434093214462291616; im_p[3] = -.6504696305522550699212995;
|
re_p[4] = -.7527355434093214462291616; im_p[4] = .6504696305522550699212995;
|
re_p[5] = -.4966917256672316755024763; im_p[5] = -1.002508508454420401230220;
|
re_p[6] = -.4966917256672316755024763; im_p[6] = 1.002508508454420401230220;
|
}
|
else if (n == 8)
|
{
|
re_p[0] = -.9096831546652910216327629; im_p[0] = -.1412437976671422927888150;
|
re_p[1] = -.9096831546652910216327629; im_p[1] = .1412437976671422927888150;
|
re_p[2] = -.8473250802359334320103023; im_p[2] = -.4259017538272934994996429;
|
re_p[3] = -.8473250802359334320103023; im_p[3] = .4259017538272934994996429;
|
re_p[4] = -.7111381808485399250796172; im_p[4] = -.7186517314108401705762571;
|
re_p[5] = -.7111381808485399250796172; im_p[5] = .7186517314108401705762571;
|
re_p[6] = -.4621740412532122027072175; im_p[6] = -1.034388681126901058116589;
|
re_p[7] = -.4621740412532122027072175; im_p[7] = 1.034388681126901058116589;
|
}
|
|
|
if (type == IIRBANDType::lowpass)
|
{
|
wc = wc / constant[n - 1];
|
|
if ((n & 1) == 1)
|
{
|
a1 = wc - re_p[0];
|
sos[k][0] = 1 / a1;
|
sos[k][1] = -1 / a1;
|
sos[k][2] = 0;
|
|
sos[k][3] = 1;
|
sos[k][4] = (re_p[0] + wc) / a1;
|
sos[k][5] = 0;
|
|
k = 1;
|
}
|
for (i = k; i < n; i += 2)
|
{
|
double re_s, re_m, im_s, im_m;
|
|
re_m = re_p[i] * re_p[i + 1] - im_p[i] * im_p[i + 1];
|
im_m = re_p[i] * im_p[i + 1] + im_p[i] * re_p[i + 1];
|
|
re_s = re_p[i] + re_p[i + 1];
|
im_s = im_p[i] + im_p[i + 1];
|
|
a1 = wc * wc - re_s * wc + re_m;
|
|
sos[k][0] = 1 / a1;
|
sos[k][1] = -2 / a1;
|
sos[k][2] = 1 / a1;
|
|
sos[k][3] = 1;
|
sos[k][4] = (2 * wc * wc - 2 * re_m) / a1;
|
sos[k][5] = (wc * wc + re_s * wc + re_m) / a1;
|
k = k + 1;
|
}
|
}
|
else if (type == IIRBANDType::highpass)
|
{
|
wc = wc * constant[n - 1];
|
|
if ((n & 1) == 1)
|
{
|
a1 = 1 - re_p[0] * wc;
|
sos[k][0] = wc / a1;
|
sos[k][1] = wc / a1;
|
sos[k][2] = 0;
|
|
sos[k][3] = 1;
|
sos[k][4] = (-re_p[0] * wc - 1) / a1;
|
sos[k][5] = 0;
|
|
k = 1;
|
}
|
for (i = k; i < n; i += 2)
|
{
|
double re_s, re_m, im_s, im_m;
|
|
re_m = re_p[i] * re_p[i + 1] - im_p[i] * im_p[i + 1];
|
im_m = re_p[i] * im_p[i + 1] + im_p[i] * re_p[i + 1];
|
|
re_s = re_p[i] + re_p[i + 1];
|
im_s = im_p[i] + im_p[i + 1];
|
|
a1 = 1 - re_s * wc + re_m * wc * wc;
|
|
sos[k][0] = wc * wc / a1;
|
sos[k][1] = 2 * wc * wc / a1;
|
sos[k][2] = wc * wc / a1;
|
|
sos[k][3] = 1;
|
sos[k][4] = (2 * re_m * wc * wc - 2) / a1;
|
sos[k][5] = (re_m * wc * wc + re_s * wc + 1) / a1;
|
k = k + 1;
|
}
|
|
}
|
g = pow(10.0, g / 20.0);
|
sos[0][0] *= g;
|
sos[0][1] *= g;
|
sos[0][2] *= g;
|
|
return nsection;
|
}
|
