吳青坡,叢源材,周紹磊

(1 92313部隊(duì),河南濟(jì)源 459000;2 92886部隊(duì),山東膠州 266300;3 海軍航空工程學(xué)院,山東煙臺(tái) 264001)

非線性確定采樣型濾波器采樣點(diǎn)優(yōu)化算法*

吳青坡1,3,叢源材2,周紹磊3

(1 92313部隊(duì),河南濟(jì)源 459000;2 92886部隊(duì),山東膠州 266300;3 海軍航空工程學(xué)院,山東煙臺(tái) 264001)

采樣點(diǎn)的選取對(duì)確定采樣型濾波器的精度和穩(wěn)定性起著決定性作用,為提高濾波器的數(shù)值穩(wěn)定性,首先,采用協(xié)方差矩陣分解方法對(duì)獲得的采樣點(diǎn)進(jìn)行優(yōu)化,解決了數(shù)值大小對(duì)采樣點(diǎn)離采樣中心距離的影響問(wèn)題;其次,通過(guò)選擇合適的正交變換,在不改變?yōu)V波精度的前提下消除了采樣點(diǎn)中存在的非局部效應(yīng);最后,利用一個(gè)落體跟蹤模型對(duì)所提基于采樣點(diǎn)優(yōu)化方法的確定采樣型濾波器進(jìn)行仿真分析,仿真結(jié)果驗(yàn)證了該采樣點(diǎn)優(yōu)化方法的有效性。

確定采樣型濾波器;采樣點(diǎn)優(yōu)化;協(xié)方差矩陣分解;正交變換

0 引言

對(duì)于線性高斯系統(tǒng),Kalman濾波即是最優(yōu)濾波器,而對(duì)于非線性系統(tǒng),人們主要采用擴(kuò)展Kalman濾波器(extended Kalman filter,EKF)及其相關(guān)改進(jìn)算法等次優(yōu)解的近似方法。但EKF存在精度偏低、需計(jì)算Jacobian矩陣、要求非線性函數(shù)連續(xù)可微等理論局限性,系統(tǒng)具有強(qiáng)非線性和高維數(shù)時(shí)數(shù)值穩(wěn)定性較差,濾波精度不佳[1]。確定采樣型濾波因具有實(shí)現(xiàn)簡(jiǎn)單、估計(jì)精度高等優(yōu)點(diǎn),目前已得到國(guó)內(nèi)外學(xué)者的廣泛關(guān)注[2-6]。

確定采樣型濾波器在應(yīng)用過(guò)程中出現(xiàn)濾波發(fā)散或失敗的情況,主要是由于在協(xié)方差矩陣的計(jì)算中出現(xiàn)了非正定的協(xié)方差矩陣和非局部效應(yīng)導(dǎo)致的協(xié)方差矩陣過(guò)大[7],Julier提出了對(duì)采樣點(diǎn)進(jìn)行比例修正的方法,提供了3個(gè)自由參數(shù)來(lái)調(diào)整采樣點(diǎn)到采樣中心的距離和權(quán)值,但并沒(méi)給出參數(shù)調(diào)整的具體方法。Merwe[8]提出在濾波過(guò)程中采用協(xié)方差矩陣的平方根矩陣代替協(xié)方差矩陣,然而導(dǎo)致協(xié)方差矩陣非正定的因素并沒(méi)有消除。Lefebvre[9]提出增加一個(gè)權(quán)值為1的中心采樣點(diǎn)來(lái)增大協(xié)方差矩陣從而保證協(xié)方差矩陣半正定,但這會(huì)大大降低濾波器的精度。Xiong[10]通過(guò)人為增大系統(tǒng)噪聲矩陣來(lái)增加濾波器的穩(wěn)定性,保證了協(xié)方差矩陣的半正定性,卻以犧牲精度為代價(jià)來(lái)提高穩(wěn)定性。Arasaratnam[6]提出基于球面徑向容積法的CKF(cubature Kalman filter),其獲得采樣點(diǎn)的方法為實(shí)現(xiàn)高精度的采樣點(diǎn)提供了理論基礎(chǔ)。CKF解決了UKF(unscented Kalman filter)中采樣點(diǎn)權(quán)值為負(fù)的問(wèn)題,同時(shí)也帶來(lái)了非局部效應(yīng),限制了其在高維系統(tǒng)中的應(yīng)用。Chang[11]巧妙地構(gòu)建了一個(gè)正交矩陣,消除了非局部效應(yīng)。

為提高確定采樣型濾波器的數(shù)值穩(wěn)定性,避免濾波發(fā)散,文中從協(xié)方差矩陣分解的角度研究了提高濾波器穩(wěn)定性的方法。采用基于相關(guān)系數(shù)分解的方法獲得平方根矩陣,可有效消除濾波采樣點(diǎn)中由于協(xié)方差矩陣中元素?cái)?shù)值大小而產(chǎn)生的遠(yuǎn)離采樣中心的問(wèn)題。利用正交變換方法,在不會(huì)改變?yōu)V波精度的前提下消除了采樣點(diǎn)中存在的非局部效應(yīng)。

1 確定采樣型濾波器

給定系統(tǒng)狀態(tài)方程和量測(cè)方程

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

(1)

yk+1=h(xk+1)+vk+1

(2)

式中:xk∈Rn為n維狀態(tài)向量;yk∈Rm為m維量測(cè)向量;系統(tǒng)噪聲wk和量測(cè)噪聲vk均為高斯白噪聲,協(xié)方差矩陣分別為Qk和Rk;wk和vk相互獨(dú)立,且與狀態(tài)量和量測(cè)量不相關(guān)。

確定采樣型濾波器在非線性高斯濾波框架下通過(guò)對(duì)一二階矩近似實(shí)現(xiàn)狀態(tài)估計(jì),過(guò)程如下[3-5]:

2)狀態(tài)傳遞方程

χi=f(σi)

3)量測(cè)更新方程

ξi=h(χi)

2 協(xié)方差矩陣分解

這里采用基于相關(guān)系數(shù)矩陣分解的新方法對(duì)協(xié)方差矩陣進(jìn)行分解。

2.1 協(xié)方差矩陣中的信息提取

方差代表隨機(jī)變量相對(duì)其數(shù)學(xué)期望的分散程度,通常當(dāng)隨機(jī)變量的取值離數(shù)學(xué)期望越遠(yuǎn)則出現(xiàn)的概率越小,與之對(duì)應(yīng)的是采樣點(diǎn)離中心點(diǎn)越遠(yuǎn),其權(quán)值越小。在確定采樣中,權(quán)值相同的基礎(chǔ)采樣點(diǎn)離中心點(diǎn)的距離相同,但經(jīng)過(guò)線性變換后,距離會(huì)發(fā)生變化,這是造成濾波器誤差的一個(gè)重要原因。標(biāo)準(zhǔn)差代表了一個(gè)隨機(jī)變量偏離其數(shù)學(xué)期望的平均距離,如果將采樣點(diǎn)看作是等權(quán)值的,那么超出標(biāo)準(zhǔn)差越遠(yuǎn)的點(diǎn)出現(xiàn)的概率越小。因此,在協(xié)方差矩陣分解時(shí),應(yīng)當(dāng)盡可能減小這一變化的影響。

由于隨機(jī)向量中各分量標(biāo)準(zhǔn)差大小不同,不同分量間協(xié)方差的大小無(wú)法準(zhǔn)確描述相關(guān)程度的大小,對(duì)協(xié)方差進(jìn)行標(biāo)準(zhǔn)化得到相關(guān)系數(shù)ρij為:

ρij描述了狀態(tài)變量xi和xj之間的線性相關(guān)程度,0≤ρij≤1,ρij越大表明xi和xj之間的線性相關(guān)程度越強(qiáng),顯然,xi和其自身呈線性關(guān)系,即ρii=1。

為了消除標(biāo)準(zhǔn)差的影響,將協(xié)方差矩陣P分解為對(duì)角矩陣D和相關(guān)系數(shù)矩陣C。

P=DCDT

(3)

式中:D為由隨機(jī)向量中各分量的標(biāo)準(zhǔn)差,即P的對(duì)角線上的元素的正平方根組成的對(duì)角矩陣:

由相關(guān)系數(shù)的定義ρij=ρji,C為對(duì)稱(chēng)矩陣。

相關(guān)系數(shù)矩陣為無(wú)量綱矩陣,其中各個(gè)元素的數(shù)值大小真正地反應(yīng)了它們的重要性,即相關(guān)程度。

2.2 基于相關(guān)系數(shù)分解的平方根矩陣

根據(jù)式(3),只需要對(duì)C進(jìn)行分解就可以得到P的平方根矩陣。相關(guān)系數(shù)矩陣C應(yīng)當(dāng)是正定對(duì)稱(chēng)矩陣,可采用cholesky分解來(lái)獲得其平方根矩陣。由于cholesky分解計(jì)算量比較小,目前在確定采樣濾波器中通常采用該方法。但濾波過(guò)程中存在的各種誤差都可能導(dǎo)致正定的條件不能滿(mǎn)足,從而出現(xiàn)濾波失敗的情況。另外一種可用于獲得平方根矩陣的矩陣分解方法是奇異值分解,該方法對(duì)矩陣的正定性沒(méi)有要求,因此采用奇異值分解可以在矩陣出現(xiàn)非正定的情況下依然能獲得平方根矩陣。

對(duì)n×n維的相關(guān)系數(shù)矩陣C進(jìn)行奇異值分解得:

C=UΛUT

(4)

式中:U為n×n維正交矩陣;Λ為由C的奇異值為對(duì)角線元素的n×n維對(duì)角矩陣。U的列向量組成矩陣C的列向量所在空間的標(biāo)準(zhǔn)正交基。奇異值λi滿(mǎn)足λ1≥λ2≥…≥λn。奇異值的大小表明了在該奇異值對(duì)應(yīng)的基向量上所包含的矩陣C的信息量的大小,即在各分量相關(guān)性較高的方向上的點(diǎn)離中心點(diǎn)的距離遠(yuǎn)。這一特點(diǎn)能更好地體現(xiàn)出采樣點(diǎn)的差異性。

由式(3)和式(4)得協(xié)方差矩陣的平方根矩陣為:

(5)

將狀態(tài)向量的標(biāo)準(zhǔn)差投影到Ui上,Ui為單位向量,得到狀態(tài)向量在該方向上的散布程度DUi,因此保證了每個(gè)分量的散布程度不超過(guò)其標(biāo)準(zhǔn)差。對(duì)相關(guān)系數(shù)矩陣C進(jìn)行奇異值分解的結(jié)果使得采樣點(diǎn)能夠更準(zhǔn)確地描述隨機(jī)狀態(tài)向量的統(tǒng)計(jì)特性。

3 正交變換下采樣點(diǎn)的優(yōu)化

正交變換利用平方根矩陣對(duì)基礎(chǔ)采樣點(diǎn)的線性變換可以獲得濾波所需的采樣點(diǎn),而對(duì)平方根矩陣的正交變換不會(huì)影響采樣點(diǎn)的分布特性。因此,可以在不改變?yōu)V波器精度的前提下,通過(guò)選擇合適的正交變換解決采樣點(diǎn)中存在的非局部效應(yīng)。

給定正交矩陣T=[T1;T2;…;Tn][11],其中:

Tk=(βk,1,βk,2,…,βk,n)T,k=1,2,…,n

CKF的基礎(chǔ)采樣點(diǎn)可表示為[6]:

在正交變換γ=Tσ下進(jìn)行采樣點(diǎn)優(yōu)化,得到新的基礎(chǔ)采樣點(diǎn):

qk=(qk,1,qk,2,…,qk,n)T,k=1,2,…,n

當(dāng)n為奇數(shù)時(shí),qk,n=(-1)k。由此得到的基礎(chǔ)采樣點(diǎn)γ將消除非局部效應(yīng)。

i=1,2,…,n(n-1)/2

{si}={(ek+el),k

{ti}={(ek-el),k

在正交變換T下該基礎(chǔ)采樣點(diǎn)變?yōu)?

i=1,2,…,n(n-1)/2

4 仿真分析

采用落體跟蹤算例[2]對(duì)不同確定型濾波器及采樣點(diǎn)優(yōu)化后對(duì)濾波結(jié)果的影響進(jìn)行驗(yàn)證,這里選擇UKF進(jìn)行對(duì)比。如圖1所示,假設(shè)一個(gè)從空中下落的物體,其高度是x1,速度為x2,常彈道系數(shù)為x3。測(cè)量裝置的高度為H,與落體的水平距離為M。

圖1 垂直落體跟蹤幾何圖

系統(tǒng)方程為:

其中:wi為過(guò)程噪聲;v為量測(cè)噪聲;ρ0為海平面空氣密度;k為描述空氣密度和高度之間關(guān)系的常數(shù);g是重力加速度。采用連續(xù)時(shí)間方程來(lái)描述系統(tǒng),假定獲得量測(cè)的周期為0.5 s。系統(tǒng)初始條件和估計(jì)給定如下:

文獻(xiàn)[2]采用該模型驗(yàn)證了UKF比EKF具有更好的估計(jì)效果。這里只對(duì)新濾波算法和UKF作比較。在相同運(yùn)行環(huán)境下,進(jìn)行50次Monte Carlo仿真,各狀態(tài)量的絕對(duì)估計(jì)誤差平均值如圖2～圖4所示。

從仿真結(jié)果看,在前10 s,落體高度和速度估計(jì)誤差逐漸增大,10 s前后估計(jì)誤差達(dá)到峰值,這是由于加速度發(fā)生了較大的變化,呈現(xiàn)出非常強(qiáng)的非線性所導(dǎo)致的,與文獻(xiàn)[12]中提到的局部方法可以處理的強(qiáng)非線性問(wèn)題只是相對(duì)的觀點(diǎn)相吻合。10 s過(guò)后,估計(jì)誤差迅速減小并趨于穩(wěn)定。

改進(jìn)方法與UKF對(duì)落體高度和速度的估計(jì)效果相當(dāng),但其在前幾秒對(duì)落體速度的估計(jì)中新濾波算法要好于UKF,且對(duì)彈道系數(shù)的估計(jì)要明顯好于UKF。由于彈道系數(shù)直接影響加速度,故前幾秒中,改進(jìn)方法對(duì)速度的估計(jì)誤差也小于UKF。這是由于彈道系數(shù)的數(shù)值遠(yuǎn)小于高度和速度,對(duì)協(xié)方差矩陣直接進(jìn)行分解,會(huì)忽略數(shù)值較小的分量,產(chǎn)生較大的誤差。新濾波方法將協(xié)方差矩陣分解為標(biāo)準(zhǔn)差對(duì)角陣和相關(guān)系數(shù)矩陣兩部分,對(duì)相關(guān)系數(shù)矩陣進(jìn)行奇異值分解,由于相關(guān)系數(shù)是無(wú)量綱的,因此消除了數(shù)值大小的影響。在這一算例中,彈道系數(shù)的估計(jì)誤差對(duì)其他狀態(tài)量的影響較小,對(duì)系統(tǒng)的穩(wěn)定性影響較小。但如果數(shù)值小的分量對(duì)系統(tǒng)穩(wěn)定性影響較大,那么很可能會(huì)導(dǎo)致濾波發(fā)散,因此,改進(jìn)方法在一定程度上提高了濾波的穩(wěn)定性。

圖2 落體高度絕對(duì)估計(jì)誤差平均值

圖3 落體速度絕對(duì)估計(jì)誤差平均值

圖4 彈道系數(shù)絕對(duì)估計(jì)誤差平均值

5 結(jié)論

文中首先從協(xié)方差矩陣分解的角度研究了提高濾波器穩(wěn)定性的方法,其次,利用正交變換方法,在不會(huì)改變?yōu)V波精度的前提下消除了采樣點(diǎn)中存在的非局部效應(yīng)。最后,通過(guò)仿真分析,表明改進(jìn)后的優(yōu)化算法具有一定的優(yōu)越性。

[1] 王小旭, 潘泉, 黃鶴, 等. 非線性系統(tǒng)確定采樣型濾波算法綜述 [J]. 控制與決策, 2012, 27(6): 801-812.

[2] JULIER S J, UHLMANN J K. A new method for the nonlinear transformation of means and covariances in filters and estimators [J]. IEEE Transactions on Automatic Control, 2000, 45(3): 477-482.

[3] ITO K, XIONG K. Gaussian filters for nonlinear filtering problems [J]. IEEE Transactions on Automatic Control, 2000, 45(5): 910-927.

[4] MERWE R V, WAN E A. Sigma-point kalman filters for integrated navigation [C]∥Proc. of the 60th Annual Meeting of the Institute of Navigation. Dayton: Institute of Navigation, 2004: 641-654.

[5] JIA B, XIN M, CHENG Y. High-degree cubature kalman filter [J]. Automatica, 2013, 49(2): 510-518.

[6] ARASARATNAM I, HAYKIN S. Cubature kalman filters [J]. IEEE Transactions on Automatic Control, 2009, 54(6): 1254-1269.

[7] JULIER S J. The scaled unscented transformation [C]∥Proc. of American Control Conference. Anchorage: Institute of Electrical and Electronics Engineers Inc, 2002: 4555-4559.

[8] MERWE R V, WAN E A. The square-root unscented kalman filter for state and parameter-estimation [C]∥Proc. of International Conference on Acoustics, Speech, and Signal Processing, Salt Lake City, Utah, 2001.

[9] LEFEBVRE T, BRUYNINCKX H, DE Schuller J. Comment on “a new method for nonlinear transformation of means and covariances in filters and estimations” [J]. IEEE Transations on Automatic Control. 2002, 47(8): 1406-1408.

[10] XIONG K, ZHANG H Y, CHAN C W. Performance evaluation of UKF-based nonlinear filtering [J]. Automatica, 2006, 42(2): 261-270.

[11] CHANG L B, HU B Q, LI A, et al. Transformed unscented kalman filter [J]. IEEE Transactions on Automatic Control, 2013, 58(1): 252-257.

[12] PEREA L, HOW J, BREGER L, et al. Nonlinearities in sensor fusion divergence issues in EKF, modified truncated SOF, and UKF [C]∥AIAA Guidance, Navigation and Control Conference and Exhibit, South Carolina, 2007.

Sample Points Optimization Technique of Nonlinear Deterministic Sampling Filter

WU Qingpo1,3,CONG Yuancai2,ZHOU Shaolei3

(1 No.92313 Unit, Henan Jiyuan 459000, China; 2 No.92886 Unit, Shandong Jiaozhou 266300, China; 3 Naval Aeronautical and Astronautical University, Shandong Yantai 264001, China)

The selection of sample points plays a decisive role on the accuracy and stability of filter during the filter process of deterministic sampling filters. In order to improve the numerical stability of the filter, the obtained sampling points was optimized by covariance matrix decomposition, which solves the problem of the numerical influence on the distance between sample points and the mean Then the appropriate orthogonal transformation is used to eliminate the non-local effect, and it will not change the accuracy of filter under invariant theory. Finally, the effectiveness of the sample points optimization technique is validated through the simulation of a tracking problem

deterministic sampling filters; sampling points optimization; covariance matrix decomposition; orthogonal transformation

2015-06-07基金項(xiàng)目:國(guó)家自然科學(xué)基金(61004002);航空基金(20110184)資助

吳青坡(1985-),男,河南南陽(yáng)人,博士研究生,研究方向:多智能體協(xié)同控制、非線性濾波理論。

TJ55

A