改進的強跟蹤求積分卡爾曼濾波算法

賀 姍，趙 旭，師 昕

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

1 概 述

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

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

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

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

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

2 強跟蹤求積分卡爾曼濾波

考慮如下的非線性系統模型，狀態方程和量測方程分別為：

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

(1)

zk=h(xk)+vk

(2)

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

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

強跟蹤求積分卡爾曼濾波算法具體的實現步驟如下：

狀態更新：

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

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

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

(22)

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

(23)

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

(24)

相應濾波發散的概率為：

1-84.1%≈16%

(25)

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

由于

(26)

則

(27)

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

(28)

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

(29)

因此，當把V0,k的值取到3C0,k時，濾波發散的概率僅為1.5%～2.6%，新算法濾波發散的概率遠遠小于強跟蹤求積分卡爾曼濾波算法濾波發散的概率。

那么，當V0,k=3C0,k時，則有：

(30)

由上式可得：

(31)

那么改進的強跟蹤濾波算法的漸消因子即為：

(32)

(33)

其中

(34)

(35)

上述改進的強跟蹤求積分卡爾曼濾波算法的推導是針對一維系統，若要推廣到多維系統中，那么判斷濾波發散的條件為：

(36)

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

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

(37)

(38)

其中，n為量測方程的維數。

(39)

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

4 仿真實驗及結果分析

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

(40)

(41)

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

設該系統的初始值分別為：x0=0.1，P0=0.1以及N=100。根據式39可知弱化因子l'=3。

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

實驗仿真結果如圖1和圖2所示。

圖1 SQKF與ISQKF狀態估計結果對比

圖2 SQKF與ISQKF估計誤差對比

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

5 結束語

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