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

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

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

在客觀物理世界中大部分動(dòng)態(tài)系統(tǒng)均屬于連續(xù)時(shí)間系統(tǒng)，其有間隔的測(cè)量是離散時(shí)間的，因此稱之為連續(xù)-離散時(shí)間系統(tǒng)[1]。目標(biāo)跟蹤系統(tǒng)在本質(zhì)上也是連續(xù)-離散系統(tǒng)，傳統(tǒng)算法常采用的離散-離散系統(tǒng)模型會(huì)導(dǎo)致額外誤差的產(chǎn)生。連續(xù)-離散系統(tǒng)的數(shù)學(xué)解算方式精度更高，狀態(tài)空間描述更精確，有效提高了跟蹤的性能，近幾年來(lá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ù)時(shí)間模型以隨機(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)估計(jì)的精度，高階的數(shù)值近似方法開(kāi)始被用于求解隨機(jī)微分方程。文獻(xiàn)[4]將基于非線性隨機(jī)微分方程的連續(xù)模型描述為其期望和方差的模型形式，進(jìn)而用高階數(shù)值近似方法對(duì)連續(xù)模型進(jìn)行了求解。在文獻(xiàn)[5]中，基于嵌套式的隱式龍格-庫(kù)塔法用于解決連續(xù)-離散擴(kuò)展卡爾曼濾波(continuous-discrete extended kalman filter，CD-EKF)問(wèn)題，其思想在連續(xù)-離散容積卡爾曼濾波中的應(yīng)用在文獻(xiàn)[6]中有所體現(xiàn)。文獻(xiàn)[7]用自適應(yīng)步長(zhǎng)對(duì)精度完成了進(jìn)一步的控制。文獻(xiàn)[8]提出的自適應(yīng)反饋算法對(duì)協(xié)方差矩陣進(jìn)行預(yù)測(cè)，從而減輕了不可預(yù)知誤差的影響，進(jìn)一步提高了濾波的性能和效率。

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

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

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

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

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

(1)

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

(2)

(3)

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

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

(4)

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

1.2 平方根CD-CKF算法

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

1.2.1 時(shí)間更新

在平方根連續(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)

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

(13)

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

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

(14)

(15)

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

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

(16)

1.2.2 量測(cè)更新

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

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

(17)

(18)

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

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

(19)

計(jì)算量測(cè)預(yù)測(cè)值：

(20)

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

(21)

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

(22)

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

Xk+1|k=

(23)

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

(24)

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

(25)

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

(26)

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

(27)

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

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

2.1 校正因子設(shè)計(jì)

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

(28)

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

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

(29)

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

2.2 異常測(cè)量判別門限

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

(30)

(31)

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

λk+1～χ2(1)

(32)

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

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

(33)

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

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

(34)

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

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

(35)

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

(36)

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

(37)

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

(38)

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

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

3 仿真與分析

離散時(shí)間的非線性測(cè)量模型為：

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

(39)

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

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

(40)

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

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

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

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

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

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

圖3 各測(cè)試條件下的RMSE

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

圖4 異常測(cè)試條件1下的誤差對(duì)比

圖5 異常測(cè)試條件2下的誤差對(duì)比

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

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

4 結(jié)語(yǔ)

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