黃碩 李冠男 荊濤 曹潔

摘 要：針對(duì)標(biāo)準(zhǔn)容積卡爾曼濾波（CKF）在目標(biāo)跟蹤中出現(xiàn)的問(wèn)題，根據(jù)系統(tǒng)噪聲統(tǒng)計(jì)特性不準(zhǔn)確或未知的特點(diǎn)，提出一種基于協(xié)方差匹配原則的自適應(yīng)容積卡爾曼濾波算法。該算法通過(guò)利用新息序列與殘差序列來(lái)實(shí)現(xiàn)對(duì)觀測(cè)噪聲協(xié)方差和過(guò)程噪聲協(xié)方差矩陣的實(shí)時(shí)跟蹤，進(jìn)而進(jìn)行在線調(diào)整噪聲統(tǒng)計(jì)特性，能夠有效的改善由于噪聲特性未知所引起的濾波發(fā)散相關(guān)問(wèn)題。將該算法應(yīng)用在目標(biāo)跟蹤仿真實(shí)驗(yàn)中，結(jié)果表明，與標(biāo)準(zhǔn)CKF算法相比，在系統(tǒng)噪聲統(tǒng)計(jì)特性未知的情況下，該算法具有更好的實(shí)時(shí)性，不僅抑制了濾波器的發(fā)散問(wèn)題，而且提高了對(duì)目標(biāo)的跟蹤精度。

關(guān)鍵詞：容積卡爾曼濾波算法；協(xié)方差匹配；自適應(yīng)濾波；目標(biāo)跟蹤

中圖分類(lèi)號(hào)：TN713；TN953；V557 文獻(xiàn)標(biāo)識(shí)碼：A 文章編號(hào)：2096-4706（2018）02-0062-05

New Adaptive Cubature Kalman Filter Algorithm and Its Application in Target Tracking

HUANG Shuo，LI Guannan，JING Tao，CAO Jie

（Department of Information Engineering，Army Armored Military Academy，Beijing 100072，China）

Abstract：In view of the problem of standard volume Calman filter （CKF） in target tracking，an adaptive volume Calman filtering algorithm based on covariance matching principle is proposed in the light of the inaccurate or unknown characteristics of the statistical characteristics of the system noise.The algorithm realizes the real-time tracking of the covariance of the observed noise and the covariance matrix of the process noise by using the new interest sequence and the residual sequence，and then adjusts the statistical characteristics of the noise on line，and can effectively improve the filtering and divergence problem caused by the unknown noise characteristics.The algorithm is applied to the target tracking simulation experiment. The results show that，compared with the standard CKF algorithm，the algorithm has better real-time performance in the case of unknown noise statistical characteristics of the system，which not only inhibits the divergence of the filter， but also improves the tracking precision of the target.

Keywords：cubature Kalman filter；covariance matching；adaptive filter；target tracking

0 引 言

為了消除跟蹤過(guò)程中產(chǎn)生的噪聲污染，進(jìn)一步根據(jù)量測(cè)值來(lái)確定目標(biāo)運(yùn)動(dòng)狀態(tài)，濾波器也演變出多種變幻形式。常用的非線性濾波算法主要有擴(kuò)展卡爾曼濾波（Extended Kalman Filter，EKF）[1]和無(wú)跡卡爾曼濾波（Unseented Kalman Filter，UKF）[2，3]。與EKF相比，UKF不需要計(jì)算雅克比矩陣，運(yùn)算量較小，并提高了非線性濾波的精度。容積卡爾曼濾波（Cubature Kalman Filter，CKF）[4，5]算法是近年提出的一種獲得廣泛關(guān)注的非線性濾波算法。它是由Arasaratnam提出的新型高斯濾波方法，利用球面-徑向積分準(zhǔn)則，選取樣本點(diǎn)。UKF與CKF有相似的特性，由于CKF將采樣點(diǎn)都選取在超球面上，不存在權(quán)重為負(fù)的情況，因此具有相同的權(quán)重值。相比于其他幾種卡爾曼濾波器，CKF有著較高的精度。

濾波算法的精準(zhǔn)度主要依賴(lài)于對(duì)噪聲統(tǒng)計(jì)特性的判斷，而噪聲統(tǒng)計(jì)特性的不確定性及特性突變等情況，均會(huì)對(duì)濾波效果產(chǎn)生較大的影響。而且現(xiàn)今大部分改進(jìn)的濾波算法，將噪聲特性定為理想狀態(tài)下的高斯白噪聲，缺乏了對(duì)突變情況下濾波器效果的判斷。針對(duì)此類(lèi)問(wèn)題，許多研究對(duì)自適應(yīng)濾波算法進(jìn)行了改良，于浛等[6]和丁家琳等[7]的研究均采用基于Sage-Husa噪聲估計(jì)器的自適應(yīng)濾波器，在噪聲特性未知的情況下，濾波精度得到了很大程度的提高，但巨大的運(yùn)算量限制了其在實(shí)際工程中的應(yīng)用。Salahshoor等[8]將擴(kuò)展卡爾曼濾波利用新息與協(xié)方差序列進(jìn)行自適應(yīng)的改進(jìn)，但從擴(kuò)展卡爾曼濾波算法自身來(lái)看，還存在許多不足。

為了降低運(yùn)算量，同時(shí)又能夠有效地提高濾波器的精確度，本文通過(guò)引入新息序列與殘差序列利用協(xié)方差匹配的原則，對(duì)觀測(cè)噪聲和量測(cè)噪聲的實(shí)際特性進(jìn)行在線修正。在仿真實(shí)驗(yàn)中，在系統(tǒng)噪聲特性未知的情況下，濾波精度有了明顯的提高，效果好于標(biāo)準(zhǔn)CKF。

1 標(biāo)準(zhǔn)CKF算法

CKF算法利用球面-徑向積分準(zhǔn)則，選取的樣本點(diǎn)均勻分布在超球體上，因此每個(gè)樣本點(diǎn)都具有相同的正權(quán)值，不斷傳播容積點(diǎn)實(shí)現(xiàn)濾波，每一次濾波都進(jìn)行時(shí)間更新和量測(cè)更新兩大步驟。設(shè)離散時(shí)間非線性系統(tǒng)如下：

（1）

（2）

其中，表示系統(tǒng)在時(shí)刻的狀態(tài)向量，表示系統(tǒng)位于時(shí)刻的量測(cè)向量，用來(lái)表示目標(biāo)此刻的位置、速度、加速度等特征參數(shù)，假設(shè)過(guò)程噪聲與量測(cè)噪聲是相互獨(dú)立的高斯白噪聲，即：（0，），：（0，）。

CKF算法需要計(jì)算容積點(diǎn)，而后利用2n個(gè)容積點(diǎn)進(jìn)行加權(quán)求和來(lái)近似高斯積分，對(duì)于解決任意分布函數(shù)利用容積積分準(zhǔn)則求解積分問(wèn)題，可以表示為：

（3）

式中

，

表示容積點(diǎn)的權(quán)值，表示傳播的容積點(diǎn)集。

現(xiàn)在假設(shè)k+1時(shí)刻的后驗(yàn)概率密度函數(shù) 已知，CKF算法步驟如下：

Step1 時(shí)間更新

（1）求解協(xié)方差矩陣平方根和容積點(diǎn)：

（4）

（5）

（2）通過(guò)非線性狀態(tài)方程傳播容積點(diǎn)：

（6）

（3）得到狀態(tài)預(yù)測(cè)值和狀態(tài)預(yù)測(cè)協(xié)方差陣：

（7）

（8）

Step2 量測(cè)更新

（1）求解預(yù)測(cè)方差平方根和容積點(diǎn)：

（9）

（10）

（2）通過(guò)非線性量測(cè)方程傳播容積點(diǎn)：

（11）

（3）k時(shí)刻量測(cè)值的觀測(cè)預(yù)測(cè)值、新息方差陣和互協(xié)方差陣：

（12）

（13）

（14）

（4）計(jì)算時(shí)刻增益矩陣、狀態(tài)更新值和協(xié)方差陣更新值：

（15）

（16）

（17）

2 自適應(yīng)容積卡爾曼濾波算法（ACKF）

2.1 自適應(yīng)噪聲協(xié)方差

根據(jù)CKF算法的要求，由于外界環(huán)境的復(fù)雜性，需要已知系統(tǒng)噪聲方差陣和量測(cè)噪聲方差陣。量測(cè)噪聲特性很難準(zhǔn)確確定，即使確定也無(wú)法根據(jù)環(huán)境進(jìn)行自我調(diào)整。因此，本文提出一種根據(jù)協(xié)方差匹配原則，利用新息和殘差序列將CKF算法進(jìn)行自適應(yīng)的改進(jìn)算法。該算法可根據(jù)噪聲統(tǒng)計(jì)特性不斷調(diào)節(jié)，能夠適應(yīng)外界環(huán)境帶來(lái)的干擾，將濾波的精準(zhǔn)度與穩(wěn)定性大幅度提高。具體自適應(yīng)CKF算法步驟如下：

分別定義新息序列與殘差序列如下：

（18）

（19）

Step1 實(shí)時(shí)自適應(yīng)量測(cè)噪聲

由量測(cè)更新過(guò)程可得：

（20）

由新息向量利用開(kāi)窗估計(jì)法可得新息協(xié)方差矩陣為（N為開(kāi)窗大小）：

（21）

同理殘差向量協(xié)方差矩陣為：

（22）

將（21）代入（20）中即可得到量測(cè)噪聲的協(xié)方差矩陣的估計(jì)值：

（23）

Step2 實(shí)時(shí)自適應(yīng)過(guò)程噪聲

將新息序列與殘差序列做差可得：

（24）

將上式與其轉(zhuǎn)置矩陣相乘，并求期望值：

（25）

將（21）、（22）及（8）代入上式：

（26）

2.2 濾波發(fā)散的檢測(cè)

針對(duì)機(jī)動(dòng)目標(biāo)跟蹤實(shí)際問(wèn)題，判斷濾波算法是否發(fā)散，這會(huì)增加運(yùn)算效率，降低運(yùn)算量。當(dāng)發(fā)生濾波發(fā)散情況時(shí)，自適應(yīng)CKF能夠及時(shí)進(jìn)行修正。根據(jù)實(shí)際誤差往往與濾波算法相差較大的特點(diǎn)，本文選用協(xié)方差匹配的方法[9]，對(duì)濾波發(fā)散情況進(jìn)行檢驗(yàn)：

（27）

即：

（28）

由殘差序列來(lái)判斷濾波情況，式中λ≥1為預(yù)先設(shè)置可調(diào)參數(shù)，利用該式來(lái)判斷濾波器是否發(fā)散，即如果不能滿足，則表明實(shí)際誤差將遠(yuǎn)遠(yuǎn)大于預(yù)測(cè)值，此時(shí)需要對(duì)噪聲協(xié)方差進(jìn)行修正。

2.3 自適應(yīng)CKF算法的實(shí)現(xiàn)

針對(duì)離散時(shí)間非線性系統(tǒng)式（1）（2），本文自適應(yīng)CKF算法可實(shí)現(xiàn)對(duì)噪聲統(tǒng)計(jì)特性的預(yù)測(cè)，具體步驟如下：

（1）狀態(tài)初始化：、、、；

（2）由給定初值，根據(jù)式（4）-（12），運(yùn)用容積積分，得到狀態(tài)預(yù)測(cè)值和量測(cè)預(yù)測(cè)值：、。

（3）計(jì)算殘差序列式（19），由式（28）判斷濾波是否發(fā)散，如果發(fā)散進(jìn)入（4），否則進(jìn)入（5）。

（4）運(yùn)行式（23）和（26）對(duì)量測(cè)噪聲協(xié)方差和觀測(cè)噪聲協(xié)方差進(jìn)行更新修正。

（5）根據(jù)式（13）-（15），得到量測(cè)方差與濾波增益，得到狀態(tài)更新值和協(xié)方差陣更新值、，返回（2）實(shí)現(xiàn)循環(huán)。

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

3.1 實(shí)驗(yàn)設(shè)計(jì)

為了更好地驗(yàn)證本算法的有效性，本研究將其應(yīng)用于二維平面內(nèi)雷達(dá)對(duì)空中目標(biāo)的跟蹤上，并選用交互多模型（IMM）[10]估計(jì)器進(jìn)行實(shí)驗(yàn)驗(yàn)證。將仿真時(shí)間設(shè)為100s，整過(guò)機(jī)動(dòng)目標(biāo)運(yùn)動(dòng)過(guò)程為：向水平方向以120m/s的速度自西向東飛行，而后進(jìn)行機(jī)動(dòng)轉(zhuǎn)彎運(yùn)動(dòng)，然后繼續(xù)進(jìn)行勻速運(yùn)動(dòng)，接下來(lái)以-3rad/s進(jìn)行第二次轉(zhuǎn)彎，剩下的時(shí)間以勻速狀態(tài)進(jìn)行飛行。飛行軌跡如圖1所示。

機(jī)動(dòng)目標(biāo)先后以1rad/s和-3rad/s先后進(jìn)行轉(zhuǎn)彎運(yùn)動(dòng)，非線性轉(zhuǎn)彎模型為：

（29）

將雷達(dá)設(shè)定在（10000m，10000m）點(diǎn)處，雷達(dá)的觀測(cè)方程為：

（30）

3.2 參數(shù)設(shè)定

目標(biāo)初始狀態(tài)，，速度，。為了更好地驗(yàn)證算法的有效性，將本算法與標(biāo)準(zhǔn)CKF算法進(jìn)行對(duì)比，引入標(biāo)準(zhǔn)均方根誤差（Root Mean Square Error，RMSE）進(jìn)行比對(duì)，現(xiàn)將位置均方根誤差定義如下：

（31）

其中M為Monte Caerlo的仿真次數(shù)。（，），（，）分別對(duì)應(yīng)于第k時(shí)刻目標(biāo)真實(shí)分量和位置估計(jì)分量，同理可得速度均方根誤差。

3.3 仿真實(shí)驗(yàn)過(guò)程

實(shí)驗(yàn)一：將量測(cè)噪聲特性設(shè)定為定常噪聲，在仿真實(shí)驗(yàn)的過(guò)程中，將實(shí)際噪聲協(xié)方差設(shè)定為初始值的兩倍，將標(biāo)準(zhǔn)CKF算法與自適應(yīng)CKF算法進(jìn)行仿真比較。位置均方根誤差、速度均方根誤差分別如圖2、圖3所示。

實(shí)驗(yàn)二：將量測(cè)噪聲特性設(shè)定為時(shí)變?cè)肼暎诜抡鎸?shí)驗(yàn)過(guò)程中，將開(kāi)始至30s的噪聲設(shè)定為初始值的二倍；當(dāng)目標(biāo)發(fā)生第一次機(jī)動(dòng)后，即達(dá)到30s到65s時(shí)，將量測(cè)噪聲協(xié)方差突變?yōu)槌跏贾档乃谋?；達(dá)到65s到100s時(shí)，將量測(cè)噪聲協(xié)方差突變至初始值的八倍。在實(shí)驗(yàn)過(guò)程中，噪聲統(tǒng)計(jì)特使隨著時(shí)間進(jìn)行不斷地改變。兩種算法的均方根誤差如圖4、圖5所示。

從圖2和圖3中可以看出量測(cè)噪聲對(duì)濾波器的準(zhǔn)確程度存在較大的影響，分析來(lái)看是由于噪聲特性的不確定性，使標(biāo)準(zhǔn)CKF算法不能像本文所提出的自適應(yīng)CKF算法，對(duì)量測(cè)噪聲進(jìn)行實(shí)時(shí)性的調(diào)整，速度和位置產(chǎn)生的誤差遠(yuǎn)大于自適應(yīng)算法。

進(jìn)一步結(jié)合圖4、圖5進(jìn)行分析，當(dāng)機(jī)動(dòng)目標(biāo)發(fā)生高機(jī)動(dòng)運(yùn)動(dòng)，并伴隨著噪聲特性的突變時(shí)，將會(huì)產(chǎn)生更大的誤差，甚至?xí)霈F(xiàn)濾波器發(fā)散問(wèn)題，導(dǎo)致目標(biāo)丟失。但本文所用的自適應(yīng)算法能將量測(cè)噪聲所引起的突變情況進(jìn)行分析，來(lái)改變系統(tǒng)本身量測(cè)噪聲協(xié)方差，有效降低了系統(tǒng)整體誤差，且有良好的穩(wěn)定性，較標(biāo)準(zhǔn)CKF算法更具實(shí)用性，效果更好。

4 結(jié) 論

本文針對(duì)實(shí)際情況中可能出現(xiàn)噪聲特性未知的問(wèn)題，利用協(xié)方差匹配原則對(duì)量測(cè)噪聲協(xié)方差以及系統(tǒng)噪聲協(xié)方差進(jìn)行在線調(diào)整，較標(biāo)準(zhǔn)CKF算法具有更強(qiáng)的適應(yīng)性。自適應(yīng)CKF算法能夠有效降低由于噪聲統(tǒng)計(jì)特性所引起的誤差。空間目標(biāo)跟蹤實(shí)驗(yàn)結(jié)果表明，本文算法濾波精度更高，適應(yīng)能力強(qiáng)，能夠抑制由于噪聲統(tǒng)計(jì)特性位置所引起的濾波發(fā)散問(wèn)題，更具實(shí)用性。

參考文獻(xiàn)：

[1] Einicke G A，White L B. Robust extended Kalman filtering [J].Signal Processing IEEE Transactions on，1999，47（9）：2596-2599.

[2] Julier S，Uhlmann J，Durrant-whyte F H. A new method for the nonlinear transformation of means and covariances in filters and estimators [J].IEEE Trans.on Automat.contr，2000，45（3）：477-482.

[3] Julier S J，Uhlmann J K. Unscented filtering and nonlinear estimation [J]. Proceedings of the IEEE，2004，92（3）：401-422.

[4] Arasaratnam I，Haykin S. Cubature Kalman Filters [J]. IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

[5] Nielsen R O. Accuracy of angle estimation with monopulse processing using two beams [J].Aerospace & Electronic Systems IEEE Transactions on，2001，37（4）：1419-1423.

[6] 于浛，魏喜慶，宋申民，等.基于自適應(yīng)容積卡爾曼濾波的非合作航天器相對(duì)運(yùn)動(dòng)估計(jì) [J].航空學(xué)報(bào)，2014，35（8）：2251-2260.

[7] 丁家琳，肖建.基于極大后驗(yàn)估計(jì)的自適應(yīng)容積卡爾曼濾波器 [J].控制與決策，2014，29（2）：327-334.

[8] Salahshoor K，Mosallaei M，Bayat M. Centralized and decentralized process and sensor fault monitoring using data fusion based on adaptive extended Kalman filter algorithm [J].Measurement，2008，41（10）：1059-1076.

[9] 石勇，韓崇昭.自適應(yīng)UKF算法在目標(biāo)跟蹤中的應(yīng)用 [J].自動(dòng)化學(xué)報(bào)，2011，37（6）：755-759.

[10] 易令.高速高機(jī)動(dòng)目標(biāo)跟蹤算法研究 [D].成都：電子科技大學(xué)，2006.