一阶 RC 低通滤波算法

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

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}{dt}=(\frac{ d{(CU_o)} }{d_t})=C \frac{ d_{U_o} }{d_t}$$

$$Ui=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\piT})})$$
因为
$$f_c=\frac{1}{2\pi
RC}$$
所以
$$RC=\frac{1}{2\pif_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)=AX(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函数计算分子和分母系数。

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