未知噪聲協方差的自適應容積卡爾曼濾波

楊恒占， 王盛博， 何紅麗

(1.西安工業大學陜西省自主系統與智能控制國際聯合研究中心，西安，710021； 2.中國飛行試驗研究院，西安，710089)

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

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

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

1 非線性估計的CKF算法

考慮如下非線性離散系統：

(1)

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

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

(2)

1.1 時間更新

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

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

i=1,2,…,2n

(3)

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

計算非線性狀態方程傳播容積點。

(4)

計算狀態估計值并估計誤差協方差矩陣。

(5)

1.2 量測更新

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

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

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

(6)

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

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

(7)

計算量測預測、量測預測協方差矩陣并估計互協方差矩陣。

i=1,2,…,2n

(8)

計算增益矩陣、狀態更新值和協方差陣更新值。

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

(9)

2 新型自適應CKF濾波

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

2.1 自適應Q估計

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

(10)

定理1對于式(1)給定的系統，在時間k處，新息與殘差之間的互相關函數為：

(11)

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

(12)

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

(13)

當過程噪聲和量測噪聲不相關時，有:

(14)

(15)

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

(16)

證明完畢。

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

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

(17)

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

(18)

(19)

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

(20)

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

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

(21)

進一步得到

(22)

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

2.2 自適應R估計

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

(23)

可以進一步引入加權因子δ對Rk進行調整，測量噪聲協方差可以更新為：

(24)

式中：0

2.3 新型自適應CKF濾波算法流程

根據上述方法，未知時變噪聲協方差下的目標跟蹤自適應CKF濾波算法流程如下：

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

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

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

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

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

3 仿真案例及結果分析

在某地地導彈試驗基地中，采用雷達對目標進行跟蹤，系統的模型如下：

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

zk=h(xk)+vk

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

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

圖1 三維彈道軌跡

圖2 二維彈道軌跡

圖3 目標量測與真值對比

圖4 X方向位置誤差

圖5 Y方向位置誤差

圖6 Z方向位置誤差

圖7 X方向速度誤差

圖8 Y方向速度誤差

圖9 Z方向速度誤差

圖10 標準差

圖11 位置均方差

表1 兩種算法RMSE均值比較

4 結語

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