改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法

賀 姍，趙 旭，師 昕

(西安工程大學(xué) 計算機(jī)科學(xué)學(xué)院，陜西 西安 710048)

1 概 述

針對目標(biāo)跟蹤系統(tǒng)[1-3]中的狀態(tài)估計問題，卡爾曼濾波(Kalman filter,KF)[4]是一種線性最優(yōu)濾波算法。針對非線性高斯系統(tǒng)濾波問題，在實際應(yīng)用中通常采用擴(kuò)展卡爾曼濾波(extended Kalman filter,EKF)[5-6]，濾波過程通過對系統(tǒng)的非線性狀態(tài)方程或量測方程進(jìn)行泰勒級數(shù)展開，并截取展開式的前一階或者二階項，即可將系統(tǒng)非線性方程變換成線性方程，從而濾波過程可以使用卡爾曼濾波。擴(kuò)展卡爾曼濾波算法具有較高的運算速度，因此在解決非線性系統(tǒng)濾波問題時人們通常會采用該算法。然而，該算法在對非線性方程進(jìn)行線性化近似時存在截斷誤差，尤其是對于強(qiáng)非線性系統(tǒng)，濾波過程較易產(chǎn)生擴(kuò)散。

為使得非線性系統(tǒng)的濾波性能得到進(jìn)一步提高，Gordon等提出了粒子濾波算法(particle filter,PF)[7-8]。PF是一種貝葉斯最優(yōu)濾波和Monte Carlo隨機(jī)采樣方法相結(jié)合的統(tǒng)計濾波算法，適用于任意非線性、非高斯系統(tǒng)，且其濾波精度可以逼近最優(yōu)估計，是一種非常有效的非線性濾波算法，但是其主要缺點是計算時間復(fù)雜度會隨著狀態(tài)變量維數(shù)的增加而迅速增大。隨后，為了處理非線性濾波問題時能夠獲得較高的精度以及較快的計算速度，Julier等在不敏變換(unscented transformation,UT)的基礎(chǔ)上提出了不敏卡爾曼濾波(unscented Kalman filter,UKF)[9-10]。UT變換通過采樣策略選取一系列的西格瑪點集，并用這些西格瑪點集近似系統(tǒng)狀態(tài)分布的均值和方差，理論上其精度可以達(dá)到二階。同時，該算法不需要計算非線性函數(shù)的雅克比矩陣，因此也得到了廣泛的應(yīng)用。但是，當(dāng)系統(tǒng)的維數(shù)超過三階時，該算法的濾波精度會隨著系統(tǒng)維數(shù)的增加而降低。2007年，Arasaratnam等在高斯厄米特濾波的基礎(chǔ)上提出了一種新的非線性濾波算法-求積分卡爾曼濾波(quadrature Kalman filtering,QKF)[11-13]。該算法是利用高斯-厄米特積分準(zhǔn)則來計算非線性系統(tǒng)的概率密度，能夠取得較好的濾波精度。

然而，上述非線性濾波算法對于系統(tǒng)模型都具有嚴(yán)格的要求，系統(tǒng)模型的不確定性，包括建模不準(zhǔn)確、模型狀態(tài)簡化以及實際系統(tǒng)模型參數(shù)變動等，濾波過程如果直接采用標(biāo)準(zhǔn)的非線性濾波算法，都會使得系統(tǒng)的狀態(tài)估計值偏離真實狀態(tài)值，導(dǎo)致濾波過程發(fā)散，濾波精度降低。為了克服系統(tǒng)模型不確定的魯棒性，周東華等提出了強(qiáng)跟蹤濾波器(strong tracking filter,STF)[14-16]，該算法在應(yīng)對系統(tǒng)狀態(tài)突變問題時具有較好的跟蹤能力和抗干擾能力?；诖?，將強(qiáng)跟蹤濾波算法思想引入到非線性濾波算法中，從而得到了幾種相應(yīng)的強(qiáng)跟蹤非線性濾波算法，包括強(qiáng)跟蹤擴(kuò)展卡爾曼濾波算法(strong tracking extended Kalman filter,SEKF)[17]、強(qiáng)跟蹤不敏卡爾曼濾波算法(strong tracking unscented Kalman filter,SUKF)[18]、強(qiáng)跟蹤粒子濾波算法(strong tracking particle Kalman filter,SPF)[19]以及強(qiáng)跟蹤求積分卡爾曼濾波算法(strong tracking quadrature Kalman filter,SQKF)[20]等。

上述強(qiáng)跟蹤非線性濾波算法的主要思想是將漸消因子引入狀態(tài)預(yù)測協(xié)方差陣中，能夠在線實時調(diào)整增益矩陣，從而強(qiáng)迫當(dāng)前時刻輸出的殘差序列能夠保持正交，那么在系統(tǒng)模型不確定的情況下，上述算法均能保持對系統(tǒng)狀態(tài)的良好跟蹤能力。但是由于強(qiáng)跟蹤濾波算法實現(xiàn)過程中對判斷濾波發(fā)散的閾值設(shè)置較小，即使在系統(tǒng)正常情況下也會產(chǎn)生漸消因子，導(dǎo)致過度調(diào)節(jié)濾波增益，造成系統(tǒng)狀態(tài)估計不夠平滑，濾波精度降低。針對上述問題，文獻(xiàn)[21]對強(qiáng)跟蹤濾波算法進(jìn)行了改進(jìn)，提出了一種改進(jìn)的強(qiáng)跟蹤濾波算法(improved strong tracking Kalman filter,ISTF)。該算法的主要思想是在濾波過程中通過增大判斷濾波發(fā)散的閾值，從而有效降低了誤判濾波發(fā)散的概率，提高了濾波器對于系統(tǒng)狀態(tài)的跟蹤性能，獲得了更高的濾波精度。

為了能夠改善強(qiáng)跟蹤求積分卡爾曼濾波算法的濾波性能，借鑒文獻(xiàn)[21]的思想，文中提出了一種改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法(improved strong tracking quadrature Kalman filter,ISQKF)，并進(jìn)行了仿真驗證。

2 強(qiáng)跟蹤求積分卡爾曼濾波

考慮如下的非線性系統(tǒng)模型，狀態(tài)方程和量測方程分別為：

xk=f(xk-1,uk-1)+wk-1

(1)

zk=h(xk)+vk

(2)

其中，xk∈n表示狀態(tài)變量；uk∈n表示控制變量；zk∈n表示觀測向量；wk-1和vk分別為互不相關(guān)的零均值過程噪聲和量測噪聲，其方差分別為Qk和Rk。

強(qiáng)跟蹤求積分卡爾曼濾波算法的主要思想是在狀態(tài)預(yù)測協(xié)方差陣中引入漸消因子λ(0≤λ≤1)，在線實時調(diào)整增益矩陣Wk，強(qiáng)行使輸出的殘差序列保持正交，最大限度地提取殘差序列中的一切有效信息，從而提高濾波器的跟蹤性能。當(dāng)系統(tǒng)模型和真實系統(tǒng)匹配度較高時，漸消因子λ趨近于1，此時，強(qiáng)跟蹤求積分卡爾曼濾波算法將會退化為標(biāo)準(zhǔn)求積分卡爾曼濾波算法，不會影響到系統(tǒng)狀態(tài)估計的結(jié)果。而當(dāng)系統(tǒng)模型不確定時，λ的取值將趨近于0，此時，強(qiáng)跟蹤算法能夠削弱前一刻的數(shù)據(jù)對于當(dāng)前時刻濾波結(jié)果的影響，從而減弱系統(tǒng)模型不確定所帶來的影響，這樣強(qiáng)跟蹤求積分卡爾曼濾波算法在模型不確定時仍然能夠具有較強(qiáng)的跟蹤性能。

強(qiáng)跟蹤求積分卡爾曼濾波算法具體的實現(xiàn)步驟如下：

狀態(tài)更新：

(3)

(4)

(5)

(6)

(7)

量測更新：

(8)

(9)

Zl,k|k-1=h(Xl,k|k-1,uk,k)

(10)

(11)

(12)

(13)

(14)

(15)

(16)

其中

(17)

(18)

其中

(19)

(20)

(21)

3 改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法

針對非線性濾波問題，強(qiáng)跟蹤求積分卡爾曼濾波算法主要思想是將漸消因子λk-1引入到狀態(tài)預(yù)測協(xié)方差陣Pk|k-1中，在線實時調(diào)整增益矩陣Wk，從而使輸出殘差序列保持正交，這樣強(qiáng)跟蹤求積分卡爾曼濾波算法在應(yīng)對模型不確定時仍然能夠保持對系統(tǒng)狀態(tài)的良好跟蹤性能。然而，在實際應(yīng)用中發(fā)現(xiàn)，強(qiáng)跟蹤求積分卡爾曼濾波算法在應(yīng)對系統(tǒng)狀態(tài)突變問題時會出現(xiàn)濾波精度降低的問題，導(dǎo)致濾波精度降低的主要原因是由于在求解V0,k時得不到解析解，而只能通過參差序列獲得其近似值，從而導(dǎo)致其近似值與理論值出現(xiàn)了偏差，使得對系統(tǒng)狀態(tài)估計不夠平滑。強(qiáng)跟蹤求積分卡爾曼濾波算法問題分析如下：

k時刻V0,k的理論值C0,k可通過下式求得：

(22)

V0,k的近似值可通過式21求得：

(23)

其中，yk～N(0,C0,k)，那么k時刻的殘差yk可以認(rèn)為是對高斯分布N(yk；0,C0,k)的一次采樣，那么可得到：

(24)

相應(yīng)濾波發(fā)散的概率為：

1-84.1%≈16%

(25)

為了解決上述問題，提出了一種新的濾波算法即改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法。該算法的主要思想是在算法實現(xiàn)過程中適當(dāng)增大判斷濾波發(fā)散的閾值，從而有效降低了誤判濾波發(fā)散的概率，大大減小了產(chǎn)生漸消因子的概率，并能夠根據(jù)不同維數(shù)的量測方程確定弱化因子的取值。

由于

(26)

則

(27)

因此，可將判定濾波發(fā)散的閾值提高到3C0,k，則

(28)

即使考慮V0,k的影響，由于V0,k

(29)

因此，當(dāng)把V0,k的值取到3C0,k時，濾波發(fā)散的概率僅為1.5%～2.6%，新算法濾波發(fā)散的概率遠(yuǎn)遠(yuǎn)小于強(qiáng)跟蹤求積分卡爾曼濾波算法濾波發(fā)散的概率。

那么，當(dāng)V0,k=3C0,k時，則有：

(30)

由上式可得：

(31)

那么改進(jìn)的強(qiáng)跟蹤濾波算法的漸消因子即為：

(32)

(33)

其中

(34)

(35)

上述改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法的推導(dǎo)是針對一維系統(tǒng)，若要推廣到多維系統(tǒng)中，那么判斷濾波發(fā)散的條件為：

(36)

假設(shè)V0,k和C0,k兩者相等，上式可改寫為：

Tr(V0,k)≥Tr(l'C0,k)

(37)

(38)

其中，n為量測方程的維數(shù)。

(39)

那么，針對非線性濾波問題，當(dāng)采用文中提出的ISQKF時，即可通過式39確定弱化因子的取值，從而避免了憑經(jīng)驗選取弱化因子的問題。

4 仿真實驗及結(jié)果分析

單變量非穩(wěn)定增長模型(univariate nonstationary growth model,UNGM)如下：

(40)

(41)

其中，α=0.5，β=25，γ=8。

設(shè)該系統(tǒng)的初始值分別為：x0=0.1，P0=0.1以及N=100。根據(jù)式39可知弱化因子l'=3。

由式40和式41可知該系統(tǒng)為非線性系統(tǒng)，濾波過程中分別采用SQKF以及提出的ISQKF進(jìn)行濾波估計。

實驗仿真結(jié)果如圖1和圖2所示。

圖1 SQKF與ISQKF狀態(tài)估計結(jié)果對比

圖2 SQKF與ISQKF估計誤差對比

由圖1可知，經(jīng)過ISQKF濾波后的狀態(tài)估計值與SQKF濾波后的狀態(tài)估計值相比更加趨近系統(tǒng)真實狀態(tài)值。由圖2可知，采用ISQKF濾波后的濾波誤差明顯小于SQKF濾波后的濾波誤差。綜上所述，針對系統(tǒng)不確定問題，提出的ISQKF與原有的SQKF相比，具有更高的濾波精度，狀態(tài)估計值更加接近于系統(tǒng)真實值。

5 結(jié)束語

對于非線性高斯系統(tǒng)，強(qiáng)跟蹤求積分卡爾曼濾波算法在實現(xiàn)的過程中由于存在對判斷濾波發(fā)散的閾值設(shè)置較小的問題，從而使得誤判濾波發(fā)散的概率增大，進(jìn)而增大了產(chǎn)生漸消因子的概率，造成過度調(diào)整濾波增益，最終導(dǎo)致對系統(tǒng)狀態(tài)估計出現(xiàn)較大偏差。為了解決上述問題，提出了一種改進(jìn)的強(qiáng)跟蹤求積分卡爾曼濾波算法。該算法主要通過增大判斷濾波發(fā)散的閾值，從而有效降低了誤判濾波發(fā)散的概率，增強(qiáng)了濾波器對系統(tǒng)狀態(tài)的跟蹤性能，獲得了更高的濾波精度，并能夠根據(jù)不同維數(shù)的量測方程確定弱化因子的取值，從而避免了憑經(jīng)驗選取弱化因子，具有較強(qiáng)的操作性。對兩種算法進(jìn)行實驗仿真，結(jié)果表明，改進(jìn)算法具有更高的濾波精度，減小了系統(tǒng)狀態(tài)估計值與真實值之間的偏差。