自適應(yīng)平方根球型無(wú)跡卡爾曼濾波算法

， ， ， ，

(1.空軍預(yù)警學(xué)院研究生大隊(duì)， 湖北武漢 430019;2.空軍預(yù)警學(xué)院， 湖北武漢 430019)

0 引言

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

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

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

本文在標(biāo)準(zhǔn)的平方根UKF算法上，通過(guò)改用球型無(wú)跡變換對(duì)權(quán)系數(shù)以及sigma點(diǎn)進(jìn)行選取，改進(jìn)平方根UKF中平方根矩陣的分解方法以及在預(yù)測(cè)誤差協(xié)方差矩陣中引入了自適應(yīng)衰減因子，設(shè)計(jì)了自適應(yīng)平方根球型無(wú)跡卡爾曼濾波算法(ASRS-UKF)。將該算法同SR-UKF算法以及強(qiáng)跟蹤UKF算法(Strong Tracking Square Root UKF，STSR-UKF)進(jìn)行仿真對(duì)比，仿真結(jié)果證明了ASRS-UKF算法的有效性。

1 ASRS-UKF濾波算法

1.1 標(biāo)準(zhǔn)的平方根UKF濾波算法

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

(1)

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

(2)

(3)

式中，chol為標(biāo)準(zhǔn)Matlab指令，表示Cholesky分解。

2) 計(jì)算選取sigma點(diǎn)：

(4)

3) 時(shí)間更新：

(5)

(6)

(7)

(8)

式中，qr和cholupdate為標(biāo)準(zhǔn)的Matlab指令，分別表示QR分解和Cholesky分解一階更新。

4) 量測(cè)更新：

(9)

(10)

(11)

(12)

(13)

5) 濾波結(jié)果更新:

(14)

(15)

U=KkSZ

(16)

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

(17)

其中，上述運(yùn)用的權(quán)值計(jì)算表達(dá)式如下：

(18)

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

1.2 算法的改進(jìn)

1.2.1 球型無(wú)跡變換選取權(quán)系數(shù)以及sigma點(diǎn)

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

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

W0∈[0,1)

(19)

(20)

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

2) 計(jì)算sigma點(diǎn)，平方根形式的球型無(wú)跡變換sigma點(diǎn)計(jì)算如下：

(21)

(22)

(23)

1.2.2 平方根矩陣的分解改進(jìn)

在標(biāo)準(zhǔn)的平方根UKF算法中對(duì)于求向前一步預(yù)測(cè)的估計(jì)協(xié)方差平方根Sk+1|k由式(7)與式(8)給出，但按式(8)實(shí)際的可表達(dá)為

(24)

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

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

(26)

實(shí)際上進(jìn)一步可令

rT=Sk+1|k

(27)

又由式(25)得

(28)

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

(29)

(30)

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

(31)

(32)

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

(33)

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

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

(34)

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

(35)

對(duì)于測(cè)量方程是在雷達(dá)測(cè)量球坐標(biāo)系建立的，但是可以采用修正無(wú)偏轉(zhuǎn)換到直角坐標(biāo)系中，以位置分量來(lái)表示觀測(cè)量，則可以得到偽觀測(cè)方程為

(36)

(37)

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

Pk+1|k+1=

(38)

Pk+1|k+1=

(39)

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

(40)

(41)

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

1.2.3 引入自適應(yīng)因子

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

(42)

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

(43)

(44)

(45)

(46)

(47)

(48)

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

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

(49)

(50)

2 雷達(dá)觀測(cè)量轉(zhuǎn)換

由上可得，雷達(dá)的非線性測(cè)量方程需要線性轉(zhuǎn)化以簡(jiǎn)化運(yùn)算，這可以通過(guò)雷達(dá)的觀測(cè)量轉(zhuǎn)換，用轉(zhuǎn)化后的間接觀測(cè)量(偽觀測(cè)量)來(lái)替代實(shí)際的觀測(cè)量來(lái)實(shí)現(xiàn)。

以雷達(dá)所在位置為原點(diǎn)，在縱平面上，目標(biāo)位置信息主要由觀測(cè)距離r以及俯仰角θ來(lái)表征，其中觀測(cè)距離標(biāo)準(zhǔn)差為σr，俯仰角標(biāo)準(zhǔn)差為σθ，則對(duì)應(yīng)的協(xié)方差矩陣為

(51)

目標(biāo)的位置矢量與觀測(cè)距離r、俯仰角θ有如下關(guān)系：

(52)

將位置x，y作為間接觀測(cè)量來(lái)代替實(shí)際觀測(cè)量r和θ，可得偽觀測(cè)方程為

(53)

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

(54)

(55)

(56)

3 仿真及分析

3.1 仿真條件設(shè)置

設(shè)空中一目標(biāo)以如下的運(yùn)動(dòng)方程運(yùn)動(dòng)：

(57)

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

設(shè)置目標(biāo)與雷達(dá)站初始橫向距離x(0)=0 m，初始高度y(0)=50 000 m，初始橫向速度vx(0)=100 m/s，初始縱向速度vy(0)=200 m/s，仿真時(shí)長(zhǎng)為500 s，目標(biāo)運(yùn)動(dòng)軌跡如圖1所示。

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

(58)

式中，

(59)

上式的離散化形式可以簡(jiǎn)化為

(60)

3.2 跟蹤結(jié)果及分析

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

由圖3得，在橫向位置上，3種濾波算法的跟蹤精度都表現(xiàn)不錯(cuò)，但是還是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)定度上都明顯要好于前兩者，最差的是無(wú)自適應(yīng)能力的SR-UKF算法。結(jié)合圖2可以發(fā)現(xiàn)，在橫向位置上目標(biāo)軌跡較平緩，機(jī)動(dòng)性較小，系統(tǒng)模型可以更好地匹配，因此3種濾波算法都表現(xiàn)不錯(cuò)；而在縱向位置上，目標(biāo)軌跡變化較大，機(jī)動(dòng)性較強(qiáng)，因此無(wú)自適應(yīng)能力的SR-UKF算法無(wú)法穩(wěn)定跟蹤。

由圖5可以看出，在橫向速度上，3種濾波算法的跟蹤情況都表現(xiàn)不錯(cuò)，其中跟蹤精度上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)。結(jié)合圖4可以發(fā)現(xiàn)，在橫向速度上目標(biāo)表現(xiàn)為勻加速運(yùn)動(dòng)，機(jī)動(dòng)性較小，系統(tǒng)模型可以更好地匹配，因此3種濾波算法都表現(xiàn)不錯(cuò)；而在縱向位置上，目標(biāo)的運(yùn)動(dòng)方程復(fù)雜，機(jī)動(dòng)性較強(qiáng)，系統(tǒng)模型難以很好的匹配，因此SR-UKF算法跟蹤性能最差。

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

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

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

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