趙 辰

(1.石家莊鐵道大學(xué) 電氣與電子工程學(xué)院，河北 石家莊 050043；2.河北省交通安全與控制重點(diǎn)實(shí)驗(yàn)室，河北 石家莊 050043)

0 引言

濾波是指將有用信號(hào)中的噪聲進(jìn)行濾除的一種方法。生活中的系統(tǒng)基本都是非線(xiàn)性的，針對(duì)非線(xiàn)性系統(tǒng)的濾波方法主要有EKF、UKF和CKF。EKF是將非線(xiàn)性函數(shù)展開(kāi)成泰勒級(jí)數(shù)并略去高階項(xiàng)，一般適用于弱非線(xiàn)性系統(tǒng)。EKF相對(duì)UKF、CKF較為簡(jiǎn)單且容易實(shí)現(xiàn)，但在強(qiáng)非線(xiàn)性情況下濾波性能差，且計(jì)算量相比UKF和CKF大，有時(shí)會(huì)導(dǎo)致濾波發(fā)散[1]。UKF則用UT變換對(duì)均值和協(xié)方差的非線(xiàn)性傳遞進(jìn)行處理，因此其精度高于EKF。UKF廣泛應(yīng)用于目標(biāo)跟蹤、汽車(chē)行駛狀態(tài)估計(jì)、導(dǎo)航系統(tǒng)和飛行器飛行姿態(tài)估計(jì)等領(lǐng)域。UKF雖然克服了EKF易發(fā)散的缺點(diǎn)，但在高維系統(tǒng)中UKF需要合理地調(diào)節(jié)參數(shù)才能得到好的濾波效果[2]，應(yīng)用比較困難。

最近，基于Cubature變換的CKF濾波算法由Arasaratnam et al提出[3-4]，該算法在狀態(tài)估計(jì)、機(jī)動(dòng)目標(biāo)定位等領(lǐng)域得到廣泛的應(yīng)用。CKF應(yīng)用球面徑向準(zhǔn)則對(duì)狀態(tài)后驗(yàn)均值和協(xié)方差進(jìn)行逼近，并通過(guò)2n個(gè)相同權(quán)值的Cubature點(diǎn)經(jīng)系統(tǒng)方程得到新的狀態(tài)點(diǎn)進(jìn)而推導(dǎo)出下一時(shí)刻狀態(tài)的預(yù)測(cè)點(diǎn)，整個(gè)過(guò)程不需對(duì)復(fù)雜的Jacobi矩陣進(jìn)行計(jì)算，具有很好的濾波效果[5]。針對(duì)目前的相關(guān)文獻(xiàn)，大多數(shù)是基于EKF與UKF濾波算法的應(yīng)用對(duì)比研究，以及UKF與CKF濾波算法的應(yīng)用對(duì)比研究，而基于3種濾波算法濾波性能對(duì)比研究的文獻(xiàn)尚不存在。因此本文在相同的非線(xiàn)性系統(tǒng)模型下，對(duì)比分析了EKF、UKF和CKF濾波算法的實(shí)現(xiàn)原理以及各自估計(jì)值與真實(shí)值的誤差，綜合比較了3種算法的濾波性能。

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

EKF的離散非線(xiàn)性系統(tǒng)狀態(tài)和觀測(cè)方程分別表示為

(1)

現(xiàn)在假定控制量的輸入為0，已知噪聲驅(qū)動(dòng)矩陣G(k)，過(guò)程噪聲W(k)和觀測(cè)噪聲V(k)相互獨(dú)立。過(guò)程噪聲W(k)、觀測(cè)噪聲V(k)都是均值為0的高斯白噪聲。

(2)

令

(3)

(4)

則狀態(tài)方程為

X(k+1)=Φ(k+1|k)X(k)+G(k)W(k)+Φ(k)

(5)

式中，初始值為X(0)=E[X(0)]。

(6)

令

(7)

(8)

則觀測(cè)方程為

Z(k)=H(k)X(k)+y(k)+V(k)

(9)

將卡爾曼濾波方程應(yīng)用到非線(xiàn)性系統(tǒng)的狀態(tài)方程(5)和觀測(cè)方程(9)可得EKF濾波遞推方程

(10)

P(k+1|k)=Φ(k+1|k)P(k|k)ΦT(k+1|k)+Q(k+1)

(11)

K(k+1)=P(k+1)HT(k+1)[H(k+1)P(k+1|k)HT(k+1)+R(k+1)]-1

(12)

(13)

P(k+1)=[I-K(k+1)H(k+1)]P(k+1|k)

(14)

式中，濾波的初始狀態(tài)值X(0)和誤差的初始協(xié)方差陣P(0)初始值分別為：E[X(0)]、var[X(0)]。

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

UKF與EKF不同的是，它并不是線(xiàn)性化處理估計(jì)點(diǎn)附近的非線(xiàn)性方程f和h，而是由UT變換確定的采樣點(diǎn)來(lái)表示非線(xiàn)性函數(shù)的高斯分布，進(jìn)而近似系統(tǒng)狀態(tài)的概率密度以達(dá)到狀態(tài)估計(jì)的目的[7]。

2.1 UT變換的原理

UT變換的原理為：UT變換用一個(gè)確定數(shù)量的參數(shù)集來(lái)近似非線(xiàn)性系統(tǒng)的高斯分布，并在原來(lái)狀態(tài)的分布中按能夠達(dá)到現(xiàn)在狀態(tài)分布和原來(lái)狀態(tài)分布的均值和協(xié)方差相等的規(guī)則選取部分采樣點(diǎn)，再在非線(xiàn)性的函數(shù)中代入上述采樣點(diǎn)求出相應(yīng)的點(diǎn)集，均值和誤差協(xié)方差就可以通過(guò)求出的點(diǎn)集得到，這樣求得的均值和協(xié)方差具有2階及以上精度[8]。

(1)求解2n+1個(gè)Sigma點(diǎn)，即采樣點(diǎn)。

(15)

(2)計(jì)算上述采樣點(diǎn)對(duì)應(yīng)的權(quán)值。

(16)

式中，上標(biāo)表示第幾個(gè)采樣點(diǎn)；m為平均值；c為協(xié)方差；a為控制采樣點(diǎn)的狀態(tài)；λ=a2(n+κ)-n是比例參數(shù)，可以降低預(yù)測(cè)誤差，κ為待選參數(shù)，它的取值不受界限的限制，并能保證(n+λ)P為半正定矩陣；β≥0是權(quán)系數(shù)，它可以合并方程中的高階項(xiàng)進(jìn)而將高階項(xiàng)的影響考慮在內(nèi)。

2.2 UKF算法實(shí)現(xiàn)

對(duì)于不同時(shí)刻k，非線(xiàn)性系統(tǒng)可由具有W(k)的隨機(jī)變量X和具有V(k)的觀測(cè)變量Z構(gòu)成并表示為

(17)

式中，f是非線(xiàn)性狀態(tài)方程函數(shù)；h是非線(xiàn)性觀測(cè)方程函數(shù)。

設(shè)W(k)含有協(xié)方差陣Q，V(k)含有協(xié)方差陣R，不同時(shí)刻k隨機(jī)變量X的UKF濾波步驟如下：

(1)利用式(16)、式(17)取得一組采樣點(diǎn)及采樣點(diǎn)的權(quán)值

(18)

(2)一步預(yù)測(cè)2n+1個(gè)Sigma點(diǎn)集的結(jié)果為

X(i)(k+1|k)=f[k,X(i)(k|k)]i=1,2,…,2n+1

(19)

(3)計(jì)算非線(xiàn)性系統(tǒng)狀態(tài)一步預(yù)測(cè)的值及協(xié)方差陣，其中權(quán)值w(i)通過(guò)式(16)得到。這不同于普通卡爾曼濾波算法，普通卡爾曼濾波算法只需通過(guò)上一時(shí)刻的狀態(tài)代入狀態(tài)方程，計(jì)算一次就可以得到狀態(tài)的預(yù)測(cè)；而UKF是利用一組Sigma的預(yù)測(cè)值，并對(duì)它們進(jìn)行加權(quán)求平均值，進(jìn)而得到系統(tǒng)狀態(tài)量的一步預(yù)測(cè)

(20)

(21)

(4)根據(jù)式(3)中一步預(yù)測(cè)結(jié)果的值，進(jìn)行相同的變換得到新Sigma點(diǎn)集

(22)

(5)將上一步驟的新Sigma點(diǎn)集代入式(1)中，得到觀測(cè)量

Z(i)(k+1|k)=h[X(i)(k+1|k)]i=1,2,…,2n+1

(23)

(6)通過(guò)步驟(5)的sigma點(diǎn)集的觀測(cè)值計(jì)算得到觀測(cè)后的均值和協(xié)方差

(24)

(25)

(26)

(7)計(jì)算卡爾曼增益

(27)

(8)最后狀態(tài)更新和協(xié)方差更新的結(jié)果為

(28)

P(k+1|k+1)=P(k+1|k)-K(k+1)PzkzkKT(k+1)

(29)

由上可得，UKF在處理非線(xiàn)性濾波時(shí)能以2階或2階以上泰勒精度逼近后驗(yàn)均值和協(xié)方差，而EKF達(dá)不到二階以上精度，因此UKF相比EKF具有更高的濾波精度。

3 容積卡爾曼濾波

CKF是一種新的用于非線(xiàn)性系統(tǒng)濾波的方法，可以高效地逼近高斯概率密度函數(shù)而成為高精度數(shù)值的卡爾曼濾波器。CKF基于Cubature規(guī)則在笛卡兒坐標(biāo)系下通過(guò)高斯加權(quán)的多維函數(shù)積分計(jì)算轉(zhuǎn)化為單個(gè)多維的幾何體容積計(jì)算，可以得到較高數(shù)值精度的計(jì)算結(jié)果，因此進(jìn)行非線(xiàn)性濾波變換后的精度優(yōu)于EKF、UKF[9]。CKF依靠確定的Cubature點(diǎn)來(lái)計(jì)算后驗(yàn)概率密度函數(shù)[10-11]，相比EKF有較好的可實(shí)現(xiàn)性，同時(shí)也避免了截?cái)嗾`差。與UKF相比,CKF的Cubature點(diǎn)權(quán)值大小相等且對(duì)稱(chēng)出現(xiàn),因此并不需要像UKF那樣提前設(shè)置參數(shù),在選擇方式上更加簡(jiǎn)捷,計(jì)算速度更快。

(30)

式中，ei∈Rn表示第i個(gè)元素為1的單位向量。

(31)

(32)

3.1 狀態(tài)更新

(1)基于狀態(tài)估計(jì)的采樣點(diǎn)的計(jì)算。

(33)

(34)

(2)容積點(diǎn)Xi,k|k-1的預(yù)測(cè)。

Xi,k|k-1=f(ξi,k-1),i=1,2,…,L

(35)

(36)

(4)狀態(tài)預(yù)測(cè)誤差Pk|k-1。

(37)

3.2 量測(cè)更新

(1)基于狀態(tài)預(yù)測(cè)的采樣點(diǎn)的計(jì)算。

(38)

(3)新息的協(xié)方差Pzz,k|k-1。

(39)

(4)狀態(tài)與量測(cè)向量之間的互協(xié)方差矩陣Pxz,k|k-1。

(40)

(5)卡爾曼增益矩陣Gk。

(41)

(42)

(7)狀態(tài)估計(jì)協(xié)方差矩陣Pk。

(43)

同樣可以看出，CKF也可以2階或2階以上Taylor精度逼近非線(xiàn)性系統(tǒng)的后驗(yàn)均值和協(xié)方差，由此判斷CKF估計(jì)精度也高于EKF。從式(33)可以看出，與UKF相比，CKF在非線(xiàn)性濾波過(guò)程中不需要任何參數(shù)，且始終為正的權(quán)值Wi，適合強(qiáng)非線(xiàn)性系統(tǒng)的估計(jì)，這就保證了濾波器可靠地運(yùn)行。因此CKF較UKF來(lái)看，可以在非線(xiàn)性系統(tǒng)都保持更高的數(shù)值穩(wěn)定性，有效克服了UKF在非線(xiàn)性系統(tǒng)中可能會(huì)濾波發(fā)散的缺點(diǎn)[13]。

4 仿真對(duì)比分析

在Matlab中進(jìn)行實(shí)驗(yàn)，選擇過(guò)程狀態(tài)協(xié)方差陣Q=10，測(cè)量噪聲協(xié)方差陣R=1，原始狀態(tài)陣X=1，原始估計(jì)方差陣p=1 000，樣本數(shù)目n=100。本模型為非線(xiàn)性模型，其離散方程為

(44)

各個(gè)算法的估計(jì)值對(duì)比圖、估計(jì)值誤差對(duì)比圖和10次仿真結(jié)果估計(jì)值誤差表分別如圖1、圖2和表1所示。

圖1 3種算法估計(jì)值對(duì)比圖

圖2 3種算法估計(jì)值誤差對(duì)比圖

表1 10次仿真結(jié)果估計(jì)值誤差

根據(jù)圖1、圖2和表1歸納得出如下結(jié)論。

(1)EKF具有較快的計(jì)算速度，但它的估計(jì)值偏離真實(shí)值的程度較大，尤其在樣本數(shù)目為50，72時(shí)刻左右偏離的較明顯，估計(jì)值誤差明顯最大。

(2)UKF較EKF的估計(jì)值離真實(shí)值更接近一些，這是因?yàn)閁KF基于UT變換進(jìn)行確定性采樣，且用一系列指定的樣本來(lái)對(duì)系統(tǒng)狀態(tài)的后驗(yàn)概率密度進(jìn)行逼近，濾波效果優(yōu)于EKF。但在樣本數(shù)目為3，49，74時(shí)刻左右偏離真實(shí)值較明顯，估計(jì)值誤差較大。

(3)CKF較EKF、UKF的估計(jì)值更加接近真實(shí)值，估計(jì)值誤差為3種算法最小，因此它的估計(jì)精度最高。因?yàn)镃KF在狀態(tài)估計(jì)過(guò)程中，不需要任何的參數(shù)，權(quán)值為正，從而保證了濾波器可以穩(wěn)定地運(yùn)行。

5 結(jié)論

通過(guò)EKF、UKF和CKF的對(duì)比可以發(fā)現(xiàn)，EKF是在普通卡爾曼濾波的基礎(chǔ)上通過(guò)對(duì)非線(xiàn)性函數(shù)方程的近似線(xiàn)性化處理進(jìn)而得到一個(gè)線(xiàn)性化的模型來(lái)進(jìn)行狀態(tài)估計(jì)；而UKF通過(guò)一個(gè)高斯分布對(duì)非線(xiàn)性方程的概率密度進(jìn)行近似，再用一些指定的Sigma點(diǎn)投影到觀測(cè)方程中得到一個(gè)新的高斯分布去近似之前的高斯分布進(jìn)而估計(jì)系統(tǒng)的狀態(tài)。在計(jì)算速度方面，EKF較快，但濾波性能最差。UKF雖能很好地解決這個(gè)問(wèn)題，但假如系統(tǒng)狀態(tài)為非高斯環(huán)境，就極易產(chǎn)生較大的誤差。CKF因采用Cubature規(guī)則無(wú)需對(duì)非線(xiàn)性系統(tǒng)進(jìn)行線(xiàn)性化處理，且無(wú)論狀態(tài)維數(shù)大小和參數(shù)的選擇，都能保證非線(xiàn)性系統(tǒng)的有效濾波估計(jì)，對(duì)比分析可知CKF的估計(jì)值最接近真實(shí)值，且估計(jì)值誤差最小。