void iir_notch_filter_design(int Fs, int Fc, double Q, double *iir_coeffs)
{
double omega, sinw, cosw, alpha, a0;
omega = 2. * M_PI * Fc / Fs; // M_PI = 3.1415926
sinw = sin(omega);
cosw = cos(omega);
alpha = sinw / (2. * Q); // Q value of filter.
// DIRECT FORM I:
// a0 * y(n) = b0 * x(n) + b1 * x(n-1) + b2 * x(n-2) - a1 * y(n-1) - a2 * y(n-2)
iir_coeffs[0] = 1. + alpha; // a0: denominator, -> y(n): output
iir_coeffs[1] = -2. * cosw; // a1
iir_coeffs[2] = 1. - alpha; // a2
iir_coeffs[3] = 1.; // b0: numerator, -> x(n): input
iir_coeffs[4] = -2. * cosw; // b1
iir_coeffs[5] = 1.; // b2
// normalize a0 to 1.0
a0 = iir_coeffs[0];
iir_coeffs[1] /= a0; // a1
iir_coeffs[2] /= a0; // a2
iir_coeffs[3] /= a0; // b0: numerator, -> x(n): input
iir_coeffs[4] /= a0; // b1
iir_coeffs[5] /= a0; // b2
iir_coeffs[0] = 1.; // a0
}
|
楼主
