常見濾波算法及其優(yōu)缺點(diǎn)

?濾波算法?是一種用于減少或消除信號中不需要的成分(如噪聲、干擾等)的技術(shù)。常見的濾波算法包括限幅濾波法、中位值濾波法、算術(shù)平均濾波法、遞推平均濾波法、中位值平均濾波法等。每種算法都有其特定的應(yīng)用場景和優(yōu)缺點(diǎn)。濾波是指通過某種方法將信號中的某些頻率成分增強(qiáng)或抑制，達(dá)到去除噪聲、改善信號質(zhì)量、分離信號等目的的過程。濾波器是實(shí)現(xiàn)濾波功能的關(guān)鍵組件，它可以改變信號的頻譜特性，對不同頻率區(qū)域的信號進(jìn)行處理。

濾波算法是一種數(shù)字信號處理技術(shù)，其目的是通過特定的數(shù)學(xué)模型和方法，從原始信號中去除或減輕不需要的成分(如噪聲、高頻干擾等)，提取或增強(qiáng)有用的信息。它廣泛應(yīng)用于各個領(lǐng)域，包括通信、語音處理、圖像處理、物理測量等。

使用場景：

(1)頻域?yàn)V波：FFT允許將時域信號轉(zhuǎn)換到頻域，然后可以輕松地應(yīng)用各種濾波器，如低通、高通、帶通和帶阻濾波器。

(2)信號分析：FFT常用于分析信號的頻率成分，識別信號中的周期性成分或檢測特定頻率的信號。

(3)圖像處理：在圖像處理中，F(xiàn)FT可以用于頻域?yàn)V波，如銳化、模糊、邊緣檢測等。

常見濾波算法及其優(yōu)缺點(diǎn)

?限幅濾波法(又稱程序判斷濾波法)?：

?方法?：根據(jù)經(jīng)驗(yàn)判斷，確定兩次采樣允許的最大偏差值(設(shè)為A)，每次檢測到新值時判斷：如果本次值與上次值之差<=A，則本次值有效;如果本次值與上次值之差>A，則本次值無效，放棄本次值，用上次值代替本次值。

?優(yōu)點(diǎn)?：能有效克服因偶然因素引起的脈沖干擾。

?缺點(diǎn)?：無法抑制周期性的干擾，平滑度差?12。

?中位值濾波法?：

?方法?：連續(xù)采樣N次(N取奇數(shù))，把N次采樣值按大小排列，取中間值為本次有效值。

?優(yōu)點(diǎn)?：能有效克服因偶然因素引起的波動干擾，對溫度、液位的變化緩慢的被測參數(shù)有良好的濾波效果。

?缺點(diǎn)?：對流量、速度等快速變化的參數(shù)不宜?13。

?算術(shù)平均濾波法?：

?方法?：連續(xù)取N個采樣值進(jìn)行算術(shù)平均運(yùn)算。N值較大時，信號平滑度較高，但靈敏度較低;N值較小時，信號平滑度較低，但靈敏度較高。

?優(yōu)點(diǎn)?：適用于對一般具有隨機(jī)干擾的信號進(jìn)行濾波。

?缺點(diǎn)?：對測量速度較慢或要求數(shù)據(jù)計算速度較快的實(shí)時控制不適用?13。

?遞推平均濾波法(又稱滑動平均濾波法)?：

?方法?：把連續(xù)取N個采樣值看成一個隊(duì)列，每次采樣到一個新數(shù)據(jù)放入隊(duì)尾，并扔掉原來隊(duì)首的一次數(shù)據(jù)(先進(jìn)先出原則)，把隊(duì)列中的N個數(shù)據(jù)進(jìn)行算術(shù)平均運(yùn)算。

?優(yōu)點(diǎn)?：對周期性干擾有良好的抑制作用，平滑度高。

?缺點(diǎn)?：靈敏度低，對偶然出現(xiàn)的脈沖性干擾抑制作用較差?13。

?中位值平均濾波法(又稱防脈沖干擾平均濾波法)?：

?方法?：連續(xù)采樣N個數(shù)據(jù)，去掉一個最大值和一個最小值，然后計算N-2個數(shù)據(jù)的算術(shù)平均值。

?優(yōu)點(diǎn)?：融合了中位值濾波法和算術(shù)平均濾波法的優(yōu)點(diǎn)，能有效消除由于脈沖干擾所引起的采樣值偏差。

?缺點(diǎn)?：比較浪費(fèi)RAM?13。

在對物體進(jìn)行跟蹤的過程中,我們需要對物體下一時刻的位置,運(yùn)動狀態(tài)進(jìn)行預(yù)測. 預(yù)測總是基于一定的規(guī)則, 對于車輛而言, 運(yùn)動模型有CV(constant velocity), CA(constant acceleration), CTRV(constant turn rate and velocity) ，IMM(Interacting Multiple Model)等等. 每次預(yù)測之后，我們需要使用合適的濾波算法，用測量值對預(yù)測值進(jìn)行更新，獲得一個更為精確的運(yùn)動信息。常用的濾波算法包括卡爾曼濾波，粒子濾波、貝葉斯濾波等等。其中卡爾曼濾波包括線性卡爾曼，非線性的擴(kuò)展卡爾曼(EKF)以及無損卡爾曼(UKF)等等。

卡爾曼濾波算法是對線性高斯系統(tǒng)(線性系統(tǒng)且噪聲服從高斯分布)的最優(yōu)無偏估計，而實(shí)際工程中大部分是非線性非高斯系統(tǒng)，因此有了各種變體解決非線性非高斯系統(tǒng)的最優(yōu)估計問題：

EKF濾波：是KF算法在非線性非高斯系統(tǒng)的直接擴(kuò)展，首先對系統(tǒng)進(jìn)行了線性化(注：其線性化工作點(diǎn)是預(yù)測狀態(tài)的均值)其次，假設(shè)在線性系統(tǒng)近似下，系統(tǒng)狀態(tài)和噪聲都是高斯分布，因此經(jīng)過這樣處理后可以直接套用KF方法。IEKF迭代濾波：為了解決EKF濾波中每次線性化的工作點(diǎn)不是真實(shí)的輸入狀態(tài)的均值，因此計算出來的結(jié)果肯定也不是最好的，所以IEKF將其線性化的工作點(diǎn)設(shè)置為上一次迭代的后驗(yàn)均值(對應(yīng)的EKF線性化工作點(diǎn)是預(yù)測狀態(tài)均值)，更加接近真實(shí)狀態(tài)，以此提高精度。ESKF濾波：估計值和真實(shí)值之間差的是一個誤差，而我們ESKF的設(shè)計思想就是利用卡爾曼濾波來實(shí)時估計這個誤差，而且通常，系統(tǒng)狀態(tài)都是比較大的值，其非線性比較強(qiáng)的值，而系統(tǒng)誤差相對來說是個小量，相對來說線性程度很可能比較高，因此再對其進(jìn)行線性化的誤差更小，精度也就更高。UKF濾波：是為了能更逼近輸出結(jié)果的分布，其基本思想就是用一個高斯分布近似系統(tǒng)輸出的分布，其方法是多選幾個采樣點(diǎn)(EKF只有一個線性化工作點(diǎn))一般對于維數(shù)為N的分布，需要選2N+1個點(diǎn)。這樣的好處是，不用求解雅可比，計算量降低。粒子濾波PF：粒子濾波直接舍棄了高斯分布的假設(shè)，因此就需要一個新的方式去描述輸出函數(shù)的分布，PF基本思想基于蒙特卡洛方法，用大量采樣點(diǎn)去描述這個分布，做法就是不斷采樣，計算權(quán)重，重采樣。優(yōu)點(diǎn)是精度肯定更高了，缺點(diǎn)就是采樣所需的粒子數(shù)量，隨分布是指數(shù)增長的，對于高維問題計算量猛增。非線性優(yōu)化：非線性優(yōu)化是將當(dāng)前時刻所有狀態(tài)構(gòu)建代價函數(shù)，進(jìn)行迭代優(yōu)化(剃度下降：牛頓法、高斯牛頓法、列文伯格-馬夸爾特法等)，每次迭代會把上一次迭代的結(jié)果作為新的工作點(diǎn)進(jìn)行優(yōu)化。因此可以考慮到整個系統(tǒng)所有約束，精度高，但是速度就慢，因此也有提出一些加速方法，例如：滑動窗口等。

▲ 平衡雙輪自行車

01慣性傳感器

在車模直立控制中被廣泛使用的慣性傳感器通常是由加速度計和陀螺儀組成。價格比較便宜的慣性測量傳感器，輸出的信號往往包含有各種噪聲。陀螺儀輸出車模傾角 的變化率(角速度) 信號。它通常包括緩慢變化的偏移量 和高斯白噪聲 。

其中偏移量 的導(dǎo)數(shù)是隨機(jī)的，通常描述為一個白噪聲信號： 。

在重力場中，加速度傳感器正交的兩個輸出，通過求反正切可以傳感器相對于重力加速度的傾角。但傳感器的運(yùn)動所產(chǎn)生的加速度也會疊加在輸出信號中，進(jìn)而產(chǎn)生干擾信號。$$\theta _M = \theta T + n\theta$$

▲ 采集數(shù)據(jù)及其顯示

通過濾波器算法將陀螺儀和加速度計所得到的角度信息進(jìn)行融合，最大限度抑制噪聲對測量角度的影響。

前面提問中所提到的三種濾波算法，分別利用傳感器噪聲不同特點(diǎn)來抑制噪聲的影響?？此七@些方法原理相差很大，借助于以下條件，能夠便于我們進(jìn)行分析和比較：

這三種算法都屬于線性濾波算法;

在穩(wěn)態(tài)下，可以借助于輸入輸出之間的傳遞函數(shù)來比較算法的特性;

針對車模直立控制，所涉及到的角度只有一個;

02三種濾波算法

1.參考濾波方案

在2012年直立車模組別首次引入智能車競賽的，競賽秘書處給出了一個 參考設(shè)計方案[1] 。它是在對陀螺儀信號進(jìn)行積分的基礎(chǔ)上，使用加速度計得到的角度信號對輸出進(jìn)行負(fù)反饋調(diào)節(jié)，從而抑制積分中存在的發(fā)散問題。下圖是參考設(shè)計方案中相應(yīng)的原理框圖和程序?qū)崿F(xiàn)。

▲ 參考濾波方案軟件實(shí)現(xiàn)

加速度傳感器的Z周輸出電壓，經(jīng)過偏移量，比率調(diào)整之后，形成算法的輸入:

陀螺儀的輸出電壓經(jīng)過偏移量，比率調(diào)整之后，形成算法輸入:

算法的輸出為角度： (g_fCarAngle)。

▲ 參考濾波方案的核心算法

根據(jù)參考程序代碼，算法可以使用迭代方程描述為：

輸出角度

通過上面的化簡公式可以看出，實(shí)際上角度輸出信號是陀螺儀和加速度計信號經(jīng)過一級低通濾波后疊加在一起，所以陀螺儀和加速度計輸出信號中如果存在直流分量的漂移，都會引起輸出角度信號的零點(diǎn)漂移。

2. 互補(bǔ)濾波器

互補(bǔ)濾波器的基本結(jié)構(gòu)如下圖所示。假設(shè)兩個測量信號 都包含了實(shí)際信號 以及噪聲。在信號 中的噪聲主要分布在高頻，而在信號 中的噪聲主要分布在低頻。

代表著低通濾波器，那么對應(yīng)的 就是互補(bǔ)的高通濾波器。它們分別提取 中的低頻和高頻成分，然后疊加在一起。

▲ 左：基本互補(bǔ)濾波器的結(jié)構(gòu)。右：基本結(jié)構(gòu)的變形，只對噪聲進(jìn)行低通濾波

在上圖右邊給出了互補(bǔ)濾波器基本結(jié)構(gòu)的恒等變形。將 與 相減之后，就剩下了高頻噪聲 與低頻噪聲 相減。再經(jīng)過低通濾波器 ，就剩下 去抵消 中的低頻噪聲了。

只所以討論這個變形，就是因?yàn)榍懊鎱⒖挤桨妇褪亲冃蔚幕パa(bǔ)濾波器。將陀螺儀信號 的積分看做帶有低頻噪聲(積分漂移信號)的觀察信號 。它與加速度計信號 (帶有高頻運(yùn)動噪聲)相減，后面有比例、積分組成的負(fù)反饋組成一個一階低通濾波器 ，濾波之后與 疊加，形成最后的輸出信號。

▲ 參考濾波方案本質(zhì)上是變形后的互補(bǔ)濾波器

通過上面討論可以看出，如果在互補(bǔ)濾波器中的低通濾波器的階次為一階的話，那么就和前面的參考方案是一致了。

在實(shí)際應(yīng)用中，可以增加濾波器的階次，來提高角度計算的精度。比如在陀螺儀輸入信號中，再增加一級高通濾波，可以進(jìn)一步抑制陀螺儀信號中的直流分量的漂移。

3. 卡爾曼濾波

使用卡爾曼濾波的方法來融合來自陀螺儀和加速度計的信號，不在頻率范圍內(nèi)考慮它們各自噪聲的特點(diǎn)，而是將這兩個信號從功能上分為兩類。陀螺儀信號 是過程驅(qū)動變量，它激動系統(tǒng)狀態(tài)(車模傾角，角速度)發(fā)生變化;加速度計得到的角度 則是觀察變量，是來修正系統(tǒng)狀態(tài)。。

選擇車模傾角 和陀螺儀偏變化量 作為系統(tǒng)狀態(tài)變量，得到離散系統(tǒng)狀態(tài)方程為：

則對應(yīng)卡爾曼濾波方程為：

在這個過程中，如果系統(tǒng)噪聲協(xié)方差矩陣 以及觀測噪聲方差 始終為常量。那么由上面方程2、3、5迭代之后，誤差協(xié)方差矩陣 和卡爾曼增益矩陣 就會逐步收斂到一個常量。

在下面參數(shù)下，經(jīng)過迭代1000次之后，卡爾曼增益矩陣參數(shù) 的變化曲線?？梢钥慈ニ饾u曲線與常數(shù) 。

當(dāng)為常量的時候，對方程1、4再聯(lián)立起來，就形成關(guān)于狀態(tài)變量 的差分方程?？紤]到 ， 都比較小，將化簡過程中涉及到這些小量乘積(二階小量)都省略掉，那么最終可以將輸出角度 與輸入 之間的差分方程化簡為：

其中 分別是 的z變換。對照上面簡化的輸入輸出之間的關(guān)系與前面參考方案中對應(yīng)的方程，可以看到卡爾曼濾波器本質(zhì)上和互補(bǔ)濾波器是相同的。如果不省略那些參數(shù)小的項(xiàng)數(shù)，濾波算法就變成二階的互補(bǔ)濾波器的形式了。

觀察噪聲 來自于加速度計傳感器，系統(tǒng)內(nèi)部噪聲 來自于陀螺儀。當(dāng) 遠(yuǎn)遠(yuǎn)大于 的時候，所計算出的卡爾曼濾波增益 的數(shù)值比較小，這樣才能夠進(jìn)行上述的算法簡化。

03算法關(guān)系

可以看出。當(dāng)所使用的慣性傳感器，陀螺儀的噪聲遠(yuǎn)遠(yuǎn)低于加速度傳感器的噪聲時，低通濾波器的階次取一階濾波器的情況下，三種濾波器的算法效果在平穩(wěn)狀態(tài)下是相同的。

從原理上，參考方案與互補(bǔ)濾波器比較接近，它們是利用不同傳感器噪聲在頻率上的差別抑制噪聲的。卡爾曼濾波器則根據(jù)傳感器對傾角影響的機(jī)制不同，通過構(gòu)造系統(tǒng)狀態(tài)變量將信息進(jìn)行融合。

在車模直立控制應(yīng)用中，互補(bǔ)濾波器和參考方案所需要的計算量小，參數(shù)只有濾波時間常數(shù) ，調(diào)試相對比較簡單。但該算法對于陀螺儀工作點(diǎn)的漂移無法抑制，使用時，可以對陀螺儀信號再通過一個高通濾波器，消除陀螺儀信號工作點(diǎn)變化對輸出角度的影響。

卡爾曼濾波器計算量相對較大，需要MCU的算力強(qiáng)才行。由于卡爾曼濾波器內(nèi)部變量包括有對陀螺儀工作點(diǎn)的估計，所以陀螺儀的工作點(diǎn)的緩慢變化不會影響計算角度的變化。

▲ RECORD1數(shù)據(jù)

在使用濾波器算法的時候，都會涉及到算法的初始狀態(tài)的設(shè)置問題。對于互補(bǔ)濾波器相對比較簡單，將內(nèi)部積分累加變量初始化為0便可以適應(yīng)大部分情況。對于卡爾曼濾波器，它的參數(shù)矩陣 則需要選擇恰當(dāng)?shù)某跏贾?，以使得算法盡快達(dá)到收斂的目的。比如可以將算法收斂后的 矩陣的值作為程序下一次運(yùn)行的初始值，這樣就可以更快使得算法進(jìn)入穩(wěn)定狀態(tài)。

此外，需要根據(jù)傳感器輸出信號的實(shí)際情況，合理的確定卡爾曼濾波器中的 參數(shù)，這樣才能夠在算法的收斂以及濾波效果方面取得良好的平衡。

關(guān)于這三種算法在理論上的分析就到這里，它們在實(shí)際控制中的效果以后在給出。