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

[算法]飞控中的一阶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来测试一下滤波器的性能吧。

%基波:幅值为31Hz正弦波
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源码,同时欢迎加我的个人微信交流。

你可能感兴趣的:(Pixhawk,算法)