【信号滤波 (补充)】二阶陷波滤波代码推导过程(C++)

在信号滤波 (中) 这篇中对于二阶陷波滤波器的传递函数到C++代码的转化过程还有人有疑问，本篇一步步推导整个过程。
信号滤波 (中)

二阶陷波滤波器计算实例

在信号滤波 (中)这篇里已经通过Z域构建了传递函数。
$$H(z)=\frac{b_0+b_1{z}^{-1}+b_2{z}^{-2}}{1+a_1{z}^{-1}+a_2{z}^{-2}}$$
式中的未知数是怎么求解出来的呢？接下来一步步推导。

一、 传递函数的参数推导

1. 首先$b_0,b_1,b_2$是怎么推导出来的？

$b_0,b_1,b_2$的设置是基于前人的经验值，我们把这几个参数带入传递函数分子计算。

• $b_0=1$
• $b_1=-2\cos ({\omega }_0)$
• $b_2=1$

所谓零点就是$H(z)$的分子等于零，带入参数后$1-2cos({\omega }_0)z^{-1}+z^{-2}=0$.
求$z$的解，大学知识就可以：
1.1. 两边同乘以$z^2$：
$$z^2-2\cos ({\omega }_0)z+1=0$$
1.2. 使用求根公式：
$$z=\frac{-b\pm \sqrt{b^2-4ac}}{2a}$$
1.3. 带入求根公式计算：
$$z=\frac{-(-2\cos ({\omega }_0))\pm \sqrt{(-2\cos ({\omega }_0){)}^2-4\cdot 1\cdot 1}}{2\cdot 1}$$
$$z=\frac{2\cos ({\omega }_0)\pm \sqrt{4{\cos }^2({\omega }_0)-4}}{2}$$
$$z=\frac{2\cos ({\omega }_0)\pm \sqrt{4({\cos }^2({\omega }_0)-1)}}{2}$$
$$z=\frac{2\cos ({\omega }_0)\pm \sqrt{4(-{\sin }^2({\omega }_0))}}{2}$$
$$z=\frac{2\cos ({\omega }_0)\pm 2j\sin ({\omega }_0)}{2}$$
1.4.求得两个解：
$$z=\cos ({\omega }_0)\pm j\sin ({\omega }_0)$$
欧拉公式转换如下：
$$z_1=e^{j{\omega }_0},z_2=e^{-j{\omega }_0}$$
1.5.结论：
可以看到，带入经验值求得的共轭复根，位于复平面的单位圆上，更具体的说是位于${\omega }_0$(陷波频率)的单位圆上。
零点的作用就是使目标频率完全衰减。

同样$a_0,a_1,a_2$也是是基于前人的经验值来设置的，保持极点在收敛域内又接近零点。

2. 带入实际值求解

假定初始参数如下：

• 陷波频率 $f_0=50\text{Hz}$
• 采样频率 $f_s=1000\text{Hz}$
• 品质因子 $Q=30$

2.1. 计算 ${\omega }_0$:
$${\omega }_0=2\pi \frac{50}{1000}=0.31416\text{角频率/样本}$$
2.2.计算 $\alpha$:
$$\alpha =\frac{\sin (0.31416)}{2\times 30}=0.005235$$
2.3. 计算参数:
$$b_0=1,b_1=-2\cos (0.31416)=-1.9021,b_2=1$$
$$a_0=1+0.005235=1.005235,a_1=-2\cos (0.31416)=-1.9021,a_2=1-0.005235=0.994765$$
2.4. 归一化:
$$b_0=\frac{1}{1.005235}=0.99477,b_1=\frac{-1.9021}{1.005235}=-1.8934,b_2=\frac{1}{1.005235}=0.99477$$
$$a_1=\frac{-1.9021}{1.005235}=-1.8934,a_2=\frac{0.994765}{1.005235}=0.98953$$
2.5. 求得传递函数的最终公式:
$$H(z)=\frac{0.99477-1.8934z^{-1}+0.99477z^{-2}}{1-1.8934z^{-1}+0.98953z^{-2}}$$

3. 验证上述的传递函数

验证方法就是1.1的计算过程，首先将分子分母才开单独求根：
$$H(z)=\frac{A(z)}{B(z)}=\frac{0.99477z^2-1.8934z+0.99477}{z^2-1.8934z+0.98953}$$

$0.99477z^2-1.8934z+0.99477=0$带入求根公式
$$z=\frac{-(-1.8934)\pm \sqrt{(-1.8934{)}^2-4(0.99477)(0.99477)}}{2(0.99477)}$$
$$z=\frac{1.8934\pm \sqrt{-0.3752}}{1.98954}$$
分子部分的根(也就是零点)为：
$$z=0.9516\pm 0.3083i$$

同理分母$z^2-1.8934z+0.98953=0$带入求根公式
$$z=\frac{-(-1.8934)\pm \sqrt{(-1.8934{)}^2-4(1)(0.98953)}}{2(1)}$$
$$z=\frac{1.8934\pm \sqrt{-0.37922}}{2}$$
分母部分的根(也就是极点)为：
$$z=0.9467\pm 0.3070i$$

画出零点和极点在Z域上的图像，可以看到零点在单位圆上，极点在单位圆内侧(收敛域内，在外侧就发散了)，极点和零点挨得很近。

二、将传递函数转化成差分方程

2.1 传递函数写成输入输出形式

将传递函数可以看成Z域上输入X(z)和输出Y(z)的比值$H(z)=\frac{Y(z)}{X(z)}$。
将输出Y(z)放到等式左边得$Y(z)=H(z)\cdot X(z)$，展开后如下：
$$Y(z)=\left(\frac{b_0+b_1{z}^{-1}+b_2{z}^{-2}}{1+a_1{z}^{-1}+a_2{z}^{-2}}\right)\cdot X(z)$$
把分母去掉得到：
$$(1+a_1{z}^{-1}+a_2{z}^{-2})\cdot Y(z)=(b_0+b_1{z}^{-1}+b_2{z}^{-2})\cdot X(z)$$

2.2 Z域转化为时域

$z^{-n}$ 代表在n时刻的采样，$z^{-1}$代表在n-1时刻的采样, $z^{-2}$代表在n-1时刻的采样，以此类推。
因此转化为时域后的差分方程如下：
$$y[n]+a_{1}y[n-1]+a_{2}y[n-2]=b_{0}x[n]+b_{1}x[n-1]+b_{2}x[n-2]$$
整理后如下：
$$y[n]=b_{0}x[n]+b_{1}x[n-1]+b_{2}x[n-2]-a_{1}y[n-1]-a_{2}y[n-2]$$

三、将差分方程转化成C++代码

3.1 改写输入输出变量

在C++中，用x_n表示输入x[n]，y_n代表输出y[n]，x0输入来自于i时刻电压采样voltageData[i]。y_n是经过差分方程计算后的输出。
同理，前一个时刻的输入x[n-1]用x_n_1表示，前两个个时刻的输入x_n_2用x[n-2]表示，前一个时刻的输出y[n-1]用y_n_1表示，前两个时刻的输出y[n-2]用y_n_2表示。
转化后如下：

double y_n = b0 * x_n + b1 * x_n_1 + b2 * x_n_2 - a1 * y_n_1 - a2 * y_n_2;

滤波后的变量存储在filteredData[i]中。

3.2 用循环改写差分方程的递推过程

差分方程其实就是一个递推过程，依据当前输入、前一时刻输入和输出、前两时刻输入和输出，推出当前输出。
当循环完成一次后执行下面的代码把当前的输入输出变成前一时刻的输入输出：

x_n_2 = x_n_1;
x_n_1 = x_n;
y_n_2 = y_n_1;
y_n_1 = y_n;

其中

• x_n_1 和 x_n_2: 前时刻的采样 x[n-1] 和 x[n-2]。
• y_n_1 和 y_n_2: 前时刻采样的输出 $y[n-1]$ 和 $y[n-2]$。

3.3 转化成C++后的代码

下面用QT C++写applyNotchFilter滤波函数：

QVector<double> applyNotchFilter(const QVector<double> &voltageData, double sampleRate, double notchFreq, double qualityFactor)
{
    QVector<double> filteredData(voltageData.size());
    double w0 = 2.0 * M_PI * notchFreq / sampleRate;
    double alpha = sin(w0) / (2.0 * qualityFactor);
    double b0 = 1.0;
    double b1 = -2.0 * cos(w0);
    double b2 = 1.0;
    double a0 = 1.0 + alpha;
    double a1 = -2.0 * cos(w0);
    double a2 = 1.0 - alpha;
    b0 /= a0;
    b1 /= a0;
    b2 /= a0;
    a1 /= a0;
    a2 /= a0;
    double x_n_1 = 0.0, x_n_2 = 0.0; 
    double y_n_1 = 0.0, y_n_2 = 0.0; 
    for (int i = 0; i < voltageData.size(); ++i) {
        double x_n = voltageData[i];
        double y_n = b0 * x_n + b1 * x_n_1 + b2 * x_n_2 - a1 * y_n_1 - a2 * y_n_2;
        filteredData[i] = y_n;
        x_n_2 = x_n_1;
        x_n_1 = x_n;
        y_n_2 = y_n_1;
        y_n_1 = y_n;
    }
    return filteredData;
}

完整代码的应用见信号滤波 (上) 3.3部分。