基于TACKF的海面目標(biāo)航向航速估計(jì)算法

程然， 何科峰， 繆禮鋒

(中國航空工業(yè)集團(tuán)公司雷華電子技術(shù)研究所， 江蘇無錫 214063)

0 引言

航向航速是海面目標(biāo)的重要特征，能準(zhǔn)確估計(jì)出航向航速對(duì)于海面目標(biāo)的跟蹤、識(shí)別和打擊具有非常重要的意義。由于海面目標(biāo)跟蹤中易出現(xiàn)量測(cè)高精度、系統(tǒng)復(fù)雜強(qiáng)非線性等情況，導(dǎo)致傳統(tǒng)非線性濾波器對(duì)海面目標(biāo)航向航速的估計(jì)精度不高[1]。此外，海面運(yùn)動(dòng)目標(biāo)自身速度較慢，濾波器的穩(wěn)態(tài)波動(dòng)對(duì)海面目標(biāo)的航速估計(jì)影響較大，精度難以保證。針對(duì)以上問題，為了改善復(fù)雜環(huán)境下目標(biāo)跟蹤的性能，對(duì)海面目標(biāo)航向航速精確估計(jì)算法的研究迫在眉睫。

近年來，國內(nèi)對(duì)海面目標(biāo)航向航速估計(jì)算法開展了廣泛的研究。文獻(xiàn)[2]針對(duì)海面目標(biāo)航向航速估計(jì)的物理概念、真值的獲取及考核方法作了初步的研究和介紹，但并沒有考慮復(fù)雜情況下提高海面目標(biāo)航向航速估計(jì)精度的具體途徑。文獻(xiàn)[3]推導(dǎo)出了雷達(dá)載體在高速運(yùn)動(dòng)條件下海面目標(biāo)跟蹤的濾波方程，并利用雷達(dá)載體的GPS信息和目標(biāo)的測(cè)量信息實(shí)現(xiàn)了對(duì)目標(biāo)航向航速的高精度解算。然而這種方法僅在海面目標(biāo)作勻速直線運(yùn)動(dòng)時(shí)估計(jì)精度才較高，當(dāng)目標(biāo)發(fā)生機(jī)動(dòng)(如協(xié)同轉(zhuǎn)彎)時(shí)，所建目標(biāo)的運(yùn)動(dòng)學(xué)模型與目標(biāo)的實(shí)際運(yùn)動(dòng)模式不匹配，導(dǎo)致濾波器發(fā)散，最終造成目標(biāo)的跟蹤丟失。鑒于以上問題和不足，本文緊密結(jié)合工程應(yīng)用背景，針對(duì)海面目標(biāo)跟蹤中易出現(xiàn)的量測(cè)高精度、系統(tǒng)復(fù)雜強(qiáng)非線性等問題，提出了一種基于截?cái)嗟淖赃m應(yīng)容積卡爾曼濾波算法(TACKF)的海面目標(biāo)航向航速估計(jì)算法。

首先，本文簡(jiǎn)要介紹了非線性高斯濾波器(GF)的基本原理，分析了GF在量測(cè)更新階段所存在的主要問題。其次，本文簡(jiǎn)要闡述了截?cái)嗫柭鼮V波器(TKF)的基本原理，并引入了一種自適應(yīng)調(diào)節(jié)機(jī)制，將TKF與容積卡爾曼濾波器(CKF)通過一個(gè)自適應(yīng)變化的參數(shù)有機(jī)地結(jié)合起來，從而推導(dǎo)得出截?cái)嗟淖赃m應(yīng)容積卡爾曼濾波器(TACKF)。最后，結(jié)合數(shù)值仿真及實(shí)驗(yàn)數(shù)據(jù)將本文所提出的濾波算法應(yīng)用到海面目標(biāo)的航向航速估計(jì)中，數(shù)據(jù)仿真結(jié)果證明了該算法的有效性。

1 GF算法結(jié)構(gòu)

考慮如下形式的狀態(tài)空間離散非線性系統(tǒng)：

(1)

式中，xk∈Rnx表示k時(shí)刻系統(tǒng)狀態(tài)向量(nx為狀態(tài)維數(shù))，zk∈Rnz表示k時(shí)刻外部量測(cè)向量(nz為狀態(tài)維數(shù))，f(·)表示狀態(tài)轉(zhuǎn)移函數(shù)，h(·)表示量測(cè)函數(shù)，wk-1與vk分別表示系統(tǒng)噪聲和量測(cè)噪聲，二者互不相關(guān)且均為零均值高斯白噪聲，即wk-1～N(0,Qk-1)，vk～N(0,Rk)。依據(jù)線性最小方差準(zhǔn)則，GF的一般結(jié)構(gòu)[4]為

(2)

式中，

(3)

式中，Zk-1表示直到k-1時(shí)刻所有的量測(cè)信息。從上述GF的一般結(jié)構(gòu)可知，GF可分為兩個(gè)部分，即時(shí)間更新和量測(cè)更新。GF的估計(jì)精度主要取決于對(duì)系統(tǒng)狀態(tài)均值和協(xié)方差的計(jì)算精度。傳統(tǒng)GF對(duì)式(3)通常采用近似的方法獲得，例如無跡卡爾曼濾波器(UKF)采用Unscented變換的近似方法[5]；容積卡爾曼濾波器(CKF)采用容積變換的近似方法[6]，不同的近似方法會(huì)得到不同的估計(jì)精度。

2 TKF算法概要

2.1 GF量測(cè)更新階段分析

(4)

(5)

GF中，狀態(tài)與量測(cè)近似的條件聯(lián)合概率密度函數(shù)[7]為

(6)

(7)

(8)

為了表示近似的條件聯(lián)合概率密度函數(shù)與真實(shí)的條件聯(lián)合概率密度函數(shù)的偏差程度，文獻(xiàn)[7]給出了一種計(jì)算KLD偏差參數(shù)的表示方法，并以此來表征GF的估計(jì)精度。KLD參數(shù)值越小，表明GF對(duì)狀態(tài)真實(shí)后驗(yàn)概率密度函數(shù)的逼近程度越高。KLD參數(shù)本質(zhì)上是一個(gè)近似化的指標(biāo)參數(shù)，它是建立在對(duì)非線性量測(cè)函數(shù)一階線性化基礎(chǔ)之上的，具體表達(dá)式[7]為

(9)

(10)

(11)

(12)

對(duì)EKF來說，由于它只對(duì)量測(cè)函數(shù)進(jìn)行一階泰勒展開并忽略所有高階矩信息，因此EKF的KLD參數(shù)表達(dá)式[7]整理為

(13)

(14)

從EKF的KLD表達(dá)式中可以看出，EKF的估計(jì)精度主要取決于3個(gè)因素，即量測(cè)噪聲方差、系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差及量測(cè)函數(shù)。對(duì)于確定的系統(tǒng)狀態(tài)先驗(yàn)分布及量測(cè)函數(shù)，當(dāng)量測(cè)噪聲方差減小時(shí)，EKF的KLD參數(shù)升高，此時(shí)EKF對(duì)系統(tǒng)狀態(tài)真實(shí)的后驗(yàn)概率密度函數(shù)逼近精度變差。而當(dāng)量測(cè)噪聲方差和量測(cè)函數(shù)一定，隨著系統(tǒng)狀態(tài)的先驗(yàn)不確定性逐漸加強(qiáng)，即系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差逐漸增大，同樣會(huì)使KLD參數(shù)升高，系統(tǒng)狀態(tài)的估計(jì)精度下降。

對(duì)于UKF，CKF等利用確定性采樣策略的傳統(tǒng)GF而言，由于考慮了系統(tǒng)狀態(tài)的高階矩信息，因此在精度上要優(yōu)于EKF。它們的KLD表達(dá)式本質(zhì)上是一樣的，唯一區(qū)別就是采樣點(diǎn)的選取方式不同。因此，這類GF對(duì)系統(tǒng)狀態(tài)真實(shí)后驗(yàn)概率密度函數(shù)的近似精度同樣受到量測(cè)噪聲方差、狀態(tài)先驗(yàn)協(xié)方差及非線性量測(cè)函數(shù)的影響，分析過程同EKF相似，不再贅述。

2.2 TKF原理分析

通過前面的分析可以得出這樣一個(gè)結(jié)論，即在量測(cè)高精度、系統(tǒng)復(fù)雜強(qiáng)非線性的情況下傳統(tǒng)GF對(duì)系統(tǒng)狀態(tài)后驗(yàn)概率密度函數(shù)的近似精度不是很高。為了解決這一問題，文獻(xiàn)[8]提出了一種截?cái)嗟目柭鼮V波器(TKF)。TKF的濾波結(jié)構(gòu)框架與傳統(tǒng)GF大體相同，唯一的區(qū)別在于TKF在時(shí)間更新階段對(duì)系統(tǒng)狀態(tài)的先驗(yàn)分布進(jìn)行了截?cái)嗵幚?，因此本?jié)只分析TKF在時(shí)間更新階段的濾波原理。

與之前的分析有所不同，假設(shè)此時(shí)的量測(cè)是高精度的，也就是說量測(cè)噪聲的方差足夠小，那么我們認(rèn)為以下的近似是合理的，即將服從高斯分布的量測(cè)噪聲近似為服從均勻分布，且近似后的量測(cè)噪聲均值和方差保持不變[8]。因?yàn)榱繙y(cè)噪聲方差足夠小，這就意味著近似為均勻分布后的量測(cè)噪聲被限制在一個(gè)很小的區(qū)域內(nèi)，因此我們認(rèn)為此時(shí)系統(tǒng)狀態(tài)的先驗(yàn)概率密度函數(shù)在其相應(yīng)的定義域內(nèi)可以近似為一個(gè)常數(shù)[8]。

(15)

(16)

整理，得

(17)

似然概率密度函數(shù)表示為

p(zk|xk)=pvk(zk-h(xk))Ix(xk)

(18)

(19)

式中，Ix表示截?cái)嗟膮^(qū)域：

(20)

根據(jù)貝葉斯準(zhǔn)則，系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)可表示為

p(xk|Zk)∝p(zk|xk)p0(xk|Zk-1)=

pvk(zk-h(xk))Ix(xk)p0(xk|Zk-1) (21)

式(21)可以看作是系統(tǒng)狀態(tài)的先驗(yàn)概率密度函數(shù)被“截?cái)唷绷耍式財(cái)嗟南到y(tǒng)狀態(tài)先驗(yàn)概率密度函數(shù)可表示為

(22)

式中，p1(·)表示截?cái)嗟南到y(tǒng)狀態(tài)先驗(yàn)概率密度函數(shù)，ε表示歸一化常數(shù)，表示為

(23)

將式(17)代入式(23)，得

(24)

整理，得

式中，|Iv|表示近似成均勻分布的量測(cè)噪聲統(tǒng)計(jì)分布區(qū)域的面積。則截?cái)嗟南闰?yàn)均值為

(26)

將式(17)代入式(26)整理，得

因?yàn)榱繙y(cè)噪聲的均值沒變，即

(28)

將式(28)代入式(27)整理，得

(29)

截?cái)嗟南到y(tǒng)狀態(tài)先驗(yàn)協(xié)方差為

(30)

將式(17)代入式(30)，得

(31)

整理，得

(32)

從式(32)可以看出，在TKF的時(shí)間更新階段，系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差與量測(cè)噪聲的方差建立起了聯(lián)系。根據(jù)前面介紹的KLD參數(shù)理論，當(dāng)系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差隨量測(cè)噪聲方差作相同趨勢(shì)的變化時(shí)，會(huì)抵消KLD參數(shù)的整體變化，從而使得濾波器對(duì)系統(tǒng)狀態(tài)真實(shí)的后驗(yàn)概率密度函數(shù)的逼近程度不會(huì)受到外部高精度量測(cè)的顯著影響。同時(shí)，文獻(xiàn)[7]還指出，當(dāng)系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差減小時(shí)會(huì)減輕外部量測(cè)非線性對(duì)GF量測(cè)更新階段的影響。對(duì)系統(tǒng)狀態(tài)的先驗(yàn)概率密度函數(shù)進(jìn)行截?cái)嗵幚?，相?dāng)于增加對(duì)系統(tǒng)狀態(tài)先驗(yàn)估計(jì)的置信程度，降低了系統(tǒng)狀態(tài)的先驗(yàn)不確定性，從而使整個(gè)濾波器的估計(jì)性能得到進(jìn)一步提升。

3 TACKF算法推導(dǎo)

本節(jié)將引入一種自適應(yīng)調(diào)整機(jī)制，將TKF與某種GF通過一個(gè)自適應(yīng)變化的參數(shù)相結(jié)合，使其能夠根據(jù)外部量測(cè)信息的變化動(dòng)態(tài)地調(diào)整二者相對(duì)權(quán)值的大小，從整體上提高濾波性能。由于CKF具有算法實(shí)現(xiàn)簡(jiǎn)單、濾波精度高、收斂性好等優(yōu)點(diǎn)，且具有更高的數(shù)值穩(wěn)定性[6]，因此本節(jié)擬采用TKF與CKF有機(jī)相結(jié)合的方法得到TACKF算法。

3.1 TKF與CKF自適應(yīng)調(diào)整機(jī)制

(33)

式中，α∈[0,1]是一個(gè)自適應(yīng)變化的參數(shù)。

從前面對(duì)GF量測(cè)更新階段的分析中可知：當(dāng)外部量測(cè)噪聲方差和量測(cè)函數(shù)一定時(shí)，隨著系統(tǒng)狀態(tài)先驗(yàn)協(xié)方差的逐漸增大(系統(tǒng)狀態(tài)的先驗(yàn)不確定性逐漸增強(qiáng))，GF的KLD參數(shù)會(huì)逐漸升高，從而導(dǎo)致系統(tǒng)狀態(tài)的估計(jì)精度下降。同時(shí)文獻(xiàn)[8]指出，當(dāng)外部量測(cè)信息較為豐富時(shí)，TKF相比于傳統(tǒng)GF能夠發(fā)揮更好的濾波性能。而當(dāng)外部量測(cè)信息相對(duì)匱乏時(shí)，TKF的優(yōu)勢(shì)則被削弱，傳統(tǒng)GF的濾波性能得到改善。

基于這兩個(gè)結(jié)論可以看出：當(dāng)外部量測(cè)信息較為豐富時(shí)，TKF系統(tǒng)狀態(tài)的先驗(yàn)不確定性比CKF的弱，TKF相比于CKF能夠發(fā)揮更好的濾波性能；相反，當(dāng)外部量測(cè)信息相對(duì)匱乏時(shí)，TKF中系統(tǒng)狀態(tài)的先驗(yàn)不確定性比CKF的強(qiáng)，TKF的優(yōu)勢(shì)被削弱，CKF的濾波性能得到改善。故可以將自適應(yīng)變化參數(shù)α的選取與系統(tǒng)狀態(tài)的先驗(yàn)協(xié)方差建立起一種關(guān)系，從而更加明確α的取值：

(34)

式中，γ是一個(gè)用戶設(shè)定的預(yù)調(diào)整參數(shù)，它可以預(yù)先調(diào)整系統(tǒng)狀態(tài)先驗(yàn)協(xié)方差矩陣跡的相對(duì)權(quán)重大小來改善參數(shù)α的自適應(yīng)調(diào)整。從式(34)可以看到，當(dāng)γ取值減小時(shí)，說明截?cái)嗟南到y(tǒng)狀態(tài)先驗(yàn)概率密度函數(shù)的相對(duì)權(quán)重被逐漸削弱；反之當(dāng)γ取值增大時(shí)，則說明要對(duì)截?cái)嗟南到y(tǒng)狀態(tài)先驗(yàn)概率密度函數(shù)的相對(duì)權(quán)重逐漸加強(qiáng)。

當(dāng)γ取值一定時(shí)，隨著外部量測(cè)信息逐漸變得豐富，TKF系統(tǒng)狀態(tài)的先驗(yàn)不確定性逐漸降低，自適應(yīng)變化參數(shù)α逐漸趨于1，此時(shí)TKF發(fā)揮主要作用，當(dāng)α=1時(shí)濾波器則轉(zhuǎn)化為純粹的TKF；相反，隨著外部量測(cè)信息逐漸變得匱乏，TKF系統(tǒng)狀態(tài)的先驗(yàn)不確定性逐漸升高，自適應(yīng)變化參數(shù)α逐漸趨于0，此時(shí)CKF發(fā)揮主要作用，當(dāng)α=0時(shí)濾波器則完全退化為CKF。故TKF與CKF的這種自適應(yīng)調(diào)整機(jī)制能夠更好地適應(yīng)外部量測(cè)信息的變化，從整體上提高濾波器的濾波性能。

3.2 TACKF濾波算法

與傳統(tǒng)GF的濾波結(jié)構(gòu)相同，TACKF也是由時(shí)間更新和量測(cè)更新兩部分組成，具體實(shí)施步驟如下。

1) 時(shí)間更新

通過Cholesky分解狀態(tài)的協(xié)方差矩陣Pk-1|k-1：

(35)

根據(jù)Spherical-Radial Cubature準(zhǔn)則計(jì)算容積點(diǎn)：

(36)

(37)

通過狀態(tài)方程傳播容積點(diǎn)：

(38)

計(jì)算CKF中系統(tǒng)狀態(tài)的先驗(yàn)均值及協(xié)方差矩陣：

(39)

計(jì)算TKF中系統(tǒng)狀態(tài)的先驗(yàn)均值及協(xié)方差矩陣：

(40)

2) 量測(cè)更新

通過Cholesky分解Ph,k|k-1(h∈{0,1})：

(41)

根據(jù)Spherical-Radial Cubature準(zhǔn)則計(jì)算容積點(diǎn)：

(42)

通過量測(cè)方程傳播容積點(diǎn)：

Zh,i,k|k-1=h(Xh,i,k|k-1)

(43)

計(jì)算量測(cè)的先驗(yàn)均值及自相關(guān)先驗(yàn)協(xié)方差矩陣：

(44)

計(jì)算互相關(guān)協(xié)方差矩陣：

(45)

分別計(jì)算CKF和TKF的卡爾曼濾波增益：

(46)

分別計(jì)算CKF和TKF狀態(tài)的后驗(yàn)均值及協(xié)方差：

(47)

計(jì)算系統(tǒng)狀態(tài)最終的后驗(yàn)均值及協(xié)方差矩陣：

(48)

(49)

4 仿真與分析

4.1 數(shù)值仿真驗(yàn)證

(50)

式中，ω表示海面目標(biāo)轉(zhuǎn)彎角速率，T表示機(jī)載雷達(dá)采樣周期，wk-1表示高斯系統(tǒng)噪聲。系統(tǒng)噪聲方差矩陣Qk-1為

(51)

量測(cè)方程表示為

從數(shù)值仿真結(jié)果中可以看出，當(dāng)海面目標(biāo)作協(xié)同轉(zhuǎn)彎運(yùn)動(dòng)時(shí)，TACKF對(duì)目標(biāo)航向航速的估計(jì)精度要明顯高于CKF及UKF。而當(dāng)海面目標(biāo)作勻速直線運(yùn)動(dòng)時(shí)，3個(gè)濾波器對(duì)目標(biāo)航向航速的估計(jì)精度相當(dāng)。證明了在量測(cè)高精度、系統(tǒng)復(fù)雜強(qiáng)非線性的條件下TACKF能夠更有效地逼近系統(tǒng)狀態(tài)的后驗(yàn)概率密度函數(shù)，從而有效提高了海面目標(biāo)航向航速的估計(jì)精度。

圖1 海面目標(biāo)運(yùn)動(dòng)軌跡

圖2 各濾波器對(duì)海面目標(biāo)航速估計(jì)的RMSE曲線

圖3 各濾波器對(duì)海面目標(biāo)航向估計(jì)的RMSE曲線

輸出參數(shù)算法分類TACAFUKFCKF航速RMSE/(m·s-1)航向RMSE/(°)2.4312.823.2921.023.0719.05

表2 勻速運(yùn)動(dòng)段目標(biāo)航向航速估計(jì)精度統(tǒng)計(jì)

4.1 實(shí)驗(yàn)數(shù)據(jù)驗(yàn)證

結(jié)合實(shí)驗(yàn)數(shù)據(jù)將本文所提出的濾波算法應(yīng)用到對(duì)海面目標(biāo)航向航速的離線估計(jì)中，分別給出在不同時(shí)間統(tǒng)計(jì)區(qū)間內(nèi)各濾波器的圖像對(duì)比分析和數(shù)據(jù)精度分析，結(jié)果如表3、圖4、圖5所示。

結(jié)合實(shí)驗(yàn)數(shù)據(jù)的仿真結(jié)果可以看出，相比于EKF，本文所提出的TACKF算法能夠發(fā)揮更好的濾波性能，具有更高的濾波精度，更適于應(yīng)用到海面運(yùn)動(dòng)目標(biāo)的航向航速估計(jì)中。

5 結(jié)束語

本文緊密結(jié)合工程應(yīng)用背景，針對(duì)海面目標(biāo)跟蹤中易出現(xiàn)的量測(cè)高精度、系統(tǒng)復(fù)雜強(qiáng)非線性等問題，提出了一種基于截?cái)嗟淖赃m應(yīng)容積卡爾曼濾波算法(TACKF)的海面目標(biāo)航向航速估計(jì)算法。通過引入一種自適應(yīng)調(diào)整機(jī)制，將TKF與CKF有機(jī)地結(jié)合起來，使其能夠根據(jù)外部量測(cè)信息的變化動(dòng)態(tài)地調(diào)整二者相對(duì)權(quán)值的大小，有效改善了濾波器的估計(jì)性能。結(jié)合實(shí)驗(yàn)數(shù)據(jù)將本論文所提出的濾波算法應(yīng)用到對(duì)海面目標(biāo)航向航速的離線估計(jì)中，分別給出在不同時(shí)間統(tǒng)計(jì)區(qū)間和內(nèi)各濾波器的圖像對(duì)比分析數(shù)據(jù)精度分析。仿真結(jié)果表明，本論文所提出的濾波算法較傳統(tǒng)的非線性濾波算法有顯著的性能提升，可以有效提高復(fù)雜環(huán)境下海面目標(biāo)航向航速的估計(jì)精度。

表3 海面目標(biāo)航向航速估計(jì)數(shù)據(jù)精度統(tǒng)計(jì)

圖4 各濾波器對(duì)海面目標(biāo)航向角的估計(jì)結(jié)果

圖5 各濾波器對(duì)海面目標(biāo)航速的估計(jì)結(jié)果

[1]徐晉,付啟眾,陸鵬程,等. 米波雷達(dá)對(duì)海面目標(biāo)探測(cè)性能分析及驗(yàn)證[J]. 雷達(dá)科學(xué)與技術(shù), 2012, 10(4):376-379. XU Jin, FU Qizhong, LU Pengcheng, et al. Analysis and Experiment Verification of Maritime Target Detection Performance of Meter-Wave Radar[J]. Radar Science and Technology, 2012, 10(4):376-379.(in Chinese)

[2]袁桂生. 目標(biāo)航向航速解算及驗(yàn)證方法研究[J]. 雷達(dá)與對(duì)抗, 2009(1):26-29.

[3]黃孟俊,陳建軍,趙宏鐘. 海上目標(biāo)航向航速解算新方法[J]. 信號(hào)處理, 2011, 27(7):1034-1038.

[4]張勇剛,黃玉龍,武哲民,等. 一種高階無跡卡爾曼濾波方法[J]. 自動(dòng)化學(xué)報(bào), 2014, 40(5):838-848. ZHANG Yonggang, HUANG Yulong, WU Zhemin, et al. A High Order Unscented Kalman Filtering Method[J]. Acta Automatica Sinica, 2014, 40(5):838-848.(in Chinese)

[5]CHANG Lubin, HU Baiqing, LI An, et al. Transformed Unscented Kalman Filter[J]. IEEE Trans on Automatic Control, 2013, 58(1):252-257.

[6]JIA Bin, XIN Ming, CHENG Yang. High-Degree Cubature Kalman Filter[J]. Automatica, 2013, 49(2):510-518.

[7]MORELANDE M R, GARCIA-FERNANDEZ A F. Analysis of Kalman Filter Approximations for Nonlinear Measurements[J]. IEEE Trans on Signal Processing, 2013, 61(22):5477-5484.

[8]GARCIA-FERNANDEZ A F, MORELANDE M R, GRAJAL J. Truncated Unscented Kalman Filtering[J]. IEEE Trans on Signal Processing, 2012, 60(7):3372-3386.