无人机项目跟踪记录四十二-----IIR巴特沃斯二阶滤波函数

滤波函数的理解方法是这样的：

1、在网上直接查找LPF2pApply函数名，没发现有价值的信息；

2、查找LPF2的意思，发现是二阶巴特沃斯滤波函数，但此项目的滤波迭代方式很怪异；

3、传统迭代很直观，此项目迭代方式很少见，很难理解，直接网上查找无果；

4、查找此代码来源，发现是从PX4项目摘抄过来的，于是查找PX4项目的相关资料，终于理解了；

此滤波系统的函数如下，函数频率参数设置部分：void LPF2pSetCutoffFreq_1(float sample_freq, float cutoff_freq)：

/***********************巴特沃斯二阶低通滤波器*************************
C1 = tan(w/2)
w = (2 * PI * fc / fs)
G  = 1/((C1)^2 + 2*cos(PI/4.0)*C1 + 1 )
_a1l = G((2C1)^2-2)
_a2l = G((C1)^2-2*cos(PI/4.0)*C1 + 1)
_b0l = G(C1)^2
_b1l = 2 * _b0l
_b12 = _b0l

//时域传递函数
y(n) =  _b0l * x(n) + _b1l * x(n-1) + _b12 * x(n-2) - _a1l * y(n-1) - _a2l * y(n-2)

****************************************************************/

static float           _cutoff_freq1;
static float           _a11;
static float           _a21;
static float           _b01;
static float           _b11;
static float           _b21;
static float           _delay_element_11;        // buffered sample -1
static float           _delay_element_21;        // buffered sample -2
void LPF2pSetCutoffFreq_1(float sample_freq, float cutoff_freq)
{
    float fr =0;                                      
    float ohm =0;                                     //正切值
    float c =0;                                      

    fr= sample_freq/cutoff_freq;                    //可对照上面公式计算
    ohm=tanf(M_PI_F/fr);
    c=1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;  // c = 1 / G 

    _cutoff_freq1 = cutoff_freq;     //截止频率
    if (_cutoff_freq1 > 0.0f)
    {
        _b01 = ohm*ohm/c;           //对照上面公式计算各个值
        _b11 = 2.0f*_b01;
        _b21 = _b01;
        _a11 = 2.0f*(ohm*ohm-1.0f)/c;
        _a21 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
    }
}

二阶巴特沃斯滤波器是数字信号处理的知识，如想详细了解可以去从头学习。工程上一般按照设计好的公式直接计算了，上面列出了具体的计算公式，直接套用就可以了。

可以参考这篇文章：【巴特沃斯低通滤波器_行走-CSDN博客_巴特沃斯低通滤波】https://blog.csdn.net/weixin_42089190/article/details/88773755

下面的函数是用滤波器进行滤波的代码：

/***********************巴特沃斯二阶低通滤波器*************************
时域传递函数为：
y(n) =  _b0l * x(n) + _b1l * x(n-1) + _b12 * x(n-2) - _a1l * y(n-1) - _a2l * y(n-2)

****************************************************************/

float LPF2pApply_1(float sample)
{

    float delay_element_0 = 0, output=0;
    if (_cutoff_freq1 <= 0.0f) {                //频率为零时后不进行滤波
        // no filtering
        return sample;    
    }
    else
    {
      //  x1(n) =   x(n)  - _a1l * y(n-1) - _a2l * y(n-2)
			delay_element_0 = sample - _delay_element_11 * _a11 - _delay_element_21 * _a21;  //只考虑当前时刻的输入与系统之前时刻的输出。当前时刻系统的输入值会受到系统之前输出值的影响而改变成delay_element_0。
        // do the filtering
        if (isnan(delay_element_0) || isinf(delay_element_0)) {
            // don't allow bad values to propogate via the filter
            delay_element_0 = sample;
        }
				//y(n) =  _b0l * x1(n) + _b1l * x1(n-1) + _b12 * x1(n-2)
        output = delay_element_0 * _b01 + _delay_element_11 * _b11 + _delay_element_21 * _b21;//不考虑之前时刻的输出，只考虑被影响后的输入与当前输出关系

        _delay_element_21 = _delay_element_11;
        _delay_element_11 = delay_element_0;

        // return the value.  Should be no need to check limits
        return output;
    }
}

此处的滤波迭代很怪异，不是很好理解。是将延时图进行了优化处理，如果不好理解可以试着按照注释理解。详细的推导关系参见此文章：https://new.qq.com/omn/20210317/20210317A0D1P700.html