基于M估計的自適應魯棒平方根連續-離散CKF算法

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

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

在客觀物理世界中大部分動態系統均屬于連續時間系統，其有間隔的測量是離散時間的，因此稱之為連續-離散時間系統[1]。目標跟蹤系統在本質上也是連續-離散系統，傳統算法常采用的離散-離散系統模型會導致額外誤差的產生。連續-離散系統的數學解算方式精度更高，狀態空間描述更精確，有效提高了跟蹤的性能，近幾年來，已廣泛應用于目標跟蹤的相關領域。

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

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

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

1 連續-離散系統目標跟蹤模型和平方根CD-CKF算法

1.1 連續-離散系統目標跟蹤模型

考慮到隨機擾動，連續時間目標運動模型描述為隨機微分方程的形式[3]：

(1)

由隨機狀態的統計學知識，依據狀態期望和協方差進行狀態估計將有利于提高連續-離散濾波算法的精度，其隨機微分方程可這樣表示[7]：

(2)

(3)

離散時間的測量模型與傳統模型類似，表示為：

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

(4)

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

1.2 平方根CD-CKF算法

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

1.2.1 時間更新

在平方根連續-離散容積卡爾曼濾波算法[10]中，狀態容積點的定義如下：

(5)

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

因此，期望和協方差矩陣可以由容積點重新進行表示:

(6)

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

(7)

W和ε的含義如下：

(8)

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

為了方便在容積準則下求解容積點并提高精度，采用文獻[11]提出的高階數值近似方法求解，其中：

(9)

(10)

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

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

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

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

(11)

協方差分解：

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

(12)

計算狀態容積點：

(13)

狀態容積點傳播：

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

(14)

(15)

求解預測平方根協方差：

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

(16)

1.2.2 量測更新

測量過程是離散時間的，時間點k是時間t的離散形式。首先計算狀態容積點：

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

(17)

(18)

容積點的測量傳播：

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

(19)

計算量測預測值：

(20)

構建測量加權中心矩陣：

(21)

計算新息協方差矩陣：

(22)

構建狀態加權中心矩陣：

Xk+1|k=

(23)

計算交叉協方差矩陣：

(24)

連續-離散的容積增益為：

(25)

計算狀態估計值：

(26)

更新協方差矩陣：

(27)

2 基于M估計的自適應魯棒平方根連續-離散CKF算法

考慮濾波過程中可能出現的異常因素會導致測量異常，進而影響濾波性能甚至濾波發散，利用校正因子根據觀測殘差自適應地調整觀測噪聲協方差矩陣的大小，在測量異常時建立魯棒機制，從而保證濾波的精確性。

2.1 校正因子設計

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

(28)

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

校正因子ψk+1可如此設計：

(29)

式中：β由經驗值取為1.345。

2.2 異常測量判別門限

實際環境帶來的測量異常受到多方面因素的影響，測量殘差只能從一定程度上對濾波進行修正。在目標的實際狀態發生突變時，測量殘差也會增大。因此，利用殘差和對應的協方差來判斷異常測量，將會更好地提升濾波效果。引入標準化新息：

(30)

(31)

式中：λk+1表示實測值與預測值的Mahalanobis距離，通過引入新息協方差來衡量統計特性。不妨令標準化新息τk+1服從高斯分布，那么Mahalanobis距離服從卡方分布：

λk+1～χ2(1)

(32)

設卡方分布的置信水平為α，則：

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

(33)

式中：γα為置信水平α的分位點。

當出現異常測量時，λk+1將不再滿足卡方分布。所以只要選擇合適的置信水平，確定相應的分位點的值，就可以區分正常測量和異常測量。將以上的判別方法與校正因子相結合，重新定義校正因子μk+1：

(34)

2.3 魯棒平方根連續-離散CKF算法

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

(35)

相應的連續-離散容積增益為：

(36)

狀態估計值為：

(37)

更新協方差矩陣為：

(38)

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

圖1 基于M估計的自適應魯棒平方根連續-離散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)

接下來，在測量異常的情況下觀察本文提出的算法對目標跟蹤的效果。這里分別模擬了單點測量異常和多點測量異常的情況，為了便于區分，分別記作“異常測試1”、“異常測試2”。異常測試1是指在t=150 s時，測量噪聲的協方差矩陣變為30Rk。異常測試2的異常測量數據及時間點見表1。

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

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

如圖2所示，目標的估計軌跡與真實軌跡在測量異常時發生了一定的變化。可以看到，不具備魯棒性的算法受異常測量的影響較大，估計結果稍差，具備魯棒性的算法其估計軌跡與真實軌跡更為接近。在兩種測試條件下MARSRCD-CKF算法都能夠跟蹤的更加準確，證明本文提出的魯棒算法是真實有效的。

圖2 轉彎運動的軌跡圖

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

圖3 各測試條件下的RMSE

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

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

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

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

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

4 結語

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