趙 辰

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

0 引言

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

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

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

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

(1)

現(xiàn)在假定控制量的輸入為0，已知噪聲驅(qū)動矩陣G(k)，過程噪聲W(k)和觀測噪聲V(k)相互獨立。過程噪聲W(k)、觀測噪聲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)

則觀測方程為

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

(9)

將卡爾曼濾波方程應(yīng)用到非線性系統(tǒng)的狀態(tài)方程(5)和觀測方程(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 無跡卡爾曼濾波

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

2.1 UT變換的原理

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

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

(15)

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

(16)

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

2.2 UKF算法實現(xiàn)

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

(17)

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

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

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

(18)

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

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

(19)

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

(20)

(21)

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

(22)

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

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

(23)

(6)通過步驟(5)的sigma點集的觀測值計算得到觀測后的均值和協(xié)方差

(24)

(25)

(26)

(7)計算卡爾曼增益

(27)

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

(28)

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

(29)

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

3 容積卡爾曼濾波

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

(30)

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

(31)

(32)

3.1 狀態(tài)更新

(1)基于狀態(tài)估計的采樣點的計算。

(33)

(34)

(2)容積點Xi,k|k-1的預(yù)測。

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

(35)

(36)

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

(37)

3.2 量測更新

(1)基于狀態(tài)預(yù)測的采樣點的計算。

(38)

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

(39)

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

(40)

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

(41)

(42)

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

(43)

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

4 仿真對比分析

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

(44)

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

圖1 3種算法估計值對比圖

圖2 3種算法估計值誤差對比圖

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

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

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

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

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

5 結(jié)論

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