二阶 IIR 滤波器 C 代码实现

#define PI 3.14159265358979323846
typedef double sample_t;

enum {
	BIQUAD_LOWPASS,
	BIQUAD_HIGHPASS,
	BIQUAD_BANDPASS_PEAK,
};

typedef struct
{
	sample_t a0, a1, a2, a3, a4;
	sample_t x1, x2, y1, y2;
}biquad_state;

// 计算滤波器的系数，2nd-order Butterworth: q_value=0.7071 ，2nd-order Chebyshev (ripple 1 dB): q_value=0.9565，2nd-order Thomson-Bessel: 
// q_value=0.5773。带通滤波器的q_value = sqrt(f1*f2)/(f2-f1)，fs是指信号的采样频率，fc为cutoff frequence 或者是centern frequenc(带通滤波器)
void filter_init(biquad_state *state, int type, int fs, float fc, float q_value)
{
	sample_t w0, sin_w0, cos_w0, alpha;
	sample_t b0, b1, b2, a0, a1, a2;

	w0 = 2 * PI * fc / fs;
	sin_w0 = sin(w0);
	cos_w0 = cos(w0);
	alpha = sin_w0 / (2.0 * q_value);

	switch(type)
	{
	case BIQUAD_LOWPASS:
		b0 = (1.0 - cos_w0) / 2.0;
		b1 = b0 * 2;
		b2 = b0;
		a0 = 1.0 + alpha;
		a1 = -2.0 * cos_w0;
		a2 = 1.0 - alpha;
		break;
	case BIQUAD_HIGHPASS:
		b0 = (1.0 + cos_w0) / 2.0;
		b1 = -b0 * 2;
		b2 = b0;
		a0 = 1.0 + alpha;
		a1 = -2.0 * cos_w0;
		a2 = 1.0 - alpha;
		break;
	case BIQUAD_BANDPASS_PEAK:
		b0 = alpha;
		b1 = 0.0;
		b2 = -alpha;
		a0 = 1.0 + alpha;
		a1 = -2.0 * cos_w0;
		a2 = 1.0 - alpha;
		break;
	}
	state->a0 = b0 / a0;
	state->a1 = b1 / a0;
	state->a2 = b2 / a0;
	state->a3 = a1 / a0;
	state->a4 = a2 / a0;
	state->x1 = state->x2 = 0.0;
	state->y1 = state->y2 = 0.0;
}

//biquard滤波
sample_t biquad(biquad_state *state, sample_t data)
{
	sample_t result = 0;
	result = state->a0 * data + state->a1 * state->x1 + state->a2 * state->x2 -  state->a3 * state->y1 - state->a4 * state->y2;
	state->x2 = state->x1;
	state->x1 = data;
	state->y2 = state->y1;
	state->y1 = result;
	return result;

}