biquad

 

飞控中对加速计进行滤波只用到的Z^-1，并没有用到Z^-2，和一阶低通滤波很像：具体对加速计进行滤波的代码如下：

// ledvinap
//y[n] = -0.89*x[n] + a1*x[n-1] - b1*y[n-1] 
void biquadRCFIR2FilterInit(biquadFilter_t *filter, uint16_t f_cut, uint32_t samplingIntervalUs)
{   // 15 500hz ÈôÊÇ 15 <1000
	if (f_cut < (1000000 / samplingIntervalUs / 2)) {
        const float dT = (float) samplingIntervalUs * 0.000001f; //0.002s
        const float RC = 1.0f / ( 2.0f * M_PIf * f_cut );        //0.016
        const float k = dT / (RC + dT);   // 0.11
        filter->b0 = k / 2; //b0=0.055
        filter->b1 = k / 2; //b1=0.055
        filter->b2 = 0;     //b2=0
        filter->a1 = -(1 - k);//a1=-0.89
        filter->a2 = 0;     //a2=0
    } else { //²ÉÑù¼ä¸ôÌ«´óʱ
        filter->b0 = 1.0f;
        filter->b1 = 0.0f;
        filter->b2 = 0.0f;
        filter->a1 = 0.0f;
        filter->a2 = 0.0f;
    }

    // zero initial samples
    filter->d1 = filter->d2 = 0; //d1=0 d2=0
}


//y[n] = 0.055*x[n] + 0.055*x[n-1] +0.89*y[n-1]
float biquadFilterApply(biquadFilter_t *filter, float input)
{
    const float result = filter->b0 * input + filter->d1;
    filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
    filter->d2 = filter->b2 * input - filter->a2 * result;
    return result;
}