实战低通滤波和卡尔曼滤波

背景

滤波这个词对任何一个工科生都不会陌生，尤其是做控制或者信号方面的从业者和学生。我们不仅可以通过硬件滤波也可以通过软件设计算法滤波，这些都是非常平常的。滤波的方法有太多了，从简单的均值滤波、中值滤波到登月的卡尔曼滤波（其实我认为应该叫状态估计）。往往我们都会选择适合工程需求的滤波器对信号进行滤波。那么本次就从理论到实践，从公式推导到算法实现来学习一阶低通滤波器和卡尔曼滤波器。

所需硬件与软件

1.笔和纸。
2.Matlab/Simulink
3.带有编码器测速的直流电机

理论基础

一阶低通滤波

这类滤波器是工程中最常用的，实现起来也非常简单的，跑到嵌入式系统里可以应付绝大多数工程需求。本科上自动控制原理的时候老师也会经常说一阶系统可以低通滤波 ，一切都需要从这个传递函数说起。
$$G(s)=\frac{a}{s+a}$$
做控制的人一看会说这是个一阶系统，做信号的人一看会说这是个低通滤波器。接下来分析一下为什么它能进行滤波，那么就需要从这个传递函数的频响先入手。令$s=jw$。得到下式：
$$G(j\omega )=\frac{a}{j\omega +a}$$
那么上下同时乘上$a-jw$，再进行化简就可以得到下式：
$$G(j\omega )=\frac{a^2}{a^2+{\omega }^2}+(-\frac{a\omega }{a^2+{\omega }^2})j$$
显而易见，前者是实部，后者是虚部。那么就非常容易得出这个系统的幅频特性和相频特性了。先看幅频特性,也可以称振幅响应，具体计算过程省略，实部的平方加虚部的平方开根号即可得出：
$$\left|G(j\omega )\right|=\sqrt{\frac{1}{1+(\frac{w}{a}{)}^2}}$$
那么可以分析一下$\omega$和$a$的关系。
1.当$\omega$<<$a$时，模值趋向于1。并且随着$\omega$增大，模值会减小。
2.当$\omega$=$a$时，模值为$\sqrt{1/2}$，即0.707。
3.当$\omega$>>$a$时，模值趋向于0。
可以发现随着频率增加，振幅在不断的减小，并且a为截止频率。大家有兴趣的话可以自己推算下相频特性。
那么这样一个滤波器要应用跑进嵌入式系统里面，是不能在连续域处理的，所以我认为分析控制系统应该多在离散域看问题，这样部署在控制器里会更加方便。那么对于如下一个一阶系统如何变换到离散域，写成我们熟悉的差分方程供单片机使用呢。
$$G(s)=\frac{1}{\tau s+1}$$
首先这里引入了时间常数$\tau$，如果我们学过一阶系统的时域分析，系统输入一个单位阶跃信号，单位阶跃信号的传递函数是$1/s$。然后对整个系统的传递函数进行拉普拉斯逆变换，最后可以发现，时间$\tau$所对应的值是阶跃值的63%。有兴趣的同学可以自己推算一下，十分简单。那么时间常数和截止频率又是什么关系呢？
要对这个传递函数进行离散化，这里采用一阶后向差分进行离散化。令$s=\frac{1-z^{-1}}{T}$，其中T是采样时间。代入传递函数后得：
$$G(z)=\frac{Y(z)}{U(z)}=\frac{T}{\tau (1-z{)}^{-1}+T}$$
变换一下：
$$(\tau +T)Y(Z)-\tau z^{{}^{-1}}Y(z)=U(z)T$$
转换为差分方程的形式：
$$(\tau +T)y(t)-\tau y(t-1)=u(t)T$$
最后再整理一下：
$$y(t)-(1-k)y(t-1)=ku(t)$$
其中，$k=\frac{T}{T+\tau }$。之所以我会推导一遍，是因为网上关于这个滤波器的推导我没找到，都是直接给出数字低通滤波器公式，并且很多教程直接说$k=\frac{T}{\tau }$，那么对一阶系统时间常数$\tau$的理解就错了。得到这个公式你尝试一下不同$\tau$对系统的影响。
如果采用零阶保持法对传递函数进行离散化，那么最后得到的还是这个公式吗，各位可以推导一下。我用零阶保持法推导出来的数字滤波器如下：
$$y(t)-(1-k)y(t-1)=ku(t-1)$$
其中$k=e^{-\tau T}$，各位有兴趣可以在Matlab里面用c2d()离散化后自己用差分方程表述。

卡尔曼滤波

卡尔曼状态估计我想在目前自动驾驶等所谓人工智能领域已经火的不能再火了，网上各类博客和论文随便一搜就是相关信息，理论和公式推导十分漂亮。下面我引用一起留下脚印的2019年的一篇原创文章《Simulink之卡尔曼滤波》，脚主作为一线工程师的确在他那里学到了很多专业技能！点赞！因为要用到实际控制器中，这里就采用扩展卡尔曼滤波器。
对任何信号s(t),在$t+\Delta t$时刻按泰勒公式展开，取二阶导数：
$$s(t+\Delta t)=s(t)+\Delta t\times \dot{s}(t)+\frac{1}{2}\Delta t\times \ddot{s}(t)+w_1(t)$$对上式分别求一阶导数和二阶导数：
$$\begin{gathered} \dot{s}(t+\Delta t)=\dot{s}(t)+\Delta t\times \ddot{s}(t)+w_2(t) \\ \ddot{s}(t+\Delta t)=\ddot{s}(t)+w_3(t) \end{gathered}$$式中$t+\Delta t$为t时刻的领域内任一时刻，w1,w2,w3为泰勒展开余项。
设系统采样时间为T，由上述各式得卡尔曼滤波系统状态空间模型。
$$\begin{gathered} X(k+1)=AX(k)+W(k) \\ y(k)=HX(k)+e(k) \end{gathered}$$其中，$X(k)=[s(k)\dot{s}(k)\ddot{s}(k){]}^T$ ,$A=\left[\begin{array}{ccc}1& T& \frac{T^2}{2}\\ 0& 1& t\\ 0& 0& 1\end{array}\right]$,$W(k)=[w_1{w}_2{w}_3{]}^T$,$H=[100]$。
式中，X(k)为系统K时刻的状态向量，A为系统举证，W(k)为系统噪声，y(k)为K时刻的测量信号，H为信号观测矩阵，e(k) 为信号观测噪声。
给定滤波初值X(0|0)、P(0|0)，最优状态估计X(k|k)则是我们需要的，这个值是通过不停迭代得出的。
$$X(k|k-1)=AX(k-1|k-1)$$$$P(k|k-1)=AP(k-1|k-1)A^T+Q$$$$X(k|k)=X(k|k-1)+K(y(k)-HX(k|k-1))$$$$K(k)=P(k|k-1)H^T(HP(k|k-1)H^T+R{)}^{-1}$$$$P(k|k)=(I-K(k)H)P(k|k-1)$$
以下就是卡尔曼滤波器的5个核心公式，我认为看一遍网上或者论文推导就行了，放心吧，记不住的。工具会拿来用，知道是怎么回事就行了。我曾记过很多次，发现和没记一样，用的时候拿来看看。

仿真验证

有了上面的推导，我们很容易就可以进行算法的建模了，打开熟悉的MATLAB软件，新建Simulink模型。根据一阶数字低通滤波器的差分方程即可轻易构建滤波器。如下图所示。

卡尔曼滤波器则模仿脚主的用5个MALTAB Fcn建立，分别对应5个公式，您可以用一个MATLAB Fcn建立，这里我不建议用S函数写，因为在基于模型的开发阶段，用S函数编写是需要另外学习tlc编程，才可以进行代码自动生成，所以能不碰就别碰。当然在Control System Toolbox中有封装好的Kalam Filter模块，如果您购买了该工具箱可以使用。

下面则是卡尔曼滤波器参数。

好的，目前为止两个滤波器的模型就搭建完了，那么建立输入，这里我采用一个带噪声的正弦输入。整体模型架构如下：

可以看到信号输入如下图所示。

运行仿真程序，在SDI中对比卡尔曼滤波器和低通滤波器的效果。

不难发现，在该参数下，两种滤波器都取得了不错的效果，这里吧两种滤波器效果和未加入噪声的正弦信号进行对比。

可以发现，卡尔曼滤波器（紫色线）可以更好的跟踪实际曲线，低通滤波器（绿色线）在相位上有一定的滞后，而且这个滞后随着时间常数增加到1时，滞后的角度变大，直到45度附近，为什么我这么说，这就要回到最开始一阶系统的相频特性了，画出系统的Bode图便一目了然。当然我知道各位观众是不会自己去尝试的。0202年了，自动控制原理课还在教手绘Bode图,手绘根轨迹之类的。在Matlab中一个函数就可以调用了。相比可以手绘，去读懂Bode图的信息难道不是更重要吗？
下面针对时间常数$\tau =0.1$的的一阶系统绘制Bode图。

系统的截止频率对应-3db,频率为10rad/s。那么在仿真环境中输入一个10rad/s频率的正弦信号加噪声（噪声的频率肯定高于10rad/s），通过这个传递函数便可以滤除高于10rad/s的噪声。看一下是什么效果。其中正弦波模块输入信号频率10rad/s。

仿真完毕直接在SDI中查看结果。

从仿真结果结合Bode图可以看出，系统过滤了10rad/s以上的频率，并且相位滞后了45度，这里结合bode的相位曲线可以明确看出为什么会滞后45度。并且振幅在0.707，对应了截止频率的幅值。结合这个例子我相信各位对这个低通滤波器更加了解了吧。
下面就要把这两个滤波器部署到之前的小车驱动系统的轮速滤波中了。

实际验证

完成了仿真验证后，将滤波器算法放在小车的控制策略中，如下图所示，通过编码器的读数转换，得到轮速。看一下实际的滤波效果。这里采用开环PWM调速，就没有用闭环PI调速了。通过改变驱动芯片占空比改变电机转速。

把这个小车调试线连接到电脑USB口。

软件完成后点击在线调试按钮，直接把模型部署到小车上。这样就可以在线修改Simulink参数了。停止仿真后实际控制器算法直接清除，如果需要刷写进控制器，则点击旁边的Build Deploy& Start按钮。

仿真完成看，进入SDI看效果，这里我直接将传感器信号线接进IO口，没有进行电阻分压，没有烧坏硬件，但是信号效果没有加电阻分压后好。

如果您觉得不满意滤波效果，就需要去调节Q和R的矩阵参数， 这个调试的经验我也是不具备的，就是不停的更改值罢了，当然如果您有比较好的方法，能有理论指导实践更好！请在评论区留言，感谢大佬！
从结果看，卡尔曼滤波后的速度曲线跟踪信号的响应更好，一阶低通滤波后的信号效果在位置上有一些滞后。综合考虑，我会选择一阶低通滤波器，我可以减小时间常数$\tau$牺牲滤波效果从而平衡相位上的滞后。原因是卡尔曼滤波器在任务周期内的计算时间很庞大，涉及到矩阵的乘法，求逆，这对低成本硬件的算力负担还是比较重的，虽然我用的是32位的处理器。况且在板子里还跑了各种控制策略，其中开环查表也相当费时间，并且在其他地方还会用到滤波算法。

总结

这次研究卡尔曼是因为在MPC对模型进行状态估计的时候需要用卡尔曼状态估计，所以我就拿过来看一下对实际的滤波效果如何。我个人不会太计较到底用什么算法，能满足需求的算法才是最好的算法，开环可以满足需求就不用闭环，传感器难道不花钱吗,难道很便宜吗？所以我认为做的一切都需要回溯到需求定义上面去。

文中如有错误，请各位积极批评指正，本人定虚心学习，欢迎各位交流讨论！