自適應平方根球型無跡卡爾曼濾波算法

， ， ， ，

(1.空軍預警學院研究生大隊， 湖北武漢 430019;2.空軍預警學院， 湖北武漢 430019)

0 引言

目標跟蹤是雷達數(shù)據(jù)處理的關鍵部分，然而很多情況下，觀測數(shù)據(jù)和目標動態(tài)參數(shù)間的關系是非線性的。對于線性系統(tǒng)或非常接近線性的系統(tǒng)，一般能用線性的濾波方法給出滿意的結果。然而，對于非線性的系統(tǒng)，用線性的估計方法往往得不到好的濾波效果，這時需要非線性的濾波方法。

目前，普遍應用的非線性濾波方法有擴展卡爾曼濾波(EKF)以及無跡卡爾曼濾波(UKF)。EKF是利用一階泰勒展開的思想，將非線性系統(tǒng)近似線性化。因此，對于強非線性系統(tǒng)，EKF存在濾波性能極不穩(wěn)定，甚至發(fā)散的問題。并且還要計算Jacobian矩陣及其冪，這在許多實際問題中很難求得[1]。而UKF是采用Unscented變換(UT)，通過選取一組精確的sigma點來匹配非線性高斯系統(tǒng)狀態(tài)的統(tǒng)計特性[2]，進行非線性模型的狀態(tài)與誤差協(xié)方差的遞推和更新。其不需要計算Jacobian矩陣，在相當運算量下具有估計精度和魯棒性更高等優(yōu)點[3]。然而，UKF需要用到對協(xié)方差矩陣的Cholesky分解，其要求協(xié)方差矩陣必須為對稱正定矩陣。在濾波計算中隨著迭代計算的累加，積累的舍入誤差往往會破壞系統(tǒng)估計誤差協(xié)方差矩陣的非負正定和對稱性，導致濾波算法不穩(wěn)定，甚至發(fā)生異常[1]。對于此，國內(nèi)外的專家學者提出了許多改進算法。

文獻[4]借助平方根卡爾曼濾波的思想，提出了一種平方根 UKF(Square-Root Unscented Kalman Filtering，SR-UKF)濾波方法，利用協(xié)方差平方根代替協(xié)方差參加遞推運算，從而解決了由于協(xié)方差矩陣負定而不能求其平方根的問題，同時還提高了算法的計算精度。文獻[5]提出的自適應平方根無跡卡爾曼濾波方法通過對傳統(tǒng)的Sage-Husa自適應濾波算法進行改進，并與SRUKF算法相結合，能直接對非線性系統(tǒng)的狀態(tài)方差陣和噪聲方差陣的平方根進行遞推與估算，確保狀態(tài)和噪聲方差陣的對稱性和非負定性。文獻[6]基于強跟蹤濾波理論在改進的平方根UKF基礎上引入多重自適應衰減因子調(diào)節(jié)協(xié)方差矩陣，提出了改進的強跟蹤平方根UKF，使得濾波器具有自適應跟蹤能力和克服系統(tǒng)模型不確定的魯棒性。

本文在標準的平方根UKF算法上，通過改用球型無跡變換對權系數(shù)以及sigma點進行選取，改進平方根UKF中平方根矩陣的分解方法以及在預測誤差協(xié)方差矩陣中引入了自適應衰減因子，設計了自適應平方根球型無跡卡爾曼濾波算法(ASRS-UKF)。將該算法同SR-UKF算法以及強跟蹤UKF算法(Strong Tracking Square Root UKF，STSR-UKF)進行仿真對比，仿真結果證明了ASRS-UKF算法的有效性。

1 ASRS-UKF濾波算法

1.1 標準的平方根UKF濾波算法

設非線性系統(tǒng)的狀態(tài)方程和量測方程：

(1)

式中：X為n×1維狀態(tài)變量，f(X)為系統(tǒng)的非線性狀態(tài)方程，Z為m×1維觀測向量，h(X)為非線性的測量方程；wk為過程噪聲，Vk為測量噪聲，且wk及Vk均為均值為零協(xié)方差矩陣分別為Qk和Rk的高斯白噪聲。

(2)

(3)

式中，chol為標準Matlab指令，表示Cholesky分解。

2) 計算選取sigma點：

(4)

3) 時間更新：

(5)

(6)

(7)

(8)

式中，qr和cholupdate為標準的Matlab指令，分別表示QR分解和Cholesky分解一階更新。

4) 量測更新：

(9)

(10)

(11)

(12)

(13)

5) 濾波結果更新:

(14)

(15)

U=KkSZ

(16)

Sk+1|k+1=cholupdate{Sk+1|k,U,-1}

(17)

其中，上述運用的權值計算表達式如下：

(18)

式中，β≥0為一個與狀態(tài)的先驗分布信息有關的參數(shù)，調(diào)節(jié)β可以提高協(xié)方差矩陣的精度。對于高斯分布來說，β=2是最優(yōu)的。

1.2 算法的改進

1.2.1 球型無跡變換選取權系數(shù)以及sigma點

標準的平方根UKF算法中的sigma點和權系數(shù)分別由式(4)和式(18)來計算選取。系統(tǒng)、權值等參數(shù)種類多，需要精心調(diào)節(jié)各參數(shù)才能獲得良好的濾波效果，調(diào)節(jié)過程較為繁瑣，調(diào)節(jié)值還需要依靠部分經(jīng)驗確定；而且，其在無跡變換中，對于n維的狀態(tài)向量需要2n個sigma點，這對于一般應用來說計算量太大。而球型無跡變換則只需要(n+2)個sigma點就能達到一般無跡變換的精度和數(shù)值穩(wěn)定度，其對于權系數(shù)的選擇也比較容易確定。

1) 權系數(shù)的選擇：

W0∈[0,1)

(19)

(20)

式中，Wi(i=0,1,2,…,n+1)表示均值以及協(xié)方差加權的權值，而且W0一般取為0。

2) 計算sigma點，平方根形式的球型無跡變換sigma點計算如下：

(21)

(22)

(23)

1.2.2 平方根矩陣的分解改進

在標準的平方根UKF算法中對于求向前一步預測的估計協(xié)方差平方根Sk+1|k由式(7)與式(8)給出，但按式(8)實際的可表達為

(24)

cholupdate要求式(24)的右邊必須是正定的，因此運用cholupdate更新協(xié)方差矩陣要求比較高。

事實上根據(jù)向前一步預測的估計協(xié)方差矩陣計算公式：

(26)

實際上進一步可令

rT=Sk+1|k

(27)

又由式(25)得

(28)

則由式(26)～式(28)可直接推得到Sk+1|k的更新方程如下：

(29)

(30)

同理對于式(11)和式(12)的量測估計協(xié)方差的平方根矩陣更新方程可以改為如下：

(31)

(32)

另外，在標準的平方根UKF濾波算法中對于狀態(tài)誤差協(xié)方差矩陣的更新按式(16)、式(17)可實際表示為

(33)

這要求式(33)的右邊必須是正定的，但是在計算過程中，等式的右邊由于含有相減的項，對舍入誤差十分敏感，很容易因為舍入誤差的積累，使結果失去正定性。因此還需對狀態(tài)估計協(xié)方差的平方根矩陣作如下修改：

根據(jù)協(xié)方差定義得

(34)

由式(15)代入式(34) 得

(35)

對于測量方程是在雷達測量球坐標系建立的，但是可以采用修正無偏轉(zhuǎn)換到直角坐標系中，以位置分量來表示觀測量，則可以得到偽觀測方程為

(36)

(37)

把式(37)代入式(35)得(其中E為單位矩陣)

Pk+1|k+1=

(38)

Pk+1|k+1=

(39)

因此可以根據(jù)式(29)和式(30)的處理方法來得到改進的更新狀態(tài)估計協(xié)方差的平方根矩陣：

(40)

(41)

下面將上述改進后的公式計算量與原公式計算量進行對比。對于一個x×y(x≥y)的矩陣[7]，qr分解的計算復雜度為O(xy2)，cholupdate更新的計算復雜度為O(2xy2)，矩陣轉(zhuǎn)置的計算復雜度為O(xlgx)，矩陣乘法的計算復雜度為O(xy2)。因此式(7)、式(8)、式(11)、式(12)、式(16)及式(17)的計算復雜度分別為O(3n3)，O(2n3)，O(2nm2+m3)，O(2m3)，O(nm2)，O(2n3)；式(29)、式(30)、式(31)、式(32)、式(40)及式(41)的計算復雜度分別為O(3n3+n2)，O(nlgn)，O(2nm2+m2+m3)，O(mlgm)，O(3n3+2nm2)，O(nlgn)。綜上，原算法的計算復雜度為O(7n3+3nm2+3m3)；改進后的算法復雜度為O(6n3+4nm2+m2+m3+mlgm+2nlgn)。一般情況下，狀態(tài)參量的維數(shù)n大于觀測量的維數(shù)m，因此，改進后的算法復雜度小于原算法。

1.2.3 引入自適應因子

對于濾波器，自適應算法的引入將有效改善其對不確定系統(tǒng)的跟蹤性能。根據(jù)文獻[6]可得，采用多重次自適應調(diào)節(jié)因子矩陣Dk+1調(diào)節(jié)狀態(tài)誤差一步預測協(xié)方差陣Pk+1|k可以達到良好的自適應調(diào)節(jié)效果，其具體形式如下：

(42)

式中，多重次自適應調(diào)節(jié)因子的計算如下：

(43)

(44)

(45)

(46)

(47)

(48)

式中：Nii，Hii以及Jii分別表示矩陣N，H，J的第i行第i列的元素；m為量測向量的維數(shù)；Fk+1為狀態(tài)方程的jacobian矩陣；ωk+1為殘差，η為遺忘因子，取值在0～1之間，通常取為0.95。

則計算出自適應因子矩陣后，對于式(29)、式(30)的向前一步預測估計協(xié)方差平方根矩陣更新方程可以改為

(49)

(50)

2 雷達觀測量轉(zhuǎn)換

由上可得，雷達的非線性測量方程需要線性轉(zhuǎn)化以簡化運算，這可以通過雷達的觀測量轉(zhuǎn)換，用轉(zhuǎn)化后的間接觀測量(偽觀測量)來替代實際的觀測量來實現(xiàn)。

以雷達所在位置為原點，在縱平面上，目標位置信息主要由觀測距離r以及俯仰角θ來表征，其中觀測距離標準差為σr，俯仰角標準差為σθ，則對應的協(xié)方差矩陣為

(51)

目標的位置矢量與觀測距離r、俯仰角θ有如下關系：

(52)

將位置x，y作為間接觀測量來代替實際觀測量r和θ，可得偽觀測方程為

(53)

此時，還需要對觀測協(xié)方差矩陣進行轉(zhuǎn)換，根據(jù)協(xié)方差傳播理論可得

(54)

(55)

(56)

3 仿真及分析

3.1 仿真條件設置

設空中一目標以如下的運動方程運動：

(57)

式中：ax與ay分別為目標的橫縱向加速度；ρ0=1.225 0 kg/m3為海平面的空氣密度，ha=6 700為空氣密度和高度之間的關系常量，g=9.8 m/s2為重力加速度，k=1為彈道系數(shù)。

設置目標與雷達站初始橫向距離x(0)=0 m，初始高度y(0)=50 000 m，初始橫向速度vx(0)=100 m/s，初始縱向速度vy(0)=200 m/s，仿真時長為500 s，目標運動軌跡如圖1所示。

為了驗證本文提出的ASRS-UKF濾波算法的有效性，分別將ASRS-UKF算法、SR-UKF濾波算法以及STSR-UKF算法對上述軌跡進行跟蹤濾波。

(58)

式中，

(59)

上式的離散化形式可以簡化為

(60)

3.2 跟蹤結果及分析

跟蹤采樣周期為0.1 s，測距精度為100 m，測角精度為0.001 5 rad，進行100次蒙特卡洛，得到的3種算法跟蹤結果如圖2～圖5所示。

由圖3得，在橫向位置上，3種濾波算法的跟蹤精度都表現(xiàn)不錯，但是還是ASRS-UKF算法精度最高，其次是STSR-UKF算法，最差的是SR-UKF算法；在收斂速度上差距較明顯，其中ASRS-UKF算法收斂速度最快(在10 s左右的位置)，其次是STSR-UKF算法(在25 s左右的位置)，最慢的是SR-UKF算法(在50 s左右的位置)。然而，在縱向位置上，ASRS-UKF算法則表現(xiàn)出了優(yōu)越的性能，在收斂速度、跟蹤精度以及穩(wěn)定度上都明顯要好于前兩者，最差的是無自適應能力的SR-UKF算法。結合圖2可以發(fā)現(xiàn)，在橫向位置上目標軌跡較平緩，機動性較小，系統(tǒng)模型可以更好地匹配，因此3種濾波算法都表現(xiàn)不錯；而在縱向位置上，目標軌跡變化較大，機動性較強，因此無自適應能力的SR-UKF算法無法穩(wěn)定跟蹤。

由圖5可以看出，在橫向速度上，3種濾波算法的跟蹤情況都表現(xiàn)不錯，其中跟蹤精度上STSR-UKF算法略微優(yōu)于ASRS-UKF算法和SR-UKF算法；但是在收斂速度上，ASRS-UKF算法則表現(xiàn)最快(在10 s左右的位置)，其次是STSR-UKF算法(在25 s左右的位置)，最慢的是SR-UKF算法(在45 s左右的位置)。在縱向速度上，ASRS-UKF算法在收斂速度、跟蹤精度以及穩(wěn)定度上要好于前兩種，明顯好于SR-UKF算法，STSR-UKF算法也有不俗的表現(xiàn)。結合圖4可以發(fā)現(xiàn)，在橫向速度上目標表現(xiàn)為勻加速運動，機動性較小，系統(tǒng)模型可以更好地匹配，因此3種濾波算法都表現(xiàn)不錯；而在縱向位置上，目標的運動方程復雜，機動性較強，系統(tǒng)模型難以很好的匹配，因此SR-UKF算法跟蹤性能最差。

表1 各算法的總體跟蹤性能比較

綜上可得，本文的ASRS-UKF算法無論在計算速度、濾波精度、收斂速度以及穩(wěn)定性都要好于SR-UKF算法和STSR-UKF算法，仿真結果驗證了算法的有效性。

4 結束語

本文通過在標準的平方根UKF算法上，首先改用了球型不敏變換對權系數(shù)以及sigma點進行選取，其次改進了平方根UKF中平方根矩陣的分解方法，以及在預測誤差協(xié)方差矩陣中引入了自適應衰減因子，設計了自適應平方根球型無跡卡爾曼濾波算法(ASRS-UKF)。最后，通過將該算法同SR-UKF算法以及STSR-UKF算法進行仿真對比，仿真結果表明，ASRS-UKF算法在減少計算量加快計算速度的同時還提高了濾波精度、收斂速度以及穩(wěn)定性，而且對于目標機動性強、系統(tǒng)模型匹配不佳的情況下，仍具有良好的跟蹤性能。