二阶滤波器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;

 

}int main(int argc, char **argv) {

    biquad_state *stat = (biquad_state *) malloc(sizeof(biquad_state));

    int numbers = 1024;

    float fs = 1024;

    sample_t data[numbers], filter_data[numbers];

    float fc = 50;

    float q_value = 0.7071;

    int type = BIQUAD_LOWPASS;

    filter_init(stat, type, fs, fc, q_value);

    for (int i = 0; i <numbers; ++i)

    {　

        data[i] = 0.5 * (sin(2 * PI * 40 * i / fs) + cos(2 * PI * 150 * i / fs + PI / 4));

    }

　　　　

    for (int i = 0; i <numbers; ++i)

    {

        filter_data[i] = biquad(stat, data[i]);

    }

    free(stat);

}