楊恒占， 王盛博， 何紅麗

(1.西安工業(yè)大學(xué)陜西省自主系統(tǒng)與智能控制國際聯(lián)合研究中心，西安，710021； 2.中國飛行試驗研究院，西安，710089)

非線性濾波問題應(yīng)用于目標(biāo)跟蹤，由于其廣泛的軍用及民用特點，是近年來研究的熱點問題[1]。目標(biāo)跟蹤的主要任務(wù)是通過從傳感觀測中提取有價值的信息來估計動態(tài)目標(biāo)參數(shù)并顯示目標(biāo)的軌跡，其跟蹤精度在很大程度上取決于濾波算法的性能。雖然測量噪聲的統(tǒng)計特性可以從傳感器中得到，但其中包含了外部干擾導(dǎo)致得到的特性不可靠。由于外部隨機干擾的影響，很難得到具有精確統(tǒng)計協(xié)方差的系統(tǒng)噪聲[2]。同時在機動目標(biāo)跟蹤系統(tǒng)中涉及到時變噪聲協(xié)方差，傳統(tǒng)的濾波算法無法滿足要求[3]。因此，為了處理噪聲協(xié)方差的不確定性，需要高性能的自適應(yīng)濾波算法。

容積卡爾曼濾波(Cubature Kalman Filter，簡稱CKF)是一種新型高斯濾波算法，為非線性系統(tǒng)的狀態(tài)估計提供新的實現(xiàn)方式，利用球面—徑面積分準(zhǔn)則計算非線性變換后隨機變量的均值和協(xié)方差，采用一組等權(quán)值的容積點對最優(yōu)狀態(tài)的后驗分布進行逼近。由于CKF將采樣點都設(shè)置在超球面上，不存在權(quán)重為負的情況，相比其它非線性濾波器具有更高的非線性逼近性能和估計精度，并且運算速度也得到了提高[4]。CKF中目標(biāo)跟蹤的精度主要依賴于對噪聲統(tǒng)計特性的研究。然而噪聲統(tǒng)計特性在實際情況下大多為非零均值、非白噪聲，非線性系統(tǒng)又往往受到外界不確定因素的干擾，這些都會影響濾波結(jié)果。為了處理這種未知噪聲，許多研究者對自適應(yīng)濾波算法進行改進。丁家琳等[5]研究者采用基于Sage-Husa噪聲估計器的自適應(yīng)濾波器，在未知噪聲特性的情況下，很大程度上提高了濾波精度，但巨大的運算量導(dǎo)致其在實際工程中很難應(yīng)用。在常用估計方法中可以看到，狀態(tài)估計時刻影響著新息的測量噪聲統(tǒng)計特性，因此，估計誤差使得測量噪聲協(xié)方差在計算過程中產(chǎn)生誤差，進而導(dǎo)致濾波器性能下降。Zhang等[6]研究者針對這一問題研究了一種基于測量的自適應(yīng)卡爾曼濾波算法(MAKF)，克服了針對導(dǎo)航系統(tǒng)改進的Sage-Husa自適應(yīng)濾波器的不穩(wěn)定問題，但是，MAKF只有當(dāng)一個測量噪聲協(xié)方差小于另一個時才有效。

本文針對未知時變噪聲協(xié)方差的系統(tǒng)，提出了一種新的非線性跟蹤系統(tǒng)自適應(yīng)CKF方法，該方法避免了兩種噪聲之間的相互作用。首先根據(jù)新息序列與殘差序列的相關(guān)性，推導(dǎo)出一種新的過程噪聲協(xié)方差Q估計方法；然后采用殘差序列對測量噪聲協(xié)方差進行估計，其次采用加權(quán)因子，得到當(dāng)前時刻更新后的噪聲協(xié)方差，利用更新后的噪聲協(xié)方差矩陣來校正狀態(tài)估計，有效避免了不準(zhǔn)確狀態(tài)估計的缺陷。仿真結(jié)果表明，未知噪聲條件下跟蹤精度有了明顯的提高。

1 非線性估計的CKF算法

考慮如下非線性離散系統(tǒng)：

(1)

式中：xk∈Rn為系統(tǒng)的狀態(tài)向量；zk∈Rm為系統(tǒng)的量測輸出向量；f(·)和h(·)分別為非線性系統(tǒng)的轉(zhuǎn)移函數(shù)和量測函數(shù)；wk-1∈Rn為系統(tǒng)噪聲，vk∈Rn為量測噪聲，wk-1和vk均為高斯白噪聲且相互獨立；協(xié)方差矩陣為Qk和Rk。

容積卡爾曼濾波算法將非線性濾波轉(zhuǎn)換為求解非線性函數(shù)與高斯概率密度乘積的積分問題，然后利用2n個容積點進行加權(quán)求和對狀態(tài)后驗概率密度進行逼近。對于任意分布函數(shù)ρ(x)利用容積積分準(zhǔn)則求解積分問題，使用容積準(zhǔn)則獲得的基本容積點和所對應(yīng)的權(quán)值如下：

(2)

1.1 時間更新

1)利用Cholesky方法對協(xié)方差進行分解并進行容積點的計算。

Sk-1/k-1=Chol(Pk-1/k-1)

i=1,2,…,2n

(3)

其中，Sk-1/k-1為協(xié)方差矩陣平方根。

計算非線性狀態(tài)方程傳播容積點。

(4)

計算狀態(tài)估計值并估計誤差協(xié)方差矩陣。

(5)

1.2 量測更新

對容積點進行更新，通過量測方程傳播容積點，預(yù)測觀測向量及協(xié)方差。

利用Cholesky方法對誤差協(xié)方差進行分解并對容積點進行更新。

Sk/k-1=Chol(Pk/k-1)

(6)

計算非線性量測方程傳播的容積點。

Zi,k/k-1=h(Xi,k/k-1)

(7)

計算量測預(yù)測、量測預(yù)測協(xié)方差矩陣并估計互協(xié)方差矩陣。

i=1,2,…,2n

(8)

計算增益矩陣、狀態(tài)更新值和協(xié)方差陣更新值。

Kk=Pxz,k/k-1(Pxz,k/k-1)T

(9)

2 新型自適應(yīng)CKF濾波

在導(dǎo)彈軌跡的測試試驗中，受各種因素的影響，量測數(shù)據(jù)和真實數(shù)據(jù)偏差較大，導(dǎo)致目標(biāo)跟蹤的精度下降。針對這一問題，本文通過估計過程噪聲協(xié)方差Q和測量噪聲協(xié)方差R，進而改進CKF濾波算法，提高目標(biāo)跟蹤的精度。

2.1 自適應(yīng)Q估計

在卡爾曼濾波理論中，新息序列和殘差序列可以定義為[7]：

(10)

定理1對于式(1)給定的系統(tǒng)，在時間k處，新息與殘差之間的互相關(guān)函數(shù)為：

(11)

證明：將式(1)中的量測方程代入方程(10)中，新息序列和殘差序列可以表示成：

(12)

根據(jù)式(10)新息序列和殘差序列在時間k處的互相關(guān)函數(shù)可以表示為：

(13)

當(dāng)過程噪聲和量測噪聲不相關(guān)時，有:

(14)

(15)

將式(14)和式(15)代入式(13)中得到新息與殘差的互相關(guān)函數(shù)：

(16)

證明完畢。

利用新息序列和殘差序列提高Q估計的魯棒性，對新息序列與殘差序列的差求期望:

E[(ηk-εk)(ηk-εk)T]=

(17)

根據(jù)式(12)和式(15)有新息序列的期望和殘差序列的期望:

(18)

(19)

進一步，根據(jù)定理1和式(18)～(19)確定新息序列和殘差序列差的協(xié)方差，可以得到:

(20)

將式(5)代入式(20)中，可以表示為：

E[(ηk-εk)(ηk-εk)T]=

(21)

進一步得到

(22)

在雷達跟蹤系統(tǒng)中Hk/k-1的秩一般大于Qk-1的未知數(shù)，因此可以得到唯一的過程噪聲協(xié)方差Qk-1。此外，Qk-1一般為對角矩陣，計算比較簡單。

2.2 自適應(yīng)R估計

基于殘差序列對測量噪聲協(xié)方差Rk進行估計，測量噪聲協(xié)方差Rk可以估計為[8]：

(23)

可以進一步引入加權(quán)因子δ對Rk進行調(diào)整，測量噪聲協(xié)方差可以更新為：

(24)

式中：0

2.3 新型自適應(yīng)CKF濾波算法流程

根據(jù)上述方法，未知時變噪聲協(xié)方差下的目標(biāo)跟蹤自適應(yīng)CKF濾波算法流程如下：

Step2：利用式(3)和式(4)對協(xié)方差分解并進行傳播容積點的計算；

Step3：利用式(22)更新過程噪聲協(xié)方差Qk-1；

Step4：利用式(5)估計誤差噪聲協(xié)方差Pk/k-1；

Step5：利用式(24)更新測量噪聲協(xié)方差Rk；

Step7：對于下一時間段，重復(fù)Step1～7。

3 仿真案例及結(jié)果分析

在某地地導(dǎo)彈試驗基地中，采用雷達對目標(biāo)進行跟蹤，系統(tǒng)的模型如下：

xk=f(xk-1)+wk-1

zk=h(xk)+vk

其中，f(xk-1)=Fxk-1，

在上述模型下，采用MATLAB進行驗證，導(dǎo)彈飛行的真實軌跡和在濾波算法下的三維軌跡見圖1。圖2為導(dǎo)彈飛行的二維軌跡，可以看到本文的自適應(yīng)CKF濾波算法精度高于自適應(yīng)UKF濾波算法。圖3將真實數(shù)據(jù)和測量數(shù)據(jù)進行對比。圖4～圖9分別表示了X方向、Y方向和Z方向的位置誤差和速度誤差。圖10和圖11分別采用標(biāo)準(zhǔn)差和均方誤差對兩種濾波算法進行比較，雖然在有的時間段自適應(yīng)UKF精度略高于本文的自適應(yīng)CKF算法，通過表1對兩種算法的RMSE均值進行比較，數(shù)據(jù)顯示本文的算法精度更高。

圖1 三維彈道軌跡

圖2 二維彈道軌跡

圖3 目標(biāo)量測與真值對比

圖4 X方向位置誤差

圖5 Y方向位置誤差

圖6 Z方向位置誤差

圖7 X方向速度誤差

圖8 Y方向速度誤差

圖9 Z方向速度誤差

圖10 標(biāo)準(zhǔn)差

圖11 位置均方差

表1 兩種算法RMSE均值比較

4 結(jié)語

本文針對目標(biāo)跟蹤過程中未知噪聲協(xié)方差問題提出了自適應(yīng)CKF濾波方法。首先通過新息序列和殘差序列導(dǎo)出的線性矩陣方程實時處理過程噪聲協(xié)方差Q；然后利用殘差序列對噪聲協(xié)方差進行估計并引入加權(quán)因子將測量噪聲協(xié)方差矩陣與估計值組成稱為新的測量噪聲協(xié)方差陣，文中對自適應(yīng)UKF和自適應(yīng)CKF兩種濾波算法進行位置誤差和速度誤差對比。仿真結(jié)果表明，在時變噪聲協(xié)方差條件下，所提出的自適應(yīng)CKF濾波精度更高。本文提出的新型CKF濾波算法可以應(yīng)用到任意非線性系統(tǒng)，具有較好的普適性及應(yīng)用價值。