陳菘 盧敏

摘要：濾波算法常用來(lái)解決對(duì)系統(tǒng)狀態(tài)估計(jì)的問(wèn)題，主要有卡爾曼濾波、粒子濾波以及在此基礎(chǔ)上改進(jìn)的擴(kuò)展卡爾曼，無(wú)跡卡爾曼，無(wú)跡粒子濾波算法等。對(duì)于線性高斯系統(tǒng)模型，卡爾曼濾波有著極強(qiáng)的處理能力，因此得到了廣泛的應(yīng)用。粒子濾波無(wú)須對(duì)系統(tǒng)狀態(tài)做線性高斯假設(shè)，其應(yīng)用范圍大于卡爾曼濾波，但時(shí)間的消耗要遠(yuǎn)遠(yuǎn)大于前者。在介紹了常見(jiàn)濾波算法的原理與應(yīng)用后，通過(guò)仿真實(shí)驗(yàn)對(duì)比了上述幾種常見(jiàn)濾波方法的跟蹤效果。實(shí)驗(yàn)表明，上述算法在非線性高斯模型下均有較好的準(zhǔn)確性與較低的誤差。

關(guān)鍵詞：卡爾曼濾波;擴(kuò)展卡爾曼濾波;無(wú)跡卡爾曼濾波;粒子濾波;高斯噪聲

中圖分類(lèi)號(hào)：TP391.9? ? 文獻(xiàn)標(biāo)識(shí)碼：A

文章編號(hào)：1009-3044（2020）32-0023-03

Abstract： Filtering is used to estimate the system state， including Kalman filter（KF）， particle filter and? extended kalman（EK）， unscented kalman（UK）， unscented particle filter algorithm. For linear Gaussian system， Kalman filter has a strong processing ability， so it has been widely used in the field. Linear and Gaussian assumption is not necessary in particle filter contrast to KF， the application range is larger than t-he latter and the time consumption is stronger than the latter. introducing the principle a=nd application of filtering algorithms， the tracking effect are compared through simulation experiments. Experimental resultsshows that improved algorithm have better accuracy and lower error in the model of nonlinear gaussian.

Key words： kalman filter; extended KF; unscented KF; particle filter; gaussian noise

1 引言

濾波問(wèn)題是求解感興趣分布的后驗(yàn)概率分布[1]。求解后驗(yàn)概率分布問(wèn)題有兩種方法，第一種是基于貝葉斯理論求解出具體的解析解，貝葉斯濾波通過(guò)結(jié)合先驗(yàn)概率密度與觀測(cè)似然函數(shù)來(lái)求解估計(jì)問(wèn)題[2]。這一方法主要包括卡爾曼濾波、擴(kuò)展卡爾曼濾波。第二種方法是基于蒙特卡羅與大數(shù)定理的思想從原分布中采樣從而估計(jì)系統(tǒng)真實(shí)的狀態(tài)[3]。蒙特卡羅思想指的是用一系列抽樣樣本值模擬原分布。這一方法主要包括無(wú)跡卡爾曼濾波與粒子濾波[4]。表1是上述幾種濾波方法應(yīng)用條件的對(duì)比。

由表1可得，卡爾曼濾波只能應(yīng)用在線性高斯過(guò)程中，其改進(jìn)方法擴(kuò)展卡爾曼濾波與無(wú)跡卡爾曼濾波對(duì)非高斯噪聲有一定的處理能力，但是三者均不能很好地處理非線性系統(tǒng)。無(wú)論系統(tǒng)線性與否，噪聲是否服從高斯分布，粒子濾波均可以處理，因此粒子濾波算法獲得了廣泛的應(yīng)用。

2 卡爾曼濾波及其變形

狀態(tài)模型由狀態(tài)轉(zhuǎn)移過(guò)程和觀測(cè)過(guò)程兩部分組成，如圖1所示。

應(yīng)用這個(gè)模型有兩個(gè)前提：（1）觀測(cè)獨(dú)立假設(shè)[5]，系統(tǒng)當(dāng)前的觀測(cè)量只與當(dāng)前時(shí)刻的狀態(tài)量有關(guān)，與其余時(shí)刻的狀態(tài)無(wú)關(guān);（2）一階馬爾可夫過(guò)程[6]，即系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)量只與上一個(gè)時(shí)刻的狀態(tài)量有關(guān)，與其余時(shí)刻的狀態(tài)量無(wú)關(guān)。

2.1 卡爾曼濾波

卡爾曼濾波的狀態(tài)方程與觀測(cè)方程必須是線性的，即圖1中的[xk-2，xk-1，...zk-2，zk-1]是線性關(guān)系。即狀態(tài)量之間的關(guān)系，狀態(tài)量與觀測(cè)量之間的關(guān)系可以用矩陣來(lái)表示。同時(shí)，觀測(cè)噪聲與狀態(tài)噪聲的統(tǒng)計(jì)分布須滿(mǎn)足高斯分布。在這兩個(gè)前提下，圖1所表示的狀態(tài)空間模型可以使用卡爾曼濾波算法來(lái)處理。卡爾曼濾波由預(yù)測(cè)和更新兩個(gè)步驟構(gòu)成，預(yù)測(cè)是根據(jù)系統(tǒng)上一時(shí)刻的狀態(tài)預(yù)測(cè)系統(tǒng)下一時(shí)刻的狀態(tài)，更新則是結(jié)合最新的觀測(cè)值數(shù)據(jù)對(duì)預(yù)測(cè)的狀態(tài)值進(jìn)行修正從而得到系統(tǒng)的估計(jì)值。

式（1）到（5）中，[xt]表示狀態(tài)的預(yù)測(cè)量，[xt]表示真實(shí)的狀態(tài)量，[Ut]表示控制量[7]，[Pt]表示預(yù)測(cè)誤差的估計(jì)值，[Rt]表示過(guò)程噪聲，[Kt]表示卡爾曼增益，[A]表示狀態(tài)轉(zhuǎn)移矩陣，[Zt]表示觀測(cè)值，[Pt]表示預(yù)測(cè)誤差的真實(shí)值，[Q]表示觀測(cè)噪聲。

2.2 擴(kuò)展卡爾曼濾波

當(dāng)系統(tǒng)不滿(mǎn)足線性條件約束時(shí)，即狀態(tài)量與狀態(tài)量，狀態(tài)量與觀測(cè)量之間無(wú)法用矩陣關(guān)系表達(dá)，但過(guò)程噪聲與觀測(cè)噪聲仍滿(mǎn)足高斯分布時(shí)，此時(shí)可通過(guò)泰勒公式近似某點(diǎn)處的狀態(tài)方程實(shí)現(xiàn)非線性方程的線性化，同時(shí)為了避免計(jì)算的復(fù)雜性，擴(kuò)展卡爾曼濾波只近似到泰勒方程的一階項(xiàng)[8]。由于忽略了高階項(xiàng)，不可避免地引入了誤差，當(dāng)非線性過(guò)強(qiáng)時(shí)，往往估計(jì)的效果比較差，甚至可能導(dǎo)致發(fā)散。在擴(kuò)展卡爾曼濾波過(guò)程中，狀態(tài)轉(zhuǎn)移矩陣[A]與觀測(cè)矩陣[Z]要通過(guò)計(jì)算雅克比矩陣與海塞矩陣得到。因此，相比卡爾濾波，增加了計(jì)算的復(fù)雜度。

2.3 無(wú)跡卡爾曼濾波

無(wú)跡卡爾曼濾波引入了無(wú)跡變換。對(duì)于非線性模型，直接用解析的方式來(lái)求解貝葉斯遞推公式比較困難，各個(gè)概率分布的均值和方差比較難求，無(wú)跡變換通過(guò)一定規(guī)律的采樣并賦予采樣值權(quán)重近似獲得均值與方差。通過(guò)無(wú)跡變換近似值得到的估計(jì)狀態(tài)精度相當(dāng)于二階擴(kuò)展卡爾曼的精度。

3 粒子濾波及其變形

3.1 粒子濾波

粒子濾波是基于蒙特卡羅仿真的遞歸貝葉斯算法，通過(guò)從后驗(yàn)概率密度中采樣對(duì)狀態(tài)進(jìn)行數(shù)值求解。粒子濾波將狀態(tài)矢量表示為一組帶有權(quán)值的隨機(jī)樣本，并且基于這些樣本和權(quán)值可以計(jì)算出狀態(tài)估值。該方法沒(méi)有線性模型與高斯噪聲的限制，因此可以處理用任何狀態(tài)模型描述的系統(tǒng)。但粒子濾波多次迭代后權(quán)值高度集中，導(dǎo)致了粒子退化，針對(duì)粒子退化的問(wèn)題，可以采用重采樣的方法來(lái)解決，重采樣過(guò)程主要有多項(xiàng)式重采樣，系統(tǒng)重采樣[9]，即復(fù)制權(quán)重大的粒子，并用這些粒子替代小權(quán)重粒子。粒子濾波的過(guò)程如表2所示。

3.2 擴(kuò)展卡爾曼粒子濾波

擴(kuò)展卡爾曼濾波是一種局部線性化的方法，它通過(guò)一階泰勒展開(kāi)式實(shí)現(xiàn)。由于使用了擴(kuò)展卡爾濾波，系統(tǒng)噪聲須滿(mǎn)足高斯分布。

擴(kuò)展卡爾曼結(jié)合粒子濾波思路如下：在采樣階段，利用擴(kuò)展卡爾曼濾波為每個(gè)粒子計(jì)算均值與協(xié)方差，然后利用均值與協(xié)方差信息進(jìn)行采樣。在擴(kuò)展卡爾曼濾波計(jì)算均值與方差的過(guò)程中利用到了最新時(shí)刻的觀測(cè)值數(shù)據(jù)，因此，相比基本的粒子濾波算法，估計(jì)的精度有了一定程度的提高。

3.3 無(wú)跡粒子濾波

無(wú)跡粒子濾波是一種遞歸的最小均方誤差估計(jì)。與基本的粒子濾波算法相比，無(wú)跡粒子濾波在計(jì)算均值與方差的過(guò)程中利用到了最新的觀測(cè)信息[Zk]，因此估計(jì)的精度要優(yōu)于粒子濾波算法。與擴(kuò)展卡爾曼粒子濾波相比，通過(guò)使用無(wú)跡變換，后驗(yàn)方差與均值的估計(jì)能夠精確到二階及以上，因此估計(jì)精度也要優(yōu)于擴(kuò)展卡爾曼粒子濾波。

4 仿真

本實(shí)驗(yàn)中所采用的非線性觀測(cè)模型與量測(cè)模型來(lái)自文獻(xiàn)[10]，如下所示：

圖2與圖3的跟蹤時(shí)長(zhǎng)設(shè)置為50，過(guò)程噪聲方差為3，觀測(cè)噪聲方差為1，采樣的粒子數(shù)目為100。由圖2可知，擴(kuò)展卡爾曼濾波（EKF）、無(wú)跡卡爾曼濾波（UKF）、粒子濾波（PF）、擴(kuò)展卡爾曼粒子濾波（EPF）、無(wú)跡粒子濾波（UPF）均有比較好的跟蹤精度，大部分時(shí)段的估計(jì)狀態(tài)與真實(shí)狀態(tài)相接近，EKF在3時(shí)刻有一個(gè)較大的誤差出現(xiàn)。由圖3可知，在上述幾種濾波方式中，UPF的誤差在整個(gè)跟蹤過(guò)程中都比較小，大部分時(shí)間都接近于0，說(shuō)明UPF的誤差穩(wěn)定性?xún)?yōu)于其他的濾波方式。EKF與PF的誤差起伏比較大，誤差的穩(wěn)定性不如其他的濾波方式，但是總體的誤差水平也比較小。通過(guò)對(duì)比圖2與圖3，可以得出EKF，UKF與PF，EPF，UPF在該模型下有著較好的跟蹤效果與較小的誤差。

5 結(jié)論

本文首先對(duì)卡爾曼濾波、粒子濾波及基于前者改進(jìn)的濾波算法進(jìn)行了理論介紹，然后通過(guò)仿真實(shí)驗(yàn)對(duì)比了擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波、粒子濾波、擴(kuò)展卡爾曼粒子濾波、無(wú)跡粒子濾波算法的跟蹤效果與誤差，得出在非線性高斯模型下卡爾曼濾波算法與粒子濾波算法均能達(dá)到比較好的估計(jì)效果，估計(jì)值與狀態(tài)真實(shí)值比較接近。下一步將研究這些濾波算法在具體場(chǎng)景中的應(yīng)用。

參考文獻(xiàn)：

[1] 蘇續(xù)軍，呂學(xué)志.基于貝葉斯推斷的復(fù)雜系統(tǒng)可靠性分析[J].計(jì)算機(jī)應(yīng)用與軟件，2019，36（3）：219-226，294.

[2] 黃小平，王巖，繆鵬程.粒子濾波原理及應(yīng)用：MATLAB仿真[M].北京：電子工業(yè)出版社，2017.

[3] 王法勝，魯明羽，趙清杰，等.粒子濾波算法[J].計(jì)算機(jī)學(xué)報(bào)，2014，37（8）：1679-1694.

[4] 喬少杰，韓楠，朱新文，等.基于卡爾曼濾波的動(dòng)態(tài)軌跡預(yù)測(cè)算法[J].電子學(xué)報(bào)，2018，46（2）：418-423.

[5] 孟慶旭.粒子濾波算法研究及其在非線性估計(jì)中的應(yīng)用[D].武漢：華中科技大學(xué)，2019.

[6] 董俊松.基于擴(kuò)展卡爾曼濾波的單目標(biāo)跟蹤算法研究[D].蘭州：西北師范大學(xué)，2019.

[7] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].2版.西安：西北工業(yè)大學(xué)出版社，2012.

[8] 周雪，張皓，王祝萍.擴(kuò)展卡爾曼濾波在受到惡意攻擊系統(tǒng)中的狀態(tài)估計(jì)[J].自動(dòng)化學(xué)報(bào)，2020，46（1）：38-46.

[9] 馮馳，呂曉鳳，汲清波.粒子濾波理論及其在目標(biāo)跟蹤中的應(yīng)用[J].計(jì)算機(jī)工程與應(yīng)用，2008，44（6）：246-248.

[10] 王航星，潘巍.基于自適應(yīng)吸引半徑的螢火蟲(chóng)算法的粒子濾波[J].計(jì)算機(jī)應(yīng)用研究，2019，36（12）：3632-3636.

【通聯(lián)編輯：梁書(shū)】