对一阶二阶低通滤波器推导，并用IMU数据验证算法效果

文章背景

一直想学习一下数字信号处理算法，而不是每次遇到数据处理就求平均，求最值，看容差，做滑动窗。。。
数字信号处理算法已经很成熟了，但网上大部分还是用matlab跑仿真，设计几个不同频率的sin信号相加，来验证算法的有效性。但是实际工程中该如何去使用这个算法，资料还是比较少的，这篇文章重心放在实际工程，对于工程中遇到的常见信号做处理。

一阶算法推导

学习一个算法，掌握其思想和推导是必不可少的路。这里先以一阶低通滤波算法为例，在硬件电路中我们可以通过RC电路实现低通滤波功能，利用的便是电容原件的充放电特性。在代码里如何实现这个硬件电路呢？

这里推导大体思路如下：

1. 根据物理模型（也就是电路状态变量之间的关系）推导出电路的时域关系和频域关系，也就是电路的传递函数H(s)。
2. 有了传递函数H（s）就可以通过一阶差分方程（双线性变换）将函数从s域变换到z域，对应的含义便是将函数从连续的频域转化为离散的频域。
3. 这个时候我们得到了H（z），然后通过z变换求出关于X（输入）和Y（输出）的离散时域函数，最终我们调用的便是这个函数。

具体的推导过程懒得手写，这里直接上图：

matlab 验证算法

首先我们通过stm32抓取一组IMU数据，这里IMU用的是博世的BMI160，先只抓取z轴的重力加速度。这里上一下matlab代码：

// 
//	create by wxc on 2021.5.9
//  e-mail:550791785@qq.com
//

>> clear
>> fs  = 200;
>> dt = 1 / fs;
>> N = length(acc_z);
>> n = 5240;
>> y = fft(acc_z,n);
>> y_abs = abs(y);
>> f = (0:n-1)*fs/n;
>> subplot(211);plot(acc_z);axis([0 6000 9.9 10.1]);
>> subplot(212);plot(f(1:n/2),y_abs(1:n/2));axis([0 100 0 5]);

这里只是仿真了原始数据的波形和对应的幅频曲线。

由图中可以看出，在35-40Hz，45-50Hz,55Hz均由较大的扰动需要滤除，幅值较低的可以认为是白噪声干扰。

这里我们添加一个截止频率为15Hz的低通滤波器，看一下效果

>> y_lowpass = fft(Mix_LowPass,5240);
>> y_lowpass_abs = abs(y_lowpass);
>> lpf0 = 0;
>> lpf1 = 0;
>> for a = 1:1:length(acc_z)
lpf1 = lpf0+(1 / (1 + 1 / (2.0 * 3.14 * (1 / fs) * 15))) * (acc_z(a) - lpf0);
lpf0 = lpf1;
Mix_LowPass(a) = lpf1;
end
>> subplot(211);plot(Mix_LowPass);axis([0 6000 9.9 10.1]);
>> subplot(212);plot(f(1:n/2),y_lowpass_abs(1:n/2);axis([0 100 0 30]);

可以明显看到高频噪声被抑制了，而且原始波形也能看出在相同尺度下，振幅明显减小。这里波形稍微有点向上倾斜是因为采集数据的时候手拿着难免有移动，并且为了制造噪声干扰我还拍了几下桌子。

二阶算法推导

二阶低通滤波器的推导思路基本一致，就是由电路推导出传递函数这一步我们直接查表获取，具体步骤见图：

matlab 验证算法

对于加速度计z轴的原始数据图形这里不再展示，下面直接上matlab二阶低通滤波器处理后的时域波形和频域的幅频特性曲线。

>> fc = 15;
>> fs = 200;
>>> n = 5240;
>> f = (0:n-1)*fs/n;
>> [B,A] = butter(2,(2*pi*fc*2)/(2*pi*fs));
>> my_iir_2_low_pass = filter(B,A,acc_z);
>> subplot(211);plot(my_iir_2_low_pass);axis([0 6000 9.9 10.1]);
>> y_iir2 = fft(my_iir_2_low_pass); 
>> y_iir2_abs = abs(y_iir2);
>> subplot(212);plot(f(1:n/2),y_iir2_abs(1:n/2));axis([0 100 0 50]);

这里直接用matlab自带的巴特沃斯滤波器求出iir二阶低通滤波器系数，然后对原始数据做滤波。

然后我们用刚才推导的公式去求取滤波后的幅频曲线和原始图形

>> fs = 200;
>> fc = 15;
>> fr = fs / fc;
>> MY_PI = 3.1415926;
>> ohm = tan(MY_PI/fr);
>> c = 1.0 + 2 * cos(MY_PI/4)*ohm+ohm*ohm;
>> b0 = ohm * ohm / c;
>> b1 = 2 * b0;
>> b2 = b0;
>> a1 = 2 * (ohm * ohm - 1) / c; 
>> a2 = (1 - 2 * cos(MY_PI / 4) * ohm + ohm * ohm ) / c;
>> delay_1 = 0;
>> delay_2 = 0; 
>> for i = 1 : 1 : length(acc_z)
delay_element_0 = acc_z(i) - delay_1 * a1 - delay_2 * a2;
out = delay_element_0 * b0 + delay_1 * b1 + delay_2 *b2;
delay_2 = delay_1;
delay_1 = delay_element_0;
acc_z_lowpass(i)=out;
end
>> mag_y = abs(fft(acc_z_lowpass,length(acc_z)));
>> f = (0:length(acc_z)-1)*fs/length(acc_z);
>> subplot(211);plot(acc_z_lowpass);axis([0 6000 9.9 10.1]);
>> subplot(212);plot(f(1:length(acc_z)/2),mag_y(1:length(acc_z)/2));axis([0 100 0 40]);

其求出图形如下：

这里可以看出求出的图形和matlab推导出来的图形基本一致，证明算法的有效性！！！

这里用stm32实现这个滤波器，并把原始波形和滤波波形打印出来对比滤波前后效果。如下图所示：

这个波形是IMU平放桌子上，拍打几次桌子，观察震动波形

下图这个是晃动观察波形跟随性

放大这个图片可以看出，波形相比原信号少了很多毛刺部分，但是确实有几个周期的滞后现象。

总结

iir滤波器全称为无限长冲击响应滤波器，如它的名字而已，这个滤波器的结果与信号整个的生命周期相关，同时也与滤波器本身的输出有关。这个特性可以理解成PID控制器的中积分器，优点是冲击信号不易对系统产生较大影响，缺点是会把每次冲击都引入滤波器中，如果冲击量大，则滤波效果较差。这个特性还可以使滤波后的信号更加平滑，即最大平坦度。

fir滤波器全称为有限长冲击响应滤波器，也即它滤波的结果只与最近的几次结果有关，缺点就是滤波后的结果会有旁瓣产生，而且无论通带还是阻带都有旁瓣，对原信号还原性没有iir那么好。我们平时代码中使用的求平均，比如采集5个数据取平均，就是某一个截止频率的fir滤波器，加权平均也是如此，因为它们的结果都与最近的几个数据相关。