賀 姍，趙 旭，師 昕

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

1 概 述

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

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

然而，上述非線性濾波算法對(duì)于系統(tǒng)模型都具有嚴(yán)格的要求，系統(tǒng)模型的不確定性，包括建模不準(zhǔn)確、模型狀態(tài)簡(jiǎn)化以及實(shí)際系統(tǒng)模型參數(shù)變動(dòng)等，濾波過(guò)程如果直接采用標(biāo)準(zhǔn)的非線性濾波算法，都會(huì)使得系統(tǒng)的狀態(tài)估計(jì)值偏離真實(shí)狀態(tài)值，導(dǎo)致濾波過(guò)程發(fā)散，濾波精度降低。為了克服系統(tǒng)模型不確定的魯棒性，周東華等提出了強(qiáng)跟蹤濾波器(strong tracking filter,STF)[14-16]，該算法在應(yīng)對(duì)系統(tǒng)狀態(tài)突變問(wèn)題時(shí)具有較好的跟蹤能力和抗干擾能力。基于此，將強(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ù)測(cè)協(xié)方差陣中，能夠在線實(shí)時(shí)調(diào)整增益矩陣，從而強(qiáng)迫當(dāng)前時(shí)刻輸出的殘差序列能夠保持正交，那么在系統(tǒng)模型不確定的情況下，上述算法均能保持對(duì)系統(tǒng)狀態(tài)的良好跟蹤能力。但是由于強(qiáng)跟蹤濾波算法實(shí)現(xiàn)過(guò)程中對(duì)判斷濾波發(fā)散的閾值設(shè)置較小，即使在系統(tǒng)正常情況下也會(huì)產(chǎn)生漸消因子，導(dǎo)致過(guò)度調(diào)節(jié)濾波增益，造成系統(tǒng)狀態(tài)估計(jì)不夠平滑，濾波精度降低。針對(duì)上述問(wèn)題，文獻(xiàn)[21]對(duì)強(qiáng)跟蹤濾波算法進(jìn)行了改進(jìn)，提出了一種改進(jìn)的強(qiáng)跟蹤濾波算法(improved strong tracking Kalman filter,ISTF)。該算法的主要思想是在濾波過(guò)程中通過(guò)增大判斷濾波發(fā)散的閾值，從而有效降低了誤判濾波發(fā)散的概率，提高了濾波器對(duì)于系統(tǒng)狀態(tài)的跟蹤性能，獲得了更高的濾波精度。

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

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

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

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

(1)

zk=h(xk)+vk

(2)

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

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

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

狀態(tài)更新：

(3)

(4)

(5)

(6)

(7)

量測(cè)更新：

(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)跟蹤求積分卡爾曼濾波算法

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

k時(shí)刻V0,k的理論值C0,k可通過(guò)下式求得：

(22)

V0,k的近似值可通過(guò)式21求得：

(23)

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

(24)

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

1-84.1%≈16%

(25)

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

由于

(26)

則

(27)

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

(28)

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

(29)

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

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

(30)

由上式可得：

(31)

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

(32)

(33)

其中

(34)

(35)

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

(36)

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

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

(37)

(38)

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

(39)

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

4 仿真實(shí)驗(yàn)及結(jié)果分析

單變量非穩(wěn)定增長(zhǎng)模型(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)，濾波過(guò)程中分別采用SQKF以及提出的ISQKF進(jìn)行濾波估計(jì)。

實(shí)驗(yàn)仿真結(jié)果如圖1和圖2所示。

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

圖2 SQKF與ISQKF估計(jì)誤差對(duì)比

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

5 結(jié)束語(yǔ)

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