【分享帖】RM入门教学系列数字滤波器设计与实现（含代码）
Extreme | 4509 | 2018-01-24 随着RoboMaster比赛的推广，不断有新的学校参与进来，有些同学因为刚接触比赛，缺乏实践经验，可能会遇到一些基础的技术问题。我们可能会陆续更新一些基础教程。今天给大家介绍一个实用技巧：利用MATLAB设计滤波器并在STM32平台上跑起来。
滤波器的重要作用不必多说，在传感器数据处理、电机控制、视觉算法中都会用到。这里举一个最简单的例子，对陀螺仪采集的数据进行低通滤波，从而达到去除噪声的作用。
1. 分析原数据
首先我们将MPU6500的陀螺仪数据带宽设置为250Hz，然后使用J-Scope采集静置时候的陀螺仪x轴的数据。可以看出角速度有幅度约为0.3°/s的跳动。（为了让J-Scope读到值，这里将原始数据乘以1000转化成整型了）
对数据进行FFT变换之后得到频谱图：
可以看出信号在250Hz之后有衰减，证明MPU6500确实有对原始adc采样数据做过滤波，但是效果不明显。
2. FDATool工具介绍
现在我们来自己设计一个低通滤波器。
首先打开MATLAB，在命令行窗口键入 fdatool，打开滤波器设计和分析工具（Filter Design & Analysis Tool），初始界面如下图所示：
这是经典的MATLAB工具箱界面，第一行菜单栏，第二行工具栏，中间展示结果，最下方的功能区可以切换，默认的功能就是设置滤波器参数。这点英文肯定难不倒大家，随便点点看就能学会。
3. 设计滤波器
在设计滤波器界面，可以根据自己的需求选择滤波器类型、阶数、频率特性、幅度特性等参数。
例如我选择了切比雪夫Ⅱ型低通滤波器，采用Minimum order模式（根据频率、幅度指标自动选择最小阶数），设置采样频率（Fs）1000Hz，通带频率（Fpass）50Hz，阻带频率（Fstop）60Hz，衰减幅度保持默认。点击Design Filter按钮，一个滤波器就出来了。
该滤波器是17阶的，点击工具栏的按钮可以看到该滤波器的详细特性，例如幅频、相频、群延时、零极点等等。蓝色线表示的幅频响应曲线，橙色线是相频响应曲线。可以看出切比雪夫Ⅱ型滤波器在通带内平滑，阻带内有等幅度纹波。
4. 转化和导出
FDATool默认生成的滤波器是二阶级联组成，这样的稳定性更高（尤其是滤波器阶数较高时），但是结构比较复杂，程序实现也不方便，一般将其转化成一阶级联型再使用，点击菜单栏的Edit->Convert to Single Section进行转换。至此，一个50Hz的切比雪夫Ⅱ型低通滤波器就完成了。
点击 Targets-> Generate C Header，将滤波器的系数存到.h文件中。然后我们可以进入C语言的部分了。
5. C语言实现
把系数提取出来，然后把下面这个滤波器函数放到定时器中断或者数据接收中断处理函数里面，按照固定频率运行，即可输出滤波后的陀螺仪数据了。（具体原理再文章最后有介绍）
（注意：分子系数是成对出现的，可以先合并来提高运算效率）
6. 结果对比
静止测试时，噪声得到了有效的抑制。
再看频谱，滤波效果非常明显，50Hz以上的分量几乎被完全消除了。
动态测试图：
曲线平滑很多，毛刺都被消除了，但是可以明显看出，滤波之后的信号相比原信号有滞后，这是滤波造成的相位延迟。这里只是为了给大家展示滤波器的效果，实际上该滤波器并不一定适合用于对陀螺仪数据进行处理，要综合考虑各种因素来确定滤波器。
7. 计算原理
详细内容可以去翻信号与系统的课本：
FDATool生成的滤波器其实传递函数形式 ，两组系数分别是传递函数的分子分母，结构如下：
上下同除以z^n ：
将上式展开，并利用z变换的时移性质
最后整理为：
此即代码里循环累加的过程。
这个算法是用差分方程表示线性时不变系统的通用方法，也适用于将自己设计的传递函数形式的控制器转化成代码。需要注意的是，如果传递函数是连续形式的，需要先经过离散化，可以利用MATLAB的c2d函数。
最后附上频谱分析的m文件和滤波器实现的C代码，有优化空间，仅供参考。
滤波器的重要作用不必多说，在传感器数据处理、电机控制、视觉算法中都会用到。这里举一个最简单的例子，对陀螺仪采集的数据进行低通滤波，从而达到去除噪声的作用。
1. 分析原数据
首先我们将MPU6500的陀螺仪数据带宽设置为250Hz，然后使用J-Scope采集静置时候的陀螺仪x轴的数据。可以看出角速度有幅度约为0.3°/s的跳动。（为了让J-Scope读到值，这里将原始数据乘以1000转化成整型了）
对数据进行FFT变换之后得到频谱图：
可以看出信号在250Hz之后有衰减，证明MPU6500确实有对原始adc采样数据做过滤波，但是效果不明显。
2. FDATool工具介绍
现在我们来自己设计一个低通滤波器。
首先打开MATLAB，在命令行窗口键入 fdatool，打开滤波器设计和分析工具（Filter Design & Analysis Tool），初始界面如下图所示：
这是经典的MATLAB工具箱界面，第一行菜单栏，第二行工具栏，中间展示结果，最下方的功能区可以切换，默认的功能就是设置滤波器参数。这点英文肯定难不倒大家，随便点点看就能学会。
3. 设计滤波器
在设计滤波器界面，可以根据自己的需求选择滤波器类型、阶数、频率特性、幅度特性等参数。
例如我选择了切比雪夫Ⅱ型低通滤波器，采用Minimum order模式（根据频率、幅度指标自动选择最小阶数），设置采样频率（Fs）1000Hz，通带频率（Fpass）50Hz，阻带频率（Fstop）60Hz，衰减幅度保持默认。点击Design Filter按钮，一个滤波器就出来了。
该滤波器是17阶的，点击工具栏的按钮可以看到该滤波器的详细特性，例如幅频、相频、群延时、零极点等等。蓝色线表示的幅频响应曲线，橙色线是相频响应曲线。可以看出切比雪夫Ⅱ型滤波器在通带内平滑，阻带内有等幅度纹波。
4. 转化和导出
FDATool默认生成的滤波器是二阶级联组成，这样的稳定性更高（尤其是滤波器阶数较高时），但是结构比较复杂，程序实现也不方便，一般将其转化成一阶级联型再使用，点击菜单栏的Edit->Convert to Single Section进行转换。至此，一个50Hz的切比雪夫Ⅱ型低通滤波器就完成了。
点击 Targets-> Generate C Header，将滤波器的系数存到.h文件中。然后我们可以进入C语言的部分了。
5. C语言实现
把系数提取出来，然后把下面这个滤波器函数放到定时器中断或者数据接收中断处理函数里面，按照固定频率运行，即可输出滤波后的陀螺仪数据了。（具体原理再文章最后有介绍）
（注意：分子系数是成对出现的，可以先合并来提高运算效率）
6. 结果对比
静止测试时，噪声得到了有效的抑制。
再看频谱，滤波效果非常明显，50Hz以上的分量几乎被完全消除了。
动态测试图：
曲线平滑很多，毛刺都被消除了，但是可以明显看出，滤波之后的信号相比原信号有滞后，这是滤波造成的相位延迟。这里只是为了给大家展示滤波器的效果，实际上该滤波器并不一定适合用于对陀螺仪数据进行处理，要综合考虑各种因素来确定滤波器。
7. 计算原理
详细内容可以去翻信号与系统的课本：
FDATool生成的滤波器其实传递函数形式 ，两组系数分别是传递函数的分子分母，结构如下：
上下同除以z^n ：
将上式展开，并利用z变换的时移性质
最后整理为：
此即代码里循环累加的过程。
这个算法是用差分方程表示线性时不变系统的通用方法，也适用于将自己设计的传递函数形式的控制器转化成代码。需要注意的是，如果传递函数是连续形式的，需要先经过离散化，可以利用MATLAB的c2d函数。
最后附上频谱分析的m文件和滤波器实现的C代码，有优化空间，仅供参考。
关联专栏
电控开源专栏请问这篇文章对你有用吗？
【分享帖】RM入门教学系列数字滤波器设计与实现（含代码）
