飞控中的IIR二阶滤波器

之前我们讲过一阶滤波器，思路就是把一个连续的滤波器形式，通过离散化的方式，转换成差分方程。

同事拿着我的文章，对照着代码里的二阶滤波，表示完全看不懂，我说不可能，二阶不过是一阶的升级版，思路应是一样的，他说不信你看。

我一看，WTF，这系数怎么来的？经验公式？

这迭代怎么这种形式？没见过呀！

行吧，说明之前咱理解的不到位，那就从头开始讲起吧。

从模拟滤波器开始

我们从书上，百度，查到的滤波器公式，通常是用传递函数表达的，这是s域下的表达形式，是连续的，这种我们称之为模拟滤波器。

以巴特沃斯低通滤波器为例：
$$H_{an}(s)=\frac{d_0}{a_0+a_{1}s+a_2{s}^2+\cdots +a_N{s}^N}$$
对应的归一化参数表如下（如果期望直流($\Omega =0$)增益为1，则d0=a0）

意思是如果你按照表格里的参数，带入滤波器公式中，就能得到对于阶数的归一化滤波器，即截止频率为1弧度（$1/2\pi$hz）的滤波器。

例如我想得到一个截止频率为$1/2\pi$hz的二阶巴特沃斯滤波器，就把n=2的系数带入方程，可得
$$H(s)=\frac{1}{s^2+1.414s+1}$$
有了这个归一化滤波器，我们可以很容易的得到任意截止频率的滤波器，只需要另$s->\frac{s}{w_a}$就可以的带一个截止频率为wa弧度的滤波器。
$$\frac{1}{{\frac{s}{w_a}}^2+1.414\frac{s}{w_a}+1}$$

模拟与数字的关系

上面举例的模拟滤波器传递函数，目的是用来设计滤波电路，针对的是连续时间的模拟信号，组成元器件是电阻，电容，电感。

而数字滤波器实现方法是把滤波器所要完成的运算编成程序并让计算机执行,也就是采用在代码的形式。它面对的是离散时间的数字信号，是把输入序列通过一定的运算变换成输出序列。

有没有办法能把连续的模拟滤波器变成离散的数字滤波器？

显然是有的，而且有很多种，其中最常使用的一种叫做双线性变换
$$s=\frac{2}{T_s}\frac{1-z^{-1}}{1+z^{-1}}=2f_s\frac{1-z^{-1}}{1+z^{-1}}$$
把这个公式带入传递函数就可以得到一个z域的差分方程了。

但是如果我们直接使用双线性变换进行离散化之后，会发现转换前的模拟滤波器和转换后的数字滤波器的幅频响应曲线并不一样。

可以看到数字滤波器曲线DF，远比模拟滤波器AF衰减的要快，也就是说如果模拟截止频率是10hz,那数字滤波器衰减更快，截止频率可以只9hz。

这是为什么呢？这是因为双线性变换中，数字截止角频率 ${\omega }_d$ 和模拟截止角频率 ${\omega }_a$ 的关系是非线性的。

所以在变换的时候我们需要找到s域与z域变换时频率变化的对应关系。

对于s域来说，在模拟截止频率为 $f_a$ 时有：
$$s=j{w}_a=j\ast 2\pi \ast f_a$$
对于z域来说，在数字截止频率为fd的情况下，$z=e^{sT}$ 其中 $s=j{w}_d,w_d=2\pi f_d,T=1/f_s$，则有：
$$z=e^{sT}=e^{j2\pi \frac{f_d}{f_s}}$$
把这个截止频率为 $f_d$ 的带入双线性变换公式可以得到：
$$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 \left(\frac{\pi f_d}{f_s}\right)$$

所以
$$j2\pi f_a=j2f_s\tan \left(\frac{\pi f_d}{f_s}\right)$$
可以得到模拟截止频率与数字截止频率的关系
$$f_a=\frac{f_s}{\pi }\tan (\frac{\pi f_d}{f_s})$$

（补充证明过程根据欧拉公式）
$$e^{ix}=\cos x+i\sin x$$

$$\frac{{\mathrm{e}}^{jA}-{\mathrm{e}}^{-jA}}{{\mathrm{e}}^{jA}+{\mathrm{e}}^{-jA}}=\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^{jw}-1}{e^{jw}+1}=\frac{{\mathrm{e}}^{jw/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 (\frac{\omega }{2})$$

设计数字滤波器

有了离散方式和频率对应关系，就可以来设计我们需要的数字滤波器了。

设计数字滤波器分几步？

1. 根据我们想要的数字滤波频率得到我们想要的模拟滤波器频率

2. 根据期望的模拟截止频率，将滤波器去归一化。

3. 进行双线性变换

4. 写成代码

如果我期望的数字滤波器截止频率为fd,对应的模拟滤波器截止角频率为：
$$w_a=2\pi f_a=2f_s\tan (\frac{\pi f_d}{f_s})$$
去归一化只需要将模拟滤波器传递函数中的s进行如下替换即可
$$s->\frac{s}{w_a}$$
最后使用双线性变换离散化,把公式带入
$$s=2f_s\frac{1-z^{-1}}{1+z^{-1}}$$
所以最终的变换方式就是
$$s->\frac{s}{w_a}=\frac{2f_s\frac{1-z^{-1}}{1+z^{-1}}}{2f_s\tan (\frac{\pi f_d}{f_s})}=\frac{\frac{1-z^{-1}}{1+z^{-1}}}{\tan (\frac{\pi f_d}{f_s})}$$

ok,看起来有一点点复杂，没关系，举个例子

还是以我们的归一化二阶巴特沃斯低通滤波器公式
$$H(s)=\frac{1}{s^2+1.414s+1}$$
带入完整的变换可得
$$H(z)=\frac{1}{{(\frac{\frac{1-z^{-1}}{1+z^{-1}}}{\tan (\frac{\pi f_d}{f_s})})}^2+1.414(\frac{\frac{1-z^{-1}}{1+z^{-1}}}{\tan (\frac{\pi f_d}{f_s})}))+1}=\frac{(\tan (\frac{\pi f_d}{f_s}){)}^2}{(\frac{1-z^{-1}}{1+z^{-1}}{)}^2+1.414(\frac{1-z^{-1}}{1+z^{-1}})\ast (\tan (\frac{\pi f_d}{f_s}))+(\tan (\frac{\pi f_d}{f_s}){)}^2}$$
为了避免过于复杂我们把常数$\tan (\frac{\pi f_d}{f_s})$重新命名为${\Omega }_{new}$
$$H(z)=\frac{Y(z)}{X(z)}=\frac{{\left(1+z^{-1}\right)}^2{\Omega }_{new}^2}{{\left(1-z^{-1}\right)}^2+1.414{\Omega }_{new}\left(1-z^{-1}\right)\left(1+z^{-1}\right)+{\Omega }_{new}^2{\left(1+z^{-1}\right)}^2}$$

$$H(z)=\frac{Y(z)}{X(z)}=\frac{{\Omega }_{new}^2+2{\Omega }_{new}^2{z}^{-1}+{\Omega }_{new}^2{z}^{-2}}{1-2z^{-1}+z^{-2}+1.414{\Omega }_{new}-1.414{\Omega }_{new}{z}^{-2}+{\Omega }_{new}^2+2{\Omega }_{new}^2{z}^{-1}+{\Omega }_{new}^2{z}^{-2}}$$

$$H(z)=\frac{Y(z)}{X(z)}=\frac{{\Omega }_{new}^2+2{\Omega }_{new}^2{z}^{-1}+{\Omega }_{new}^2{z}^{-2}}{\left(1+1.414{\Omega }_{new}+{\Omega }_{new}^2\right)+\left(2{\Omega }_{new}^2-2\right)z^{-1}+\left(1-1.414{\Omega }_{new}+{\Omega }_{new}^2\right)z^{-2}}$$

到这里我们的化简就全部结束了，什么还是好复杂？那我再重新给他们起个名字吧

令$B0={\Omega }_{new}^2$,$B1=2{\Omega }_{new}^2$,$B2={\Omega }_{new}^2$,

$c=1+1.414{\Omega }_{new}+{\Omega }_{new}^2$,$A1=2{\Omega }_{new}^2-2$,$A2=1-1.414{\Omega }_{new}+{\Omega }_{new}^2$
$$H(z)=\frac{Y(z)}{X(z)}=\frac{B_0+B_1{z}^{-1}+B_2{z}^{-2}}{c+A_1{z}^{-1}+A_2{z}^{-2}}$$

$$Y(z)(c+A_1{z}^{-1}+A_2{z}^{-2})=X(z)(B_0+B_1{z}^{-1}+B_2{z}^{-2})$$

$$Y(z)=\frac{B_0}{c}X(z)+\frac{B_1}{c}X(z)z^{-1}+\frac{B_2}{c}X(z)z^{-2}-\frac{A_1}{c}Y(z)z^{-1}-\frac{A_2}{c}Y(z)z^{-2}$$

$$Y(z)=\frac{B_0}{c}X(z)+\frac{B_1}{c}X(z-1)+\frac{B_2}{c}X(z-2)-\frac{A_1}{c}Y(z-1)-\frac{A_2}{c}Y(z-2)$$

再改写一下
$$Y(z)=b0X(z)+b1X(z-1)+b2X(z-2)-a1Y(z-1)-a2Y(z-2)$$
有没稍微熟悉一点？这不就是书上的公式吗？

根据我们之前的推导，这个差分方程的系数只跟数字截止频率 $f_d$，和采样频率 $f_s$ 有关。

${\Omega }_{new}=\tan (\frac{\pi f_d}{f_s})$

$c=1+1.414{\Omega }_{new}+{\Omega }_{new}^2$

$b0=B0/c={\Omega }_{new}^2/c$,$b1=B1/c=2{\Omega }_{new}^2/c$,$b2=B2/c={\Omega }_{new}^2/c$,

$a1=A1=(2{\Omega }_{new}^2-2)/c$,$A2=(1-1.414{\Omega }_{new}+{\Omega }_{new}^2)/c$

看看APM和PX4怎么用程序怎么计算这些些系数呢？（跟我们的推导一模一样）

系数有了那这样的差分方程怎么写成代码呢？转换成差分方程后，下标n就代表当前值，下标n-1代表这个数是上个周期的值，n-2代表这个数延时了两周期。

所以方程写在代码里就是：

这种形式有个名称叫做直接I型，我们可以将它化成框图形式

从框图中可以直观的看见使用了4个z^-1（延时模块）,从代码里也可以看见，我们需要保存4个过去的值（x(n-2),x(n-1),y(n-2),y(n-1)）,

一个二阶滤波器需要4个延时模块我们能不能化简一下呢？

我们观察上图的直接I型滤波器，可以看成是两个较小系统串联而成的系统，那么，我们将其调整一下位置，得到就像下图一样的一个新的系统。

仔细观察其实我们可以共用延时模块

这样我们只需要使用两个延时模块就行，写成代码如下

这样我们在代码里只需要保存两个数据就行了，在看看源码，是不是一模一样？

到这里我们才算完成了一个滤波器设计，完整的称呼应该叫做 二阶巴特沃斯低通滤波器使用双线性变换法的IIR数字滤波器实现。

什么是IIR？什么是巴特沃斯？

IIR 是一种实现滤波器的方法，对数字滤波器，从实现方法上有IIR和FIR滤波器之分，他们分别对应的是不同的滤波器结构

IIR实现的最终效果就是我们刚才设计的过程对应的通用转移函数结构如下
$$H(z)=\frac{{\sum }_{r=0}^M{b}_r{z}^r}{1+{\sum }_{k=1}^N{a}_k{z}^{-k}}$$
如果使用FIR的方式实现，对应的转移函数形式如下
$$H(z)=\sum _{n=0}^{N-1}h(n)z^{-n}$$

FIR滤波器可以对给定的率特性直接进行设计，而IIR滤波器目前最通用的方法是利用已经很成熟的模拟滤波器的设计方法来进行设计。

所以IIR可以利用不同的模拟滤波器来设计，

而模拟滤波器又有Butterworth滤波器、Chebyshev(I型、Il型)滤波器、椭圆滤波器等不同的设计方法，

对应不同的幅度平方函数，以巴特沃斯滤波器为例：

使用这种函数需要进行一些零极点配置，才能得到我们想要的传递函数，好在模拟滤波器设计非常成熟，有各种表格，我们查表就能直接得到对应的滤波器传递函数。

而双线性变换是离散化的一种方法，通过这种方式离散可以直接得到IIR的结构。

谁能想到一个二阶滤波器而已，不过十几行代码，里面有这么多数字信号处理的知识呢？OK，今天就讲这么多，我是zing，我们下期再见。