[算法]飞控中的低通滤波算法

[算法]飞控中的一阶RC低通滤波算法

---

在阅读飞控的源码时，我们经常看见类似下面的算法

thr_lpf+=(1 / (1 + 1/(2.0f * 3.14f * T )))*(height_thr - thr_lpf)

通过变量名thr_lpf可以知道这是对油门进行低通滤波后的值，可是为什么这个算法可以实现低通滤波呢？它的截止频率是多少呢？我们来一步一步揭开算法背后的秘密。

首先整理一下上式可以得到：

令：

可以得到:

所以程序其实对应的就是这个迭代过程。

电路中的滤波

为了解什么低通滤波，通过搜索发现低通滤波得到的是这样的电路图：

这个算法和电路里的滤波电路有什么关系吗？接下来我们先对滤波电路进行分析。

时域分析

设回路电流为IcIc,由基尔霍夫定律可以写出回路方程为：

解微分方程可以得到

其中RC是时间常数，当电阻单位是”欧姆(ΩΩ)”，电容单位是”法拉(F)”时，时间常数的单位是”秒(s)”。

频域分析

既然是滤波器，我们最想知道的就是它的截至频率，那么就需要从频域分析。
以电容电压作为输出，电路的网络函数[^电网络函数]为：

可得电压的传递函数为：

增益

wcwc为截止角频率

fcfc截止频率

离散化

先从S域到Z域，再将Z反变换求得差分方程
z变换（方法很多，如一阶前向差分、双线性变换等这里用一阶后向差分法）
一阶后向差分法中

其中T是采样周期，带入S域传递函数中

z反变换求差分方程后可得：

令

可得

到这里式子已经跟程序里的非常像了，现在就差系数的问题了

为什么

因为

所以

带入

可得

滤波器电路经过离散化分析后得到的就是源程序的迭代过程，两者有着相同的数学描述，硬件电路可以实现低通滤波的效果，那么这个程序同样可以，并且通过计算我们可以得到原程序是截止频率fc=1Hz,那么用MATLAB来测试一下滤波器的性能吧。

%基波：幅值为3的1Hz正弦波
Signal_Original_1 = 3*sin(2*pi*t); 
%噪声函数 赋值为基波的1/3 10hz 30hz 50hz 100hz 200hz 300hz 400hz 500hz正弦波 
Noise_White_1  = sin(2*pi*10*t)+sin(2*pi*30*t)+sin(2*pi*50*t)+sin(2*pi*100*t)+sin(2*pi*200*t)+sin(2*pi*300*t)+sin(2*pi*400*t)+sin(2*pi*500*t);    
%构造的混合信号
Mix_Signal   = Signal_Original + Noise_White;

我们用MATLAB自带的有源一阶RC低通滤波器（一阶巴特沃斯低通滤波器）来进行对比

设计滤波器

一阶RC滤波器设计

根据：

设计截止频率fCfC=1Hz的一阶RC滤波器：

其中输入混合信号Mix_Signal，经过滤波得到结果LPF_RC

Fs = 10000;  %采样频率 10KHz
fc = 1;      %截止频率 1Hz                       
for a = 1:1:length(t)
    %T采样周期等于1/采样频率
    lpf1=lpf0+(1 / (1 + 1/(2.0 * 3.14*(1/Fs)*fc)))*(Mix_Signal(a) - lpf0);
    lpf0=lpf1;
    LPF_RC(a)=lpf1;
end  

巴特沃斯滤波器设计

输入混合信号Mix_Signal，经过滤波得到结果Butter_Filter

%截止频率 fc
Wc=2*fc/Fs;  
%返回具有归一化截止频率Wn的lv阶低通数字巴特沃斯滤波器的传递函数系数。                                           
[b,a]=butter(lv,Wc,'low');
Butter_Filter=filter(b,a,Mix_Signal);

快速傅里叶变换

将波形进行快速傅里叶变换，可以分析波形里包含的正弦波频率和幅值。

可以看到傅里叶变换后的结果，经过滤波后50hz以后的波基本上都滤干净了，就是10Hz和30HZ还有一点，效果还是非常好的。

幅频特性曲线

幅频特性曲线可以量化滤波器的对不同频率谐波的抑制效果。

标准的RC滤波器传递函数为：

取其分子系数b=[1],分母系数a=[RC 1]

b=[1];
a=[RC 1];
[mag,phase,w]=bode(b,a);
dB=20*log10(mag);
semilogx(w/(2*pi),dB(:),'r'); 

一阶RC滤波器采用了一阶后向差分法,整理得到：

根据MATLAB官方给出的离散形式定义传递函数的分子和分母系数，
https://www.mathworks.com/help/matlab/ref/filter.html?searchHighlight=filter&s_tid=doc_srchtitle

得到b=[T 0],a=[RC+T -RC];

b=[T 0];
a=[RC+T -RC];
[h,w]=freqz(b,a);
set(gca,'XScale','log')
plot((w*Fs/(2*pi)),20*log10(abs(h)),'g')

巴特沃斯滤波是使用双线性变换的RC标准滤波，MATLAB提供了自带的butter函数计算分子和分母系数。

[b,a]=butter(lv,Wc,'low');
[h,w]=freqz(b,a);
plot((w*Fs/(2*pi)),20*log10(abs(h)),'b'); grid;

将三个方法的RC滤波幅频特性曲线画在一起，可以看出不管是一阶向后差分的一阶RC滤波，还是双线性变换的巴特沃斯滤波,在100Hz之前的曲线几乎是完全重合的，理论性能和实际性能是一致的。

在回到最初的算法

thr_lpf+=(1 / (1 + 1/(2.0f * 3.14f * T )))*(height_thr - thr_lpf)

你能想到一个这么短的算法里蕴含了这么多数学原理吗？

参考资料
http://www.360doc.com/content/15/0714/22/22888854_484947052.shtml
https://wenku.baidu.com/view/85f8dc20dd36a32d7375814d.html
https://wenku.baidu.com/view/3efd1b0e51e79b8969022627.html

[网络函数] 动态电路激励作用下下，响应（输出）相量与激励（输入）相量之比，称为网络函数（network functions），记为H。
[z变换] Z变换（英文：z-transformation）可将时域信号（即：离散时间序列）变换为在复频域的表达式。它在离散时间信号处理中的地位，如同拉普拉斯变换在连续时间信号处理中的地位。

关注我的微信公众号，回复【rc】即可获取本文的参考文献和MATLAB源码，同时欢迎加我的个人微信交流。