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

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

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

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

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

$Out_1+=(\frac{1}{1+(\frac{1}{2 \pi * T})}) *(In_0-Out_0)$ $Out_1=Out_0+(\frac{1}{1+(\frac{1}{2 \pi * T})}) *(In_0-Out_0)$

令：

$A=(\frac{1}{1+(\frac{1}{2 \pi * T})})$

可以得到:

$Out_1=(1-A)Out_0+A*In_0$

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

电路中的滤波

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

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

时域分析

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

$I_c=\frac{d_q}{d_t}=(\frac{ d_{(C*U_o)} }{d_t})=C *\frac{ d_{U_o} }{d_t}$ $U_i=R*C(\frac{ d_{U_o} }{d_t})+U_o$

解微分方程可以得到

$U_o(t)=U_i(1-e^{-\frac{t}{RC}})$

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

$\tau=RC$

频域分析

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

$H(jw)=(\frac{U_o}{U_i})=(\frac{\frac{1}{jwc}}{ R+\frac{1}{jwc} })=\frac{ 1 }{1+jwRC}$

可得电压的传递函数为：

$\frac{U_o}{U_i}=\frac{1}{RCs+1},(s=jw,w=2\pi f)$

增益

$A(f)=\frac{U_o}{U_i}=\frac{1}{2\pi fRCj+1}$

$w_c$为截止角频率

$w_c=\frac{1}{RC}$

$f_c$截止频率

$f_c=\frac{1}{2\pi RC}$

离散化

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

$s=\frac{1-z^{-1}} {T}$

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

$H(z)=\frac{Y(z)}{X(z)}=\frac{1}{RC{\frac{1-z^{-1}}{T}+1}}=\frac{T} {RC(1-Z^{-1})+T}$ $X(z)=Y(z)+\frac{RC(1-Z^{-1})+T}{T}$ $X(z)=\frac{RC}{T}Y(z)-\frac{RC}{T}Y(z)(Z^{-1})+Y(z)$

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

$X(n)=(1+\frac{RC}{T})Y(n)-\frac{RC}{T}Y(n-1)$ $Y(n)=(\frac{1}{1+\frac{RC}{T}})X(n)+(\frac{\frac{RC}{T}}{1+\frac{RC}{T}})X(n-1)$ $Y(n)=\frac{T}{RC+T}X(n)+\frac{RC}{RC+T}Y(n-1)$

令

$A=\frac{T}{RC+T}$

可得

$Y(n)=A*X(n)+(1-A)Y(n-1)$

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

为什么

$A=(\frac{1}{1+(\frac{1}{2\pi*T})})$

因为

$f_c=\frac{1}{2\pi*RC}$

所以

$RC=\frac{1}{2\pi*f_c}$

带入

$A=\frac{T}{RC+T}$

可得

$A=\frac{T}{\frac{1}{2\pi*f_c}+T}=\frac{1}{1+(\frac{1}{2 \pi*Tf_c})}$

滤波器电路经过离散化分析后得到的就是源程序的迭代过程，两者有着相同的数学描述，硬件电路可以实现低通滤波的效果，那么这个程序同样可以，并且通过计算我们可以得到原程序是截止频率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滤波器设计

根据：

$Y(n)=A*X(n)+(1-A)Y(n-1)$ $A=\frac{T}{\frac{1}{2\pi f_c}+T}=(\frac{1}{1+(\frac{1}{2 \pi * Tf_c})})$

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

$Y(n)=Y(n-1)+A*(X(n)-Y(n-1))$ $A=(\frac{1}{1+(\frac{1}{2 \pi * Tf_c})})$

其中输入混合信号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滤波器传递函数为：

$\frac{U_o}{U_i}=\frac{1}{RCs+1}$

取其分子系数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滤波器采用了一阶后向差分法,整理得到：

$H(z)=\frac{TZ} {(RC+T)Z-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函数计算分子和分母系数。

1
2
3

[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之前的曲线几乎是完全重合的，理论性能和实际性能是一致的。

在回到最初的算法

1	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源码，同时欢迎加我的个人微信交流。