胡浩然， 陳樹新， 吳 昊， 何仁珂， 汪家寶， 郝思沖

(1.空軍工程大學(xué)信息與導(dǎo)航學(xué)院， 西安， 710077； 2.93184部隊， 北京， 100076)

在客觀物理世界中大部分動態(tài)系統(tǒng)均屬于連續(xù)時間系統(tǒng)，其有間隔的測量是離散時間的，因此稱之為連續(xù)-離散時間系統(tǒng)[1]。目標(biāo)跟蹤系統(tǒng)在本質(zhì)上也是連續(xù)-離散系統(tǒng)，傳統(tǒng)算法常采用的離散-離散系統(tǒng)模型會導(dǎo)致額外誤差的產(chǎn)生。連續(xù)-離散系統(tǒng)的數(shù)學(xué)解算方式精度更高，狀態(tài)空間描述更精確，有效提高了跟蹤的性能，近幾年來，已廣泛應(yīng)用于目標(biāo)跟蹤的相關(guān)領(lǐng)域。

為進(jìn)一步提升連續(xù)-離散的濾波精度，文獻(xiàn)[2]提出了基于容積準(zhǔn)則的連續(xù)-離散容積卡爾曼濾波(continuous-discrete cubature kalman filter，CD-CKF)。其中，連續(xù)時間模型以隨機(jī)微分方程(stochastic differential equation，SDE)表示，隨機(jī)微分方程的求解是連續(xù)-離散方法區(qū)別于離散-離散方法的重要體現(xiàn)。文獻(xiàn)[3]總結(jié)了基于0.5階Euler-Maruyama方法和It-Taylor方法[2]，認(rèn)為其具有較高的精確度。為進(jìn)一步提高狀態(tài)估計的精度，高階的數(shù)值近似方法開始被用于求解隨機(jī)微分方程。文獻(xiàn)[4]將基于非線性隨機(jī)微分方程的連續(xù)模型描述為其期望和方差的模型形式，進(jìn)而用高階數(shù)值近似方法對連續(xù)模型進(jìn)行了求解。在文獻(xiàn)[5]中，基于嵌套式的隱式龍格-庫塔法用于解決連續(xù)-離散擴(kuò)展卡爾曼濾波(continuous-discrete extended kalman filter，CD-EKF)問題，其思想在連續(xù)-離散容積卡爾曼濾波中的應(yīng)用在文獻(xiàn)[6]中有所體現(xiàn)。文獻(xiàn)[7]用自適應(yīng)步長對精度完成了進(jìn)一步的控制。文獻(xiàn)[8]提出的自適應(yīng)反饋算法對協(xié)方差矩陣進(jìn)行預(yù)測，從而減輕了不可預(yù)知誤差的影響，進(jìn)一步提高了濾波的性能和效率。

以上的目標(biāo)跟蹤系統(tǒng)建立在理想條件下，但實(shí)際中各種異常條件影響著濾波的性能，模型改變、信號遮擋、信號干擾等都會導(dǎo)致觀測站的測量異常。在文獻(xiàn)[9]中，針對模型的不確定性提出一種基于魯棒的CD-EKF方法，從殘差范數(shù)的平方和最優(yōu)的角度出發(fā)來進(jìn)行優(yōu)化，成功建立了魯棒機(jī)制。但大部分魯棒方法中將非線性問題線性化處理，引入了截斷誤差，進(jìn)而導(dǎo)致精度的下降。如何應(yīng)對實(shí)際問題中非線性的異常測量問題已經(jīng)成為了研究熱點(diǎn)。

本文針對異常測量，設(shè)計了一種具有魯棒機(jī)制的非線性連續(xù)-離散跟蹤算法。

1 連續(xù)-離散系統(tǒng)目標(biāo)跟蹤模型和平方根CD-CKF算法

1.1 連續(xù)-離散系統(tǒng)目標(biāo)跟蹤模型

考慮到隨機(jī)擾動，連續(xù)時間目標(biāo)運(yùn)動模型描述為隨機(jī)微分方程的形式[3]：

(1)

由隨機(jī)狀態(tài)的統(tǒng)計學(xué)知識，依據(jù)狀態(tài)期望和協(xié)方差進(jìn)行狀態(tài)估計將有利于提高連續(xù)-離散濾波算法的精度，其隨機(jī)微分方程可這樣表示[7]：

(2)

(3)

離散時間的測量模型與傳統(tǒng)模型類似，表示為：

Zk=h(x(tk))+vk,k=1,2,…,N

(4)

式中：Zk是實(shí)際測量值；h為觀測函數(shù)；假設(shè)vk滿足零均值的高斯分布，即：vk～N(0,Rk)；k指離散的時間點(diǎn)；xk指x(tk)在時間tk處的狀態(tài)值。

1.2 平方根CD-CKF算法

CKF算法利用容積數(shù)值積分來逼近狀態(tài)后驗(yàn)分布，數(shù)值精度較高，穩(wěn)定性較好。為保證誤差協(xié)方差矩陣的正定性和對稱性，這里介紹連續(xù)-離散容積卡爾曼濾波的平方根形式。與典型的CKF方法相似，將連續(xù)-離散與容積準(zhǔn)則相結(jié)合，同樣分為時間更新與測量更新兩個步驟。

1.2.1 時間更新

在平方根連續(xù)-離散容積卡爾曼濾波算法[10]中，狀態(tài)容積點(diǎn)的定義如下：

(5)

式中：S(t)是協(xié)方差矩陣P(t)的下三角矩陣，滿足P(t)=S(t)ST(t)。

因此，期望和協(xié)方差矩陣可以由容積點(diǎn)重新進(jìn)行表示:

(6)

P′(t)=X(t)WFT(X(t))XT(t)+Q(t)

(7)

W和ε的含義如下：

(8)

式中：I2n表示維數(shù)為2n的單位矩陣；1是單位列向量；?代表直積。

為了方便在容積準(zhǔn)則下求解容積點(diǎn)并提高精度，采用文獻(xiàn)[11]提出的高階數(shù)值近似方法求解，其中：

(9)

(10)

式中：Bi,j是Φ(B(t))的第i行、第j列的元素。式(10)可在Matlab里使用ode45命令直接求解，其精度符合要求。

由容積準(zhǔn)則，矩陣B(t)可表示為：

B(t)=S-1(t)[X(t)WFT(X(t))+

F(X(t))XT(t)+Q(t)]S-T(t)

(11)

協(xié)方差分解：

P(tk)=S(tk)ST(tk)

(12)

計算狀態(tài)容積點(diǎn)：

(13)

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

S′(t)=S(t)Φ(B(t))

(14)

(15)

求解預(yù)測平方根協(xié)方差：

S(tk+1)=[S1(tk+1)S2(tk+1) …S2n(tk+1)]

(16)

1.2.2 量測更新

測量過程是離散時間的，時間點(diǎn)k是時間t的離散形式。首先計算狀態(tài)容積點(diǎn)：

Sk+1|k=S(tk+1)

(17)

(18)

容積點(diǎn)的測量傳播：

Zi,k+1|k=h(Xi,k+1|k,k+1)

(19)

計算量測預(yù)測值：

(20)

構(gòu)建測量加權(quán)中心矩陣：

(21)

計算新息協(xié)方差矩陣：

(22)

構(gòu)建狀態(tài)加權(quán)中心矩陣：

Xk+1|k=

(23)

計算交叉協(xié)方差矩陣：

(24)

連續(xù)-離散的容積增益為：

(25)

計算狀態(tài)估計值：

(26)

更新協(xié)方差矩陣：

(27)

2 基于M估計的自適應(yīng)魯棒平方根連續(xù)-離散CKF算法

考慮濾波過程中可能出現(xiàn)的異常因素會導(dǎo)致測量異常，進(jìn)而影響濾波性能甚至濾波發(fā)散，利用校正因子根據(jù)觀測殘差自適應(yīng)地調(diào)整觀測噪聲協(xié)方差矩陣的大小，在測量異常時建立魯棒機(jī)制，從而保證濾波的精確性。

2.1 校正因子設(shè)計

由觀測殘差建立無量綱數(shù)Гk+1：

(28)

式中：Rk+1為觀測誤差協(xié)方差矩陣；δ是一個比較小的正實(shí)數(shù)，本文取0.01。

校正因子ψk+1可如此設(shè)計：

(29)

式中：β由經(jīng)驗(yàn)值取為1.345。

2.2 異常測量判別門限

實(shí)際環(huán)境帶來的測量異常受到多方面因素的影響，測量殘差只能從一定程度上對濾波進(jìn)行修正。在目標(biāo)的實(shí)際狀態(tài)發(fā)生突變時，測量殘差也會增大。因此，利用殘差和對應(yīng)的協(xié)方差來判斷異常測量，將會更好地提升濾波效果。引入標(biāo)準(zhǔn)化新息：

(30)

(31)

式中：λk+1表示實(shí)測值與預(yù)測值的Mahalanobis距離，通過引入新息協(xié)方差來衡量統(tǒng)計特性。不妨令標(biāo)準(zhǔn)化新息τk+1服從高斯分布，那么Mahalanobis距離服從卡方分布：

λk+1～χ2(1)

(32)

設(shè)卡方分布的置信水平為α，則：

p(λk+1>γα)=1-α

(33)

式中：γα為置信水平α的分位點(diǎn)。

當(dāng)出現(xiàn)異常測量時，λk+1將不再滿足卡方分布。所以只要選擇合適的置信水平，確定相應(yīng)的分位點(diǎn)的值，就可以區(qū)分正常測量和異常測量。將以上的判別方法與校正因子相結(jié)合，重新定義校正因子μk+1：

(34)

2.3 魯棒平方根連續(xù)-離散CKF算法

基于以上內(nèi)容，形成了一種具有魯棒機(jī)制的平方根連續(xù)-離散CKF方法。其時間更新過程與平方根連續(xù)-離散CKF方法一致，在測量更新中，式(18)～(24)步驟不變，而后新息協(xié)方差矩陣被重新定義：

(35)

相應(yīng)的連續(xù)-離散容積增益為：

(36)

狀態(tài)估計值為：

(37)

更新協(xié)方差矩陣為：

(38)

可知，通過Mahalanobis距離對異常測量進(jìn)行門限判別，引入校正因子自適應(yīng)地調(diào)整觀測噪聲協(xié)方差陣的大小，基于連續(xù)-離散方法跟蹤目標(biāo)實(shí)際運(yùn)動。該算法在時間更新上使用連續(xù)時間狀態(tài)估計思想，在測量更新上嵌入了異常測量判別和修正方案。根據(jù)以上分析，具體的算法流程圖見圖1。

圖1 基于M估計的自適應(yīng)魯棒平方根連續(xù)-離散CKF算法流程圖

3 仿真與分析

離散時間的非線性測量模型為：

zk=[tan-1(y(tk)/x(tk))]+vk

(39)

測量噪聲vk滿足零均值的高斯分布，即：vk～N(0,Rk)。本次仿真中Rk的初始值為一維常值0.01。

為了評估濾波的性能，定義位置量的均方根誤差(root mean square error，RMSE)。它的計算方法是：

(40)

接下來，在測量異常的情況下觀察本文提出的算法對目標(biāo)跟蹤的效果。這里分別模擬了單點(diǎn)測量異常和多點(diǎn)測量異常的情況，為了便于區(qū)分，分別記作“異常測試1”、“異常測試2”。異常測試1是指在t=150 s時，測量噪聲的協(xié)方差矩陣變?yōu)?0Rk。異常測試2的異常測量數(shù)據(jù)及時間點(diǎn)見表1。

表1 異常測試2測量噪聲的協(xié)方差矩陣

首先，對本文提出算法的有效性進(jìn)行驗(yàn)證。在異常測試條件下使用不具有魯棒性的SRCD-CKF算法進(jìn)行仿真，魯棒性測試使用本文提出的MARSRCD-CKF算法。仿真得到的目標(biāo)軌跡跟蹤見圖2。

如圖2所示，目標(biāo)的估計軌跡與真實(shí)軌跡在測量異常時發(fā)生了一定的變化?？梢钥吹剑痪邆漪敯粜缘乃惴ㄊ墚惓y量的影響較大，估計結(jié)果稍差，具備魯棒性的算法其估計軌跡與真實(shí)軌跡更為接近。在兩種測試條件下MARSRCD-CKF算法都能夠跟蹤的更加準(zhǔn)確，證明本文提出的魯棒算法是真實(shí)有效的。

圖2 轉(zhuǎn)彎運(yùn)動的軌跡圖

為了更清晰地觀察異常測試點(diǎn)的誤差情況，分析其對濾波性能的影響，做出RMSE隨時間的變化曲線見圖3?？梢钥吹皆诟鳒y試條件下濾波都具有良好的收斂性。在t=150 s時，測量異常帶來的影響直觀表現(xiàn)為RMSE的增加，異常測試2亦是如此。異常測試條件下曲線出現(xiàn)比較陡峭的變化，相應(yīng)的其再次收斂也比較緩慢；具有魯棒性的算法受到異常測量影響時變化緩慢，很快重新收斂，且最終估計誤差的大小基本與無異常情況下相似。兩種魯棒性能的測試幾乎一致，說明不論單個測量點(diǎn)異常，還是多個測量點(diǎn)異常，算法都是有效的。此算法能夠解決目標(biāo)跟蹤過程中的異常測量問題。

圖3 各測試條件下的RMSE

為進(jìn)一步評估本文提出的濾波算法的性能，分別在異常測試1和異常測試2下與SRCD-CKF算法和MRSRCD-CKF算法作對比分析，見圖4～5。

圖4 異常測試條件1下的誤差對比

圖5 異常測試條件2下的誤差對比

如圖4所示，各算法在異常測量出現(xiàn)前走勢大致相同。在第150 s出現(xiàn)異常測量后，不具備魯棒性的SRCD-CKF算法誤差突然增大，而MRSRCD-CKF算法與MARSRCD-CKF算法都可以抑制異常測量。第150 s之后，MARSRCD-CKF算法比MRSRCD-CKF算法的誤差更小，說明本文提出的魯棒方法在單個測量點(diǎn)異常的條件下比MRSRCD-CKF算法要更有效。

在圖5中，在設(shè)置的各異常測量點(diǎn)處，不具備魯棒性的SRCD-CKF算法的估計誤差均出現(xiàn)了突然增大的現(xiàn)象，MRSRCD-CKF算法與MARSRCD-CKF算法都有效抑制了異常測量，只有小幅度的波動，并且MARSRCD-CKF算法比MRSRCD-CKF算法的誤差更小，說明本文提出的魯棒方法在多個測量點(diǎn)異常的條件下比MRSRCD-CKF算法要更有效。

4 結(jié)語

為了能夠應(yīng)對測量中出現(xiàn)的異常情況，本文提出了一種基于M估計的自適應(yīng)魯棒平方根連續(xù)-離散CKF算法。假設(shè)在一處或者多處測量點(diǎn)處出現(xiàn)異常測量，通過觀測殘差自適應(yīng)地調(diào)整觀測噪聲協(xié)方差矩陣的大小，建立了相應(yīng)的校正因子，而后以Mahalanobis距離作為判別門限，對新息協(xié)方差矩陣進(jìn)行更新，進(jìn)而與連續(xù)-離散容積卡爾曼濾波結(jié)合形成了MARCD-CKF算法。為進(jìn)一步提升濾波的精度以及保證協(xié)方差矩陣的正定性，本文采用了連續(xù)-離散容積卡爾曼濾波的平方根(SRCD-CKF)形式。仿真表明，該算法能夠有效限制異常測量的影響，并且比傳統(tǒng)魯棒算法更有效，在目標(biāo)跟蹤系統(tǒng)中優(yōu)勢更為明顯。