時變噪聲統(tǒng)計估計的自適應(yīng)UKF目標(biāo)跟蹤算法

蔡 佳，黃長強，李美亞，齊曉林

(1.空軍工程大學(xué), 陜西 西安 710038；2.西安機(jī)電信息研究所, 陜西 西安 710056)

目標(biāo)跟蹤是為了維持對目標(biāo)當(dāng)前狀態(tài)的估計，同時也是對傳感器接收到的量測信息進(jìn)行處理的過程[1]。但是，傳感器的測量方程很多都是非線性的，需要采用非線性的估計方法。擴(kuò)展卡爾曼濾波(Extended Kalman Filtering，EKF)算法是一種常用的方法[2-4]，但它有兩點缺陷：必須滿足小擾動假設(shè)，只適合弱非線性系統(tǒng)，對于強非線性系統(tǒng)，濾波性能極不穩(wěn)定，甚至發(fā)散；需要計算Jacobian矩陣，計算復(fù)雜，容易出錯。

Julier和Wan等人提出了基于U變換的無跡卡爾曼濾波算法[5-7]。UKF不是像EKF那樣去近似非線性模型，而是對后驗概率密度函數(shù)進(jìn)行近似以得到次優(yōu)的濾波算法，具有不需求導(dǎo)，估計精度高等優(yōu)點。然而，UKF算法需要知道系統(tǒng)模型參數(shù)和噪聲統(tǒng)計特性等先驗知識。如果先驗知識未知或不準(zhǔn)確，濾波器將會產(chǎn)生較大的估計誤差，甚至造成濾波發(fā)散。傳統(tǒng)做法是將未知的噪聲統(tǒng)計特性假設(shè)為1組均值為0、協(xié)方差已知的高斯白噪聲序列，但是零均值假設(shè)并不適于處理目標(biāo)持續(xù)機(jī)動的情形，最終造成濾波發(fā)散。

為此，本文設(shè)計了一種非零均值的時變噪聲統(tǒng)計估計器，引入含有自適應(yīng)衰減因子的加權(quán)系數(shù)，將時變噪聲統(tǒng)計估計與傳統(tǒng)UKF算法相結(jié)合形成一種自適應(yīng)濾波(Adaptive UKF，AUKF)算法。

1 非零均值UKF算法

UKF規(guī)定一組確定性的σ點，當(dāng)狀態(tài)向量的概率密度函數(shù)是高斯型時，利用這組σ點獲取高斯密度函數(shù)的真正均值和協(xié)方差，當(dāng)高斯型狀態(tài)向量的概率密度函數(shù)經(jīng)由非線性系統(tǒng)進(jìn)行傳遞時，對任何一種非線性系統(tǒng)，利用這組σ點能獲取精確到二階的后驗均值和協(xié)方差。

假設(shè)非線性離散系統(tǒng)的狀態(tài)方程和測量方程為：

Xk=f(Xk-1,k-1)+wk-1

(1)

Zk=h(Xk,k)+vk

(2)

式中，過程噪聲wk與測量噪聲vk均為零均值高斯白噪聲序列，分別具有協(xié)方差陣：cov(wk,wl)=Qkδkl,cov(vk,vl)=Rkδkl。初始狀態(tài)X0的統(tǒng)計特性為EX0=x0，varX0=P0，且X0、wk和vk互不相關(guān)。

傳統(tǒng)UKF算法[8-10]通常假定過程噪聲和測量噪聲均值為零，然而在實際情況中，噪聲零均值的條件并不能夠滿足，因此首先要對非線性系統(tǒng)進(jìn)行修正：

Xk=f(Xk-1,k-1)+wk-1+q

(3)

Zk=h(Xk,k)+vk+r

(4)

式中，q和r分別為狀態(tài)方程和測量方程的附加噪聲均值，E(wk-1+q)=q，E(vk+r)=r。

當(dāng)噪聲均值非零時，同樣需要對傳統(tǒng)UKF算法進(jìn)行修正，具體步驟如下：

(5)

(χk/k-1)i=f((χk-1/k-1)i)+q,i=0,1,2,…,2L

(6)

(7)

(8)

(Zk/k-1)i=h((χk/k-1)i)+r,i=0,1,…,2L

(9)

(10)

(11)

(12)

式中，Rk為測量噪聲協(xié)方差矩陣。

(13)

(14)

(15)

2 時變噪聲統(tǒng)計估計的AUKF算法

2.1 噪聲統(tǒng)計估計器

基于極大后驗估計原理[11]和測量值Zk，根據(jù)參考文獻(xiàn)[12-13]，得到如下次優(yōu)MAP噪聲統(tǒng)計估計器：

(16)

(17)

(18)

(19)

上述MAP估計器是針對時變的噪聲統(tǒng)計特性，定常時的噪聲統(tǒng)計估計比較簡單，且估計值已被證明為無偏的[14]。本文考慮時變噪聲統(tǒng)計特性。

(20)

式中，b為遺忘因子，0

2.2 自適應(yīng)UKF算法

將上述時變噪聲統(tǒng)計的估計值代入傳統(tǒng)UKF，即可得到改進(jìn)的自適應(yīng)UKF算法，計算流程如下：

(21)

(22)

(23)

(24)

(25)

(26)

(27)

(28)

由于過程噪聲可以經(jīng)過建模進(jìn)行補償，對模型變化的影響不是很大；而測量噪聲主要是由工作環(huán)境引起的，其不確定性很大，不能進(jìn)行建模補償。因此，實際工作中系統(tǒng)模型的變化主要是由測量噪聲引起的。本文僅對測量噪聲的均值r和方差陣R進(jìn)行估計，暫不考慮過程噪聲的影響。

3 仿真分析

為了驗證本文設(shè)計的自適應(yīng)UKF算法性能，首先做出以下假設(shè)：

1)狀態(tài)方程線性，測量方程非線性。

2)過程噪聲統(tǒng)計特性：均值q=0，方差為Q；測量噪聲統(tǒng)計特性：均值r≠0，方差為R。

根據(jù)以上假設(shè)條件，則系統(tǒng)模型為：

Xk=Φk,k-1Xk-1+wk-1

(29)

Zk=h(Xk,k)+vk+r

(30)

3.1 參數(shù)設(shè)置

初始狀態(tài)設(shè)為：x0=[1 000 3 000 20 50 3 -4]T，P0=diag[100 100 1 1 0.1 0.1]。

時變噪聲的初始統(tǒng)計特性為：

Q0=diag[1 1 0.120.120.0120.012]，

采樣時間0.1 s，仿真時間100 s，對本文提出的AUKF算法進(jìn)行100次Monte Carlo仿真實驗。

3.2 結(jié)果分析

仿真結(jié)果的評價指標(biāo)選為狀態(tài)向量的均方根誤差(RMSE)，定義為：

(31)

AUKF算法的跟蹤效果如圖1所示，可見估計值與目標(biāo)的真實軌跡基本重合，這說明采用本文的自適應(yīng)濾波算法可以實現(xiàn)對目標(biāo)的有效跟蹤。

圖2～4所示為AUKF算法的濾波結(jié)果及與EKF算法和傳統(tǒng)UKF算法的誤差比較。

圖2至圖4所示為笛卡爾坐標(biāo)系下，目標(biāo)在x和y方向的位置、速度和加速度均方根誤差比較。從仿真結(jié)果圖對比看出，EKF算法和傳統(tǒng)UKF 算法的狀態(tài)估計結(jié)果均不及AUKF，尤其是EKF算法的均方根誤差更大，這是因為EKF本身忽略了高階截斷誤差，對噪聲統(tǒng)計特性估計不準(zhǔn)確造成的。本文提出的AUKF 算法可以不斷地實時估計時變噪聲的統(tǒng)計特性，根據(jù)量測信息對濾波發(fā)散趨勢進(jìn)行判斷，并引入指數(shù)加權(quán)的衰減因子抑制濾波發(fā)散，因此能夠有效地解決由于噪聲統(tǒng)計特性未知或不準(zhǔn)確而引起的濾波精度不高的問題，大大提高了收斂速度和估計精度。

表1 結(jié)合本文選取的評價指標(biāo)-狀態(tài)向量各個元素的均方根誤差，比較了3種算法的濾波性能。仿真結(jié)果表明，AUKF 算法在濾波精度方面明顯優(yōu)于EKF算法和傳統(tǒng)UKF算法，因此，狀態(tài)估計也更接近于真實狀態(tài)，目標(biāo)跟蹤性能更優(yōu)。

表1 算法性能比較(均方根誤差)

4 結(jié)束語

對于目標(biāo)跟蹤問題，傳統(tǒng)UKF算法在噪聲先驗知識未知時將其均值假設(shè)為零，這種處理方法容易造成濾波發(fā)散。為此，本文在對系統(tǒng)模型和傳統(tǒng)濾波算法修正的同時，設(shè)計了一種時變噪聲統(tǒng)計估計器，并將其引入到傳統(tǒng)UKF算法當(dāng)中，提出了一種自適應(yīng)UKF算法，該算法可以在濾波計算的同時，對噪聲的未知先驗信息進(jìn)行實時統(tǒng)計估計和修正。仿真結(jié)果表明，該算法在保證濾波收斂的同時提高了跟蹤精度。

[1] 韓崇昭, 朱洪艷, 段戰(zhàn)勝. 多源信息融合[M]. 北京: 清華大學(xué)出版社, 2006: 124-188.

HAN Chong-zhao, ZHU Hong-yan, DUAN Zhan-sheng. Multi-source information fusion[M]. Beijing: Tsinghua University Press, 2006: 124-188. (in Chinese)

[2] EINICKE G A, WHITE L B. Robust extended Kalman filtering[J]. IEEE Transactions on Signal Processing, 1999, 47(9): 2596-2599.

[3] 秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998: 182-188.

QIN Yong-yuan, ZHANG Hong-yue, WANG Shu-hua. Kalman filtering and integrated navigation theory[M]. Xi’an: Northwestern Polytechnical University Press, 1998: 182-188. (in Chinese)

[4] JOSEPH J, LAVIOLA Jr. A comparison of unscented and extended Kalman filtering for estimating quaternion motion[C]//Proceedings of the American Control Conference. Denver:[s.n.],2003: 2435-2440.

[5] JULIER S J. The scaled unscented transformation[C]//Proceedings of the American Control Conference.Anchorage:[s.n.],2002: 4555-4559.

[6] JULIER S J, UHLMANN J K. Unscented filtering and nonlinear estimation[J].Proceedings of the IEEE, 2004, 92(3): 401-422.

[7] WAN E A, VAN DER MERVE R. The unscented Kalman filter for nonlinear estimation[C]//Proceedings of the IEEE Adaptive Systems for Signal Processing, Communication and Control Symposium.Lake Louise:[s.n.],2000:153-158.

[8] 楊凱, 倪龍強, 張麗華, 等. 基于IMM-UKF的非線性機(jī)動目標(biāo)跟蹤仿真研究[J]. 火炮發(fā)射與控制學(xué)報, 2010(3): 12-16.

YAN Kai, NI Long-qiang, ZHANG Li-hua, et al. Study on tracking simulation of nonlinear maneuvering target based on IMM-UKF[J]. Journal of Gun Launch & Control, 2010(3): 12-16. (in Chinese)

[9] 江寶安, 萬群. 基于UKF-IMM的雙紅外機(jī)動目標(biāo)跟蹤算法[J]. 系統(tǒng)工程與電子技術(shù), 2008,30(8):1454-1459.

JIANG Bao-an, WAN Qun. Maneuvering target passive tracking with dual infrared observers using IMM algorithm based on UKF[J]. Systems Engineering and Electronics, 2008, 30(8): 1454-1459. (in Chinese)

[10] 張文玲, 朱明清, 陳宗海. 基于強跟蹤UKF的自適應(yīng)SLAM算法[J]. 機(jī)器人, 2010, 32(2): 190-195.

ZHANG Wen-ling, ZHU Ming-qing, CHEN Zong-hai. An adaptive SLAM algorithm based on strong tracking UKF[J]. Robot, 2010, 32(2): 190-195. (in Chinese)

[11] SAGE A P, HUSA G W. Adaptive filtering with unknown prior statistics[C].Tokyo: Proceedings of the Joint Automatic Control Conference, 1969, 760-769.

[12] 石勇, 韓崇昭. 自適應(yīng)UKF算法在目標(biāo)跟蹤中的應(yīng)用[J]. 自動化學(xué)報, 2011, 37(6): 755-759.

SHI Yong, HAN Chong-zhao. Adaptive UKF method with applications to target tracking[J]. Acta Automatica Sinica, 2011, 37(6): 755-759. (in Chinese)

[13] 孫堯, 張強, 萬磊. 基于自適應(yīng)UKF算法的小型水下機(jī)器人導(dǎo)航系統(tǒng)[J]. 自動化學(xué)報, 2011,37(3):342-353.

SUN Yao, ZHANG Qiang, WAN Lei. Small autonomous underwater vehicle navigation system based on adaptive UKF algorithm[J]. Acta Automatica Sinica, 2011, 37(3): 342-353. (in Chinese)

[14] 趙琳, 王小旭, 薛紅香, 等. 帶噪聲統(tǒng)計估計器的Unscented卡爾曼濾波器設(shè)計[J]. 控制與決策, 2009, 24(10): 1483-1488.

ZHAO Lin, WANG Xiao-xu, XUE Hong-xiang, et al. Design of unscented Kalman filter with noise statistic estimator[J]. Control and Decision, 2009, 24(10): 1483-1488. (in Chinese)