高 燾，崔威威，袁劍華，王善民

(1. 海軍駐武漢四六一廠軍事代表室， 武漢 430000；2. 中國(guó)船舶重工集團(tuán)公司第七二四研究所，南京 211153)

基于目標(biāo)狀態(tài)的自適應(yīng)卡爾曼濾波器

高 燾1，崔威威2，袁劍華2，王善民2

(1. 海軍駐武漢四六一廠軍事代表室， 武漢 430000；2. 中國(guó)船舶重工集團(tuán)公司第七二四研究所，南京 211153)

針對(duì)過(guò)程誤差難以確定、觀測(cè)精度較差、目標(biāo)位置較遠(yuǎn)時(shí)卡爾曼濾波性能下降的情況，結(jié)合目標(biāo)當(dāng)前運(yùn)動(dòng)狀態(tài)，定義了基于當(dāng)前觀測(cè)值置信度的權(quán)重矩陣，推導(dǎo)了基于權(quán)重矩陣的修正卡爾曼濾波觀測(cè)更新方程，實(shí)現(xiàn)目標(biāo)狀態(tài)協(xié)方差矩陣、增益矩陣快速自適應(yīng)調(diào)整，提高了算法穩(wěn)健性。實(shí)驗(yàn)驗(yàn)證了算法的可行性和改進(jìn)性。

目標(biāo)跟蹤；卡爾曼濾波；自適應(yīng)濾波

0 引 言

目標(biāo)數(shù)據(jù)處理是自動(dòng)控制、航空/航天、雷達(dá)數(shù)據(jù)處理的重要研究課題[1]，研究對(duì)象是存在隨機(jī)干擾的觀測(cè)數(shù)據(jù)。因具備無(wú)偏最小方差估計(jì)以及線性遞推的優(yōu)點(diǎn)，卡爾曼濾波器在目標(biāo)跟蹤數(shù)據(jù)處理領(lǐng)域得到了廣泛的應(yīng)用。卡爾曼濾波器面臨的主要困難包括難以獲得目標(biāo)運(yùn)動(dòng)的先驗(yàn)信息，觀測(cè)噪聲會(huì)隨著環(huán)境、天氣和設(shè)備狀態(tài)不同而變化，過(guò)程噪聲也需要進(jìn)行相應(yīng)的調(diào)整。但是，對(duì)過(guò)程噪聲進(jìn)行精確的分析的算法計(jì)算復(fù)雜[2]，實(shí)踐中假定過(guò)程噪聲和觀測(cè)噪聲是互不相關(guān)的，并使用有界噪聲模型。[3-5]

在過(guò)程噪聲模型難以確定情況下，存在卡爾曼濾波器發(fā)散、濾波性能下降等問(wèn)題，尤其在觀測(cè)精度不高、目標(biāo)速度低條件下，錯(cuò)誤的過(guò)程模型會(huì)嚴(yán)重影響目標(biāo)狀態(tài)估計(jì)?？柭鼮V波器獲得相當(dāng)廣泛的研究和改進(jìn)以增強(qiáng)濾波穩(wěn)健性和自適應(yīng)性。王文正等人給出了一類(lèi)表征過(guò)程噪聲模型優(yōu)劣的簡(jiǎn)便算法[6]，但依賴最小上界的選取，難以做到自適應(yīng)。周宏仁基于最大最小加速度假設(shè)提出了當(dāng)前統(tǒng)計(jì)模型對(duì)目標(biāo)的狀態(tài)協(xié)方差矩陣進(jìn)行調(diào)整，增強(qiáng)了目標(biāo)機(jī)動(dòng)情況下目標(biāo)跟蹤的能力。[7]朱洪艷基于交互多模型算法結(jié)合多掃描觀測(cè)信息，采用離散優(yōu)化技術(shù)，給出了近似于實(shí)際過(guò)程噪聲水平的最優(yōu)子集模型集。[8]劉建書(shū)基于何衍等人的工作，結(jié)合模糊推理系統(tǒng)給出過(guò)程噪聲的調(diào)整算法，提升了對(duì)目標(biāo)的跟蹤能力。[9]田俊林等人基于周東華提出的強(qiáng)跟蹤濾波器[10]，引入了時(shí)變漸消因子來(lái)嘗試修正過(guò)程噪聲以及狀態(tài)協(xié)方差矩陣，拓展了卡爾曼濾波器在非定軌目標(biāo)跟蹤的應(yīng)用。[11]相關(guān)作者深入研究的成果為本文提供了研究思路和參照依據(jù)。

1 標(biāo)準(zhǔn)卡爾曼濾波器

標(biāo)準(zhǔn)卡爾曼濾波器的二維空間目標(biāo)運(yùn)動(dòng)方程和觀測(cè)方程分別是

xk=Fk-1xk-1+wk

yk=Hkxk+νk

其中，xk=(Xx,vxk,Xy,vyk)表示k時(shí)刻目標(biāo)的位x、y坐標(biāo)及其速度；wk是零均值白噪聲過(guò)程的誤差序列，其方差矩陣是Qk；vk是零均值白噪聲序列，其方差矩陣為Rk；xk的協(xié)方差矩陣記為Pk；Fk-1、Hk、Kk表示轉(zhuǎn)移矩陣、觀測(cè)矩陣和增益矩陣。

根據(jù)上述的運(yùn)動(dòng)模型和觀測(cè)模型，卡爾曼濾波算法可以描述如下：

(1) 時(shí)間更新

(2) 觀測(cè)更新

-、+分別表示時(shí)間更新結(jié)果和觀測(cè)更新結(jié)果，對(duì)于觀測(cè)的目標(biāo)狀態(tài)更新，可以進(jìn)一步寫(xiě)為

2 基于當(dāng)前狀態(tài)的卡爾曼濾波器

卡爾曼濾波器的增益Kk將當(dāng)前觀測(cè)誤差、過(guò)程誤差引入當(dāng)前目標(biāo)狀態(tài)估計(jì)值，決定了濾波器的帶寬和反應(yīng)速度以及濾波效果。較小的卡爾曼濾波器增益能夠?qū)Ψ菣C(jī)動(dòng)狀態(tài)起到良好的降噪作用，而較大的卡爾曼增益能夠?qū)δ繕?biāo)機(jī)動(dòng)做出快速反應(yīng)。標(biāo)準(zhǔn)卡爾曼濾波器中，過(guò)程噪聲和觀測(cè)噪聲對(duì)當(dāng)前狀態(tài)估計(jì)作出了共同的貢獻(xiàn)，在觀測(cè)精度較差、目標(biāo)位置較遠(yuǎn)以及噪聲模型和真實(shí)情況存在偏差等情況下會(huì)出現(xiàn)濾波能力下降。

在觀測(cè)誤差較差、難以準(zhǔn)確獲得過(guò)程噪聲的信息情況下，將觀測(cè)先驗(yàn)信息、目標(biāo)運(yùn)動(dòng)后驗(yàn)信息結(jié)合起來(lái)，通過(guò)定義基于置信度的權(quán)重矩陣，實(shí)時(shí)調(diào)整增益矩陣、目標(biāo)狀態(tài)協(xié)方差矩陣，有助于降低由觀測(cè)噪聲引起的目標(biāo)狀態(tài)起伏，增強(qiáng)卡爾曼濾波的穩(wěn)健性和自適應(yīng)性：

其中Bk被定義為由觀測(cè)信息和目標(biāo)運(yùn)動(dòng)后驗(yàn)信息共同決定的權(quán)重矩陣。該矩陣描述了目標(biāo)觀測(cè)值和目標(biāo)狀態(tài)之間的匹配程度。特別針對(duì)雷達(dá)觀測(cè)目標(biāo)，觀測(cè)值(方位、距離)服從正態(tài)分布，因此利用外推位置和觀測(cè)值之間的方位、距離差計(jì)算關(guān)聯(lián)點(diǎn)和外推點(diǎn)之間的置信度λk，此處定義權(quán)重矩陣Bk=λkI，也可以使用其他方式權(quán)重矩陣計(jì)算方式：

式中，σa、σd分別表示方位、距離誤差均方根，λa、λd表示目標(biāo)外推和觀測(cè)位置的方位、距離差。使用取小是為了避免Bk=λkI矩陣值太小，產(chǎn)生濾波失敗的情況?；谛拚龣?quán)重矩陣Bk進(jìn)一步推導(dǎo)出噪聲協(xié)方差矩陣、增益矩陣的計(jì)算。

由協(xié)方差的定義可知：

得到

考慮到增益矩陣Kk是使代價(jià)函數(shù)取得最小值的最優(yōu)解，通過(guò)對(duì)矩陣進(jìn)行微分得到Kk的推導(dǎo)公式：

使上式為0得到

代入狀態(tài)更新公式：

在實(shí)驗(yàn)中，為簡(jiǎn)化問(wèn)題以及降低計(jì)算復(fù)雜度，考慮Bk=λkI，可知計(jì)算過(guò)程中使用λk等同于Bk，因此使用λk替代Bk：

考慮使用目標(biāo)濾波前后的狀態(tài)變化作為過(guò)程誤差的近似形式在試驗(yàn)中使用如下的狀態(tài)補(bǔ)償公式，即使用目標(biāo)的外推位置和濾波位置距離差作為補(bǔ)償因子。

3 仿真驗(yàn)證

本文考慮低速條件下的兩種運(yùn)動(dòng)方式：直線運(yùn)動(dòng)和圓周運(yùn)動(dòng)。對(duì)處于勻速直線運(yùn)動(dòng)狀態(tài)的目標(biāo)進(jìn)行濾波試驗(yàn)分析，能夠檢驗(yàn)濾波器的收斂和平滑性能。對(duì)處于圓周運(yùn)動(dòng)狀態(tài)的目標(biāo)進(jìn)行濾波試驗(yàn)分析，能夠檢驗(yàn)濾波器對(duì)機(jī)動(dòng)狀態(tài)的反應(yīng)速度。設(shè)置雷達(dá)的方位距、離觀測(cè)精度分別是0.2°和50 m，設(shè)置一個(gè)目標(biāo)以5 m/s的速度，從坐標(biāo)(60 000,70 000)(單位：m)的位置向240°方向勻速運(yùn)動(dòng)300 s后，開(kāi)始以w=2°s-1法向加速度(對(duì)應(yīng)的法向加速度an=0.1745 ms-2)保持做圓周運(yùn)動(dòng)500 s，繼續(xù)保持直線運(yùn)動(dòng)200 s，進(jìn)行1000次蒙特卡洛仿真實(shí)驗(yàn)，得到圖1的濾波結(jié)果。圖中左側(cè)表示改進(jìn)后的濾波結(jié)果，右側(cè)表示標(biāo)準(zhǔn)卡爾曼濾波結(jié)果。

圖1 目標(biāo)跟蹤全局

從圖1可以看出，改進(jìn)后的算法濾波位置抖動(dòng)較小，而標(biāo)準(zhǔn)卡爾曼濾波抖動(dòng)較大。在前面若干周期內(nèi)，在觀測(cè)精度較差條件下，目標(biāo)的速度難以準(zhǔn)確建立起來(lái)，需要大約30個(gè)周期才能獲得較為準(zhǔn)確的當(dāng)前目標(biāo)速度，在目標(biāo)速度估計(jì)中存在相應(yīng)的反映。本文考慮目標(biāo)的位置和速度作為分析指標(biāo)。首先給出直觀的統(tǒng)計(jì)結(jié)果，如圖2、3所示。

圖2 XY坐標(biāo)跟蹤誤差

圖3 XY速度濾波精度

從圖2、3可以看出，改進(jìn)后的算法誤差分布范圍較小，較為集中，說(shuō)明算法濾波結(jié)果有一定的提升。按照目標(biāo)的運(yùn)動(dòng)特征，將運(yùn)動(dòng)過(guò)程分為3部分：運(yùn)動(dòng)全程、直線運(yùn)動(dòng)階段和圓周運(yùn)動(dòng)階段。3個(gè)運(yùn)動(dòng)階段指標(biāo)的蒙特卡洛仿真統(tǒng)計(jì)值如以下3個(gè)表所示。

表1 全過(guò)運(yùn)動(dòng)程跟蹤精度表

表2 直線運(yùn)動(dòng)階段跟蹤精度表

表3 圓周運(yùn)動(dòng)階段跟蹤精度表

從上面3個(gè)表格看出，標(biāo)準(zhǔn)卡爾曼濾波器在3種情況下得到的指標(biāo)值近似，改進(jìn)后的算法在圓周運(yùn)動(dòng)時(shí)指標(biāo)略有下降，除了距離均方根誤差外其余指標(biāo)均有所提升，具體表現(xiàn)為指標(biāo)的均方根誤差小于標(biāo)準(zhǔn)卡爾曼濾波器的均方根誤差，尤其是目標(biāo)的速度和XY坐標(biāo)濾波。

實(shí)驗(yàn)分析表明，當(dāng)前狀態(tài)估計(jì)增強(qiáng)了卡爾曼濾波器的自適應(yīng)性，提高了卡爾曼濾波器的濾波效果，同時(shí)沒(méi)有花費(fèi)更多的計(jì)算資源。

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

針對(duì)卡爾曼濾波系統(tǒng)在噪聲模型不匹配，以及處理觀測(cè)精度較差、距離較遠(yuǎn)的低速目標(biāo)時(shí)跟蹤性能下降，基于觀測(cè)信息和目標(biāo)運(yùn)動(dòng)后驗(yàn)信息給出了一類(lèi)新算法，該算法能夠根據(jù)觀測(cè)值自動(dòng)調(diào)整增益矩陣以及協(xié)方差矩陣，增強(qiáng)了算法的穩(wěn)健性，同時(shí)取得了較標(biāo)準(zhǔn)卡爾曼濾波算法更好的跟蹤效果。但是，該算法存在目標(biāo)機(jī)動(dòng)時(shí)跟蹤指標(biāo)存在下降的情況，下一步會(huì)針對(duì)提高目標(biāo)在高速機(jī)動(dòng)時(shí)的跟蹤性能做進(jìn)一步的研究。

[1] Dan Simon. 最優(yōu)狀態(tài)估計(jì)[M]. 北京：國(guó)防工業(yè)出版社, 2012: 88-100.

[2] 陳嘉鴻, 韓九強(qiáng), 席震東, 等. 一般相關(guān)噪聲下多傳感器平滑融合算法[J]. 自動(dòng)化學(xué)報(bào), 2009,35(5):491-497.

[3] 符安迪, 朱云民. 過(guò)程噪聲和量測(cè)噪聲多步相關(guān)的卡爾曼型濾波[J]. 四川大學(xué)學(xué)報(bào)(自然科學(xué)版), 2009, 46(5): 1237-1240.

[4] 李昇平. 過(guò)程噪聲未知但有界情況下系統(tǒng)最優(yōu)濾波其設(shè)計(jì)方法[J]. 電子學(xué)報(bào), 2004,32(6): 1050-1053.

[5] 周振威, 方海濤. 在不準(zhǔn)確方差下帶隨即系數(shù)矩陣的卡爾曼濾波穩(wěn)定性[J]. 自動(dòng)化學(xué)報(bào), 2013,39(1):43-52.

[6] 王文正. 基于最小噪聲確定模型的結(jié)構(gòu)[J]. 自動(dòng)化學(xué)報(bào), 1999, 25(2): 1-4.

[7] 周宏仁. 機(jī)動(dòng)目標(biāo)當(dāng)前統(tǒng)計(jì)模型與自適應(yīng)算法[J]. 航空學(xué)報(bào), 4(3):73-86.

[8] 朱洪艷, 韓紅, 韓崇昭, 等. 一種改進(jìn)的多模型噪聲識(shí)別方法[J]. 系統(tǒng)仿真學(xué)報(bào), 2003,15(6): 800-803.

[9] 劉建書(shū), 何亞娟, 王小永, 等. 模糊自適應(yīng)機(jī)動(dòng)目標(biāo)跟蹤算法[J]. 彈箭與制導(dǎo)學(xué)報(bào), 2010.8, 30(4)：8-9.

[10] 田俊林, 付承毓, 唐濤, 等. 過(guò)程噪聲方差實(shí)時(shí)補(bǔ)償?shù)馁M(fèi)定軌目標(biāo)跟蹤[J]. 光電工程, 2012，39(1)：68-72.

[11] 周東華, 王慶林. 有色噪聲干擾的非線性系統(tǒng)前跟蹤濾波[J]. 北京理工大學(xué)學(xué)報(bào), 1997, 17(3)：321-326.

Adaptive Kalman filter based on target status

GAO Tao1, CUI Wei-wei2, YUAN Jian-hua2, WANG Shan-min2

(1.Military Representatives Office of the PLA Navy in No. 461 Factory in Wuhan, Wuhan 430000;2. No. 724 Research Institute of CSIC, Nanjing 211153)

In view of the difficulty of determining the process error, the poor observation accuracy and the performance degradation of the Kalman filtering when the target position is relatively far, in combination with the current target movement status, the weight matrix is defined based on the current confidence coefficient of the observation value, and the updated observation equations of the modified Kalman filtering are derived based on the weight matrix, realizing the covariance matrix of the target status and rapid adaptive adjustment of the gain matrix, and improving the robustness of the algorithm. The feasibility and improvement of the algorithm are verified via the test.

target tracking; Kalman filtering; adaptive filtering

TN713

A

1009-0401(2017)04-0011-04

2017-10-11；

2017-10-20

高燾(1983-)，男，工程師，研究方向：船舶制造、船舶特種裝備儀器；崔威威(1987-),男，工程師，碩士，研究方向：雷達(dá)數(shù)據(jù)處理；袁劍華(1984-),男，高級(jí)工程師，碩士，研究方向：雷達(dá)系統(tǒng)應(yīng)用處理；王善民(1986-)，工程師，碩士，研究方向：雷達(dá)數(shù)據(jù)處理。