PX4中的二阶巴特沃斯低通滤波

源码

const float fr = sample_freq/_cuttof_freq;
const float ohm = tanf(M_PI_F, fr);
const float c = 1.0f + 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm;

_b0 = ohm * ohm / c;
_b1 = 2.0f * _b0;
_b2 = _b0;

_a1 = 2.0f * (ohm * ohm - 1.0f) / c;
_a2 = (1.0f - 2.0f * cosf(M_PI_F / 4.0f) * ohm + ohm * ohm) / c;

float LPF_Apply(float sample)
{
    //滤波器
    float delay_element_0 = sample - delay_element_1 * _a1 - delay_element_2 * _a2;
    //判断特殊值
    if(/**/)
    {
        delay_element_0 = sample;
    }
    
    const float output = delay_element0 * _b0 + delay_element1 * _b1 + delay_element2 * _b2;
    _delay_element_2 = _delay_element_1;
    _delay_element_1 = _delay_element_0;
    //返回滤波后的值
    return output;
}

巴特沃斯数字滤波器归一化

滤波器传递函数:
$$H(s)=\frac{1}{a_0+a_{1}s+a_2{s}^2+...+a_N{s}^N}$$

当$N=2$时，传递函数为：

$$H(s)=\frac{1}{1+1.414s+s^2}$$
在实际使用中，只要用$\frac{s}{w_a}$代替上式中的$s$即可以得到任意截至频率为$w_a$弧度的滤波器,即
$$H(s)=\frac{1}{1+1.414\frac{s}{w_a}+(\frac{s}{w_a}{)}^2}$$

模拟传递函数离散化，常用方法：双线性变换

$$s=\frac{2}{T_s}\frac{1-z^{-}1}{1+z^{-1}}=2f_s\frac{1-z^{-1}}{1+z^{-1}}$$
数字滤波器的截止频率要比模拟滤波器的截止频率要小，下面讨论这两个频率之间的关系：

对于$s$域来说，截止频率$w_a$满足的关系式为：
$$s=j{w}_a=j2\pi f_a$$
对于$z$域来说，截止频率$w_d$满足的关系式为：
$$\begin{gathered} z=e^{s{T}_s}s=j{w}_d{w}_d=2\pi f_d \\ z=e^{j2\pi \frac{f_d}{f_s}} \end{gathered}$$
将上式代入到双线性变换公式可得：
$$s=2f_s\frac{1-z^{-1}}{1+z^{-1}}=2f_s\frac{1-e^{-j2\pi \frac{f_d}{f_s}}}{1+e^{-j2\pi \frac{f_d}{f_s}}}=2f_s\frac{e^{j2\pi \frac{f_d}{f_s}}-1}{e^{j2\pi \frac{f_d}{f_s}}+1}=j2f_{s}tan(\frac{\pi f_d}{f_s})$$
计算过程如下：
$$
\begin{array}{c}
e^{i x}=\cos x+i \sin x \

\frac{\mathrm{e}^{j A}-\mathrm{e}^{-j A}}{\mathrm{e}^{j A}+\mathrm{e}^{-j A}}=\frac{\cos (A)+j \sin (A)-(\cos (-A)+j \sin (-A))}{\cos (A)+j \sin (A)+(\cos (-A)+j \sin (-A))}=j \tan (A) \

\frac{e^{j \omega}-1}{e^{j w}+1}=\frac{\mathrm{e}^{j w / 2}}{\mathrm{e}^{\mathrm{j} \omega / 2}} \frac{\left(\mathrm{e}^{\mathrm{j} \omega / 2}-\mathrm{e}^{-\mathrm{j} \omega / 2}\right)}{\left(\mathrm{e}^{\mathrm{j} \omega / 2}+\mathrm{e}^{-\mathrm{j} \omega / 2}\right)}=j \tan \left(\frac{\omega}{2}\right)
\end{array}
$$

将频域的$s$公式代入得：
$$\begin{gathered} j{w}_a=j2\pi f_a=j2f_{s}tan(\frac{\pi f_d}{f_s}) \\ {f}_a=\frac{f_s}{\pi }tan(\frac{\pi f_d}{f_s}) \end{gathered}$$

设计数字滤波器