基于Monte Carlo采樣策略的UKF算法應(yīng)用

萬某峰，趙長勝，王 飛，陳士城

（徐州師范大學(xué) 測繪學(xué)院，江蘇 徐州 221116）

由于近似非線性函數(shù)的概率密度分布比近似其函數(shù)更容易。因此，UT變換選擇一組點(diǎn)集（Sigma點(diǎn)集）來表征輸入狀態(tài)的分布特征，再將非線性變換應(yīng)用于Sigma點(diǎn)，得到非線性轉(zhuǎn)換后的點(diǎn)集，對轉(zhuǎn)換后的點(diǎn)集通過加權(quán)計(jì)算得到統(tǒng)計(jì)特性。UT變換算法的關(guān)鍵是Sigma點(diǎn)采樣策略，由采樣策略得到的Sigma點(diǎn)集要能保證輸入變量的分布特征，體現(xiàn)非線性函數(shù)本身的非線性［1－7］。Monte Carlo模擬方法的基本思想是面對一個(gè)問題，首先建立與描述該問題有相似的概率模型，然后對模型進(jìn)行隨機(jī)模擬或統(tǒng)計(jì)抽樣，即產(chǎn)生一組隨機(jī)數(shù)來模擬該隨機(jī)模型，它的分布與該隨機(jī)模型相同，再以該隨機(jī)數(shù)的統(tǒng)計(jì)特征作為原始問題的近似解［8］。使用Monte Carlo模擬方法生成符合輸入狀態(tài)分布的隨機(jī)變量，符合UT變換思想。

1 非線性系統(tǒng)的卡爾曼濾波

設(shè)某非線性系統(tǒng)模型如下：

式（1）為狀態(tài)模型，式（2）為量測模型。式中：xk為k時(shí)刻狀態(tài)向量，uk－1為控制向量，wk－1為模型噪聲向量，zk為k時(shí)刻的觀測向量，vk為量測噪聲向量，這里假設(shè)wk－1和vk均為高斯白噪聲，即均值為0，方差分別為Qk－1和Rk。

假設(shè)所有的隨機(jī)變量均服從高斯分布，非線性卡爾曼濾波公式如下：

預(yù)測方程

更新方程

2 基于Monte Carlo采樣策略的UKF算法

這里考慮wk－1和vk均為加性噪聲，并且狀態(tài)方程為線性，觀測方程為非線性，設(shè)系統(tǒng)的狀態(tài)滿足高斯分布，系統(tǒng)模型為

假設(shè)wk－1和vk互不相關(guān)，且協(xié)方差矩陣分別為Qk－1和Rk，n維隨機(jī)變量X初始狀態(tài)的均值和方差分別為x0和P0，即隨機(jī)變量X服從高斯分布。

2.1 Monte Carlo采樣策略

Monte Carlo采樣策略的關(guān)鍵是產(chǎn)生符合輸入狀態(tài)分布的隨機(jī)數(shù)，隨機(jī)數(shù)產(chǎn)生步驟：

1）確定一個(gè)數(shù)學(xué)模型或某種規(guī)則；

2）規(guī)定初始值；

3）按一定的步驟產(chǎn)生隨機(jī)數(shù)，重復(fù)下去，得到隨機(jī)數(shù)序列。

這里直接調(diào)用MATLAB軟件帶有的高斯分布隨機(jī)數(shù)產(chǎn)生函數(shù)—normrnd函數(shù)［9－10］

返回m個(gè)均值為ave，標(biāo)準(zhǔn)差為sig的高斯分布的隨機(jī)數(shù)A。

采用Monte Carlo采樣策略，對均值為x0，方差為P0的n維隨機(jī)變量X，生成的m個(gè)χ（Sigma點(diǎn)集）為

其中：（x0）i表示x0的第i個(gè)值，表示P0第i個(gè)對角線元素的平方根值。

2.2 UKF算法步驟

1）根據(jù)k－1時(shí)刻的狀態(tài)估計(jì)值和其協(xié)方差Pk，計(jì)算狀態(tài)預(yù)測值和協(xié)方差Pk，k－1。

2）由和Pk，k－1，采用Monte Carlo采樣策略生成Sigma點(diǎn)集

3）通過非線性量測方程傳遞Sigma點(diǎn)，計(jì)算預(yù)測值和其協(xié)方差Pzk。

4）計(jì)算濾波增益和k時(shí)刻狀態(tài)估值＾Xk，k和其協(xié)方差Pk。

3 仿真實(shí)驗(yàn)分析

一個(gè)在二維平面內(nèi)運(yùn)動(dòng)的質(zhì)點(diǎn)M，初始位置（0，3 000），它以初速度Vx＝10m/s，Vy＝50m/s，加速度ax＝2m/s2，ay＝－4m/s2運(yùn)動(dòng)，假設(shè)一坐標(biāo)位置為（0，0）雷達(dá)對M進(jìn)行測距rk和測角φk，系統(tǒng)噪聲和測量噪聲均為加性噪聲，狀態(tài)向量為Xk＝，量測向量為Zk＝［zk1，zk2］T，建立卡爾曼濾波模型

其中：

假設(shè)系統(tǒng)噪聲wk和vk協(xié)方差分別為Qk和Rk，二者不相關(guān)。MonteCarlo采樣策略生成Sigma點(diǎn)數(shù)為5 000，采樣時(shí)間t＝0.5s，觀測次數(shù)100次。

當(dāng)R＝diag［0.09 0.0012］，Q＝diag［10 10 0.01 0.01 0.0120.012］時(shí)，比較結(jié)果見圖1、圖2。

圖1 實(shí)際軌跡值與UKF濾波值比較

當(dāng)R＝diag［0.09 0.0012］，Q＝diag［100 100 0.01 0.01 0.0120.012］時(shí)，比較結(jié)果見圖3、圖4。

當(dāng)R＝diag［10 0.0012］，Q＝diag［1 1 0.01 0.01 0.0120.012］時(shí)，比 較結(jié)果見圖5、圖6。

當(dāng)R＝diag［100 0.0012］，Q＝diag［1 1 0.01 0.01 0.0120.012］時(shí)，比 較結(jié)果見圖7、圖8。

1）從圖1、圖3、圖5、圖7，可以看出，在模型誤差和觀測誤差不是很大時(shí)，利用MonteCarlo采樣策略的UKF濾波結(jié)果均能很好地跟蹤目標(biāo)。

2）比較圖2和圖4，可以得到，在觀測誤差較小的情況下，模型誤差的大小對UKF濾波結(jié)果影響較小，即此時(shí)濾波結(jié)果精度更依賴觀測值精度。

3）比較圖6和圖8可以得到，在模型誤差保持不變的情況下，觀測誤差對UKF濾波結(jié)果影響較大。

4 結(jié)束語

MonteCarlo采樣策略是一種隨機(jī)性采樣方法，保證了得到的Sigma點(diǎn)集符合輸入狀態(tài)分布特征，再用大量的Sigma點(diǎn)逼近概率密度分布，從而得到更高階近似，充分體現(xiàn)了非線性函數(shù)本身的非線性。采用MonteCarlo采樣策略的UKF具有一定的實(shí)用性和參考價(jià)值，因此，可做進(jìn)一步應(yīng)用研究。

［1］王新洲.非線性模型參數(shù)估計(jì)理論與應(yīng)用［M］.武漢：武漢大學(xué)出版社，2002.

［2］JULIER S J，UHLMANN J K，DURRANT－WHYTE H F.A New Approach for Filtering Nonlinear System［C］//Proceeding of the American Control Conference.USA：IEEE，1995：1628－1632.

［3］JULIER S J，UHLMANN J K，DURRANT－WHYTE H F.A New Method for the Nonlinear Tranformation of Means and Covariances in Filters and Estimators［J］.IEEE Trans，Automatic Control，2000，45（3）：477－482.

［4］JULIER S J，UHLMANN J K.Unscented filtering and nonlinear estimation［J］.Proceeding of the IEEE，2004，92（3）：401－422.

［5］JULIER S J.The spherical simplex unscented transformation［C］//Proceeding of the American Control Conference.USA：IEEE，2003：2430－2434.

［6］程水英.無葉變換與無葉卡爾曼濾波［J］.計(jì)算機(jī)工程與應(yīng)用，2008，44（24）：25－35.

［7］劉也，余安喜，朱炬波，等.加性噪聲條件下的UKF算法［J］.中國科學(xué)，2010，40（11）：1286－1299.

［8］王中宇，劉智敏，夏新濤，等.測量誤差與不確定度評定［M］.北京：科學(xué)出版社，2008.

［9］鄧薇.MATLAB函數(shù)速查手冊［M］.北京：人民郵電出版社，2010.

［10］李海濤，鄧櫻.MATLAB程序設(shè)計(jì)教程［M］.北京：高等教育出版社，2010.