飞控中对加速计进行滤波只用到的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;
}