基于似然分布的樣本數(shù)自適應(yīng)UPF算法

高 怡，高 雅，高社生

（1. 西安石油大學(xué) 電子工程學(xué)院，西安 710065；2. 西安工業(yè)大學(xué) 電子信息工程學(xué)院，西安 710021；3. 西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072）

基于似然分布的樣本數(shù)自適應(yīng)UPF算法

高 怡1，高 雅2，高社生3

（1. 西安石油大學(xué) 電子工程學(xué)院，西安 710065；2. 西安工業(yè)大學(xué) 電子信息工程學(xué)院，西安 710021；3. 西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，西安 710072）

針對(duì)粒子濾波算法的實(shí)時(shí)性較差，計(jì)算量隨著粒子數(shù)的增加成級(jí)數(shù)增加，提出一種基于似然分布的樣本數(shù)自適應(yīng)UPF算法。該算法以UPF為基礎(chǔ)，吸收了似然分布自適應(yīng)和樣本數(shù)自適應(yīng)的優(yōu)點(diǎn)，在每一步狀態(tài)方差估計(jì)中規(guī)定樣本數(shù)的下限，同時(shí)考慮狀態(tài)方差過大和過小的情況，在重采樣階段嵌入似然采樣，根據(jù)反映量測(cè)噪聲實(shí)時(shí)統(tǒng)計(jì)性能的精度因子α自適應(yīng)地調(diào)整似然分布狀態(tài)，使之尾部更為平坦，增加先驗(yàn)和似然的重疊區(qū)，減少粒子退化。利用 UT變換獲得各個(gè)粒子的重要性密度函數(shù)，并將最新的量測(cè)信息引入到重要性密度函數(shù)設(shè)計(jì)以及重采樣過程中，從而達(dá)到提高算法估計(jì)性能的目的。將提出的算法應(yīng)用到SINS/SAR組合導(dǎo)航系統(tǒng)中進(jìn)行仿真驗(yàn)證，結(jié)果表明，與PF和UPF算法相比，提出的基于似然分布的粒子數(shù)自適應(yīng)UPF算法能有效改善濾波性能，提高解算精度。

Unscented粒子濾波；樣本數(shù)；自適應(yīng)濾波；似然分布；組合導(dǎo)航

粒子濾波(Particle Filters, PF)算法是一種基于貝葉斯采樣估計(jì)的順序重要采樣濾波算法，通過尋找一組在狀態(tài)空間中傳播的隨機(jī)樣本對(duì)概率密度函數(shù)進(jìn)行近似，以樣本均值代替積分運(yùn)算，從而獲得狀態(tài)最小方差估計(jì)的過程。它對(duì)非線性系統(tǒng)和非高斯估計(jì)問題提供了統(tǒng)一的解決框架，尤其對(duì)強(qiáng)非線性系統(tǒng)的濾波問題有獨(dú)特的優(yōu)勢(shì)，擺脫了解決非線性濾波問題時(shí)隨機(jī)量必須滿足高斯分布的制約條件[1]。但粒子濾波算法在濾波過程中使用的粒子數(shù)目較多，導(dǎo)致計(jì)算量大，實(shí)時(shí)性較差，無法避免地出現(xiàn)粒子退化現(xiàn)象。而解決退化問題的常用方法是選取重要性密度函數(shù)和重采樣，R. Van der Merwe等學(xué)者提出了一種Unscented粒子濾波(Unscented Particle Filter, UPF)算法。該算法利用UT變換得到粒子狀態(tài)均值和方差，以近似正態(tài)分布N(,p)作為采樣的重要性密度函數(shù)，從重要性函數(shù)中抽取隨機(jī)粒子，由UKF的Sigma點(diǎn)構(gòu)成粒子集，使得這些采樣粒子包含了新的量測(cè)信息，從而提高了濾波性能。UPF算法克服了標(biāo)準(zhǔn)粒子濾波不考慮最新量測(cè)信息的缺點(diǎn)[1-2]。但UPF算法依然存在粒子退化問題，當(dāng)粒子數(shù)分布不合理也會(huì)減少粒子的多樣性。

粒子數(shù)量是決定粒子濾波效率和精度的關(guān)鍵因素之一，同時(shí)也與計(jì)算復(fù)雜度和收斂速度密不可分。因此，在保證一定濾波精度的條件下，降低濾波過程使用的粒子數(shù)，就能有效降低濾波的時(shí)間復(fù)雜度，但是無限制地減少粒子數(shù)有可能造成粒子集表征后驗(yàn)概率密度真實(shí)分布的能力退化，從而使得濾波精確程度大幅降低。對(duì)標(biāo)準(zhǔn)的粒子濾波算法而言，其樣本數(shù)是固定不變的，不變的樣本容量直接影響計(jì)算復(fù)雜度，增加了算法計(jì)算負(fù)擔(dān)，使得算法的實(shí)時(shí)性和精度變差。為了減少計(jì)算量，使得粒子濾波能用于實(shí)時(shí)數(shù)據(jù)處理，2002年 D Fox提出了粒子個(gè)數(shù)可變的自適應(yīng)粒子濾波，即基于Kullback-Leibler（KL）信息數(shù)或KL距離（KLD）采樣的自適應(yīng)粒子濾波器[3]。該方法通過粒子數(shù)的自適應(yīng)變化來保證后驗(yàn)密度的真值與估計(jì)值之間的誤差限，這種誤差限用 KLD表示。在概率密度集中在狀態(tài)空間的小范圍(狀態(tài)分布不確定性較小)時(shí)采用少量粒子數(shù)目，反之則采用較多粒子。但是，考慮到實(shí)際系統(tǒng)應(yīng)用的硬件條件，該方法的計(jì)算負(fù)荷過高。Torma等學(xué)者提出了基于似然分布自適應(yīng)調(diào)整的粒子濾波算法[4]，該算法選取先驗(yàn)密度作為重要性密度函數(shù)，所需的粒子數(shù)應(yīng)能保證非歸一化似然值和超過某一預(yù)定的門限，實(shí)現(xiàn)簡單。但是，該方法沒有考慮最新量測(cè)信息的影響[5]，權(quán)值方差對(duì)確定粒子數(shù)影響很大，而且還會(huì)增強(qiáng)粒子間的相關(guān)性，增加了高速并行實(shí)現(xiàn)的難度。

針對(duì)標(biāo)準(zhǔn)粒子濾波器在較高觀測(cè)精度的場(chǎng)合下可能導(dǎo)致濾波發(fā)散，文獻(xiàn)[6]提出了一種基于自調(diào)整粒子濾波的組合導(dǎo)航方法，根據(jù)觀測(cè)噪聲的統(tǒng)計(jì)大小，自適應(yīng)調(diào)整似然分布的形狀，使之與先驗(yàn)分布重疊的區(qū)域更大，有效提高濾波穩(wěn)定性。但是，該方法人為地增大了似然分布的影響，雖然增強(qiáng)了粒子濾波的魯棒性，但在一定程度上影響了估計(jì)精度[7]。文獻(xiàn)[8]提出了一種用于狀態(tài)估計(jì)的自適應(yīng)粒子濾波算法，該算法通過UKF構(gòu)造粒子群，而粒子中的每個(gè)Sigma點(diǎn)用自適應(yīng)強(qiáng)跟蹤濾波來更新，可以在線調(diào)節(jié)因子。但是，隨著系統(tǒng)維數(shù)的增大，計(jì)算量將迅速增加，特別是在系統(tǒng)非線性、非高斯較強(qiáng)時(shí)，濾波性能急劇下降，甚至發(fā)散。

本文在上述學(xué)者研究成果的基礎(chǔ)上，提出一種基于似然分布的樣本數(shù)自適應(yīng) UPF(Unscented Particle Filter)算法。該方法以UPF算法為基礎(chǔ)，吸收了似然分布自適應(yīng)和樣本數(shù)自適應(yīng)的優(yōu)點(diǎn)，對(duì)粒子數(shù)進(jìn)行自適應(yīng)調(diào)整，利用UT變換獲得各粒子的重要性密度函數(shù)，將最新量測(cè)信息引入到粒子的重要性密度函數(shù)設(shè)計(jì)以及重采樣中，觀測(cè)噪聲實(shí)時(shí)統(tǒng)計(jì)性能，通過改進(jìn)策略自適應(yīng)調(diào)整似然分布的分布狀態(tài)，使之尾部更為平坦，增加先驗(yàn)和似然的重疊區(qū)，有效提高濾波穩(wěn)定性，從而達(dá)到提高算法估計(jì)性能的目的。

1 基于似然分布的樣本數(shù)自適應(yīng)UPF算法

考慮非線性系統(tǒng)模型如下[7]：

1.1 樣本數(shù)可自適應(yīng)調(diào)整的粒子濾波

樣本數(shù)可自適應(yīng)調(diào)整的粒子濾波算法描述如下[7]：

① 參數(shù)初始化。

1.2 基于似然分布的樣本數(shù)自適應(yīng)UPF算法

基于似然分布的樣本數(shù)自適應(yīng) UPF算法的基本思想是，首先在每一步狀態(tài)方差估計(jì)中規(guī)定樣本數(shù)的下限，同時(shí)考慮了狀態(tài)方差過大和過小的情況，利用UT變換獲得各個(gè)粒子的重要性密度函數(shù)，并將最新的量測(cè)信息引入到粒子的重要性密度函數(shù)設(shè)計(jì)以及重采樣中，然后在重采樣階段嵌入似然采樣，根據(jù)反映量測(cè)噪聲信息實(shí)時(shí)統(tǒng)計(jì)性能的精度因子的大小，自適應(yīng)地調(diào)整似然分布狀態(tài)，使之尾部更為平坦，增加先驗(yàn)和似然的重疊區(qū)，從而提高濾波精度。

基于似然分布的樣本數(shù)自適應(yīng)UPF算法步驟如下：

① 初始化

b) 時(shí)間更新

在獲得新的量測(cè)yk后，進(jìn)行濾波量測(cè)更新，得到

2）計(jì)算權(quán)值

式中，β為自適應(yīng)系數(shù)，由量測(cè)噪聲的統(tǒng)計(jì)特性決定。

d）歸一化權(quán)值

這一步充分利用了新的量測(cè)值來改進(jìn)粒子采樣的精度。

3）均值與方差估計(jì)

4）重采樣

5）預(yù)測(cè)樣本數(shù)

結(jié)合當(dāng)前k的量測(cè)值來估計(jì)k+1時(shí)刻狀態(tài)誤差的方差σ2(k+1)，粒子數(shù)的下界表達(dá)式如下：

根據(jù)式(23)計(jì)算樣本數(shù)量Nk+1，即給定置信區(qū)間L和參數(shù)α， k時(shí)刻的方差，即可得到粒子數(shù)的下限。

6）令k=k+1，返回步驟1），計(jì)算下一時(shí)刻的狀態(tài)估值，直到仿真時(shí)間T結(jié)束。

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

2.1 SINS/SAR組合導(dǎo)航系統(tǒng)

將提出的基于似然分布的樣本數(shù)自適應(yīng) UPF (LDSNAUPF)算法應(yīng)用到 SINS/SAR組合導(dǎo)航系統(tǒng)中，并與 Unscented粒子濾波(UPF)算法進(jìn)行仿真比較。實(shí)驗(yàn)數(shù)據(jù)來源于某型號(hào)運(yùn)載器在西安郊區(qū)的一段飛行試驗(yàn)。系統(tǒng)數(shù)學(xué)模型見文獻(xiàn)[9]。設(shè)運(yùn)載器的初始位置為初始位置為北緯34.24°，東經(jīng)108.99°，初始高度為800 m，初始速度為90 m/s，航向正北，經(jīng)過水平、爬升、左右轉(zhuǎn)彎、俯沖等動(dòng)作，終點(diǎn)位置為北緯37.34°，東經(jīng)110.19°，高度為1256 m，飛行時(shí)間為1800 s。設(shè)陀螺常值漂移為0.1 (°)/h，測(cè)量白噪聲為0.05 (°)/h，隨機(jī)游走為加速度計(jì)零偏為1× 10-4g，隨機(jī)游走為SAR圖像匹配計(jì)算時(shí)間取為1 s，SAR的水平定位精度為15 m，氣壓高度表測(cè)量誤差為10 m。設(shè)SINS初始水平對(duì)準(zhǔn)誤差為0.01°，方位對(duì)準(zhǔn)誤差為0.03°，初始速度誤差(東向、北向、天向)分別為0.5 m/s、0.3 m/s和0.5 m/s，初始位置誤差(經(jīng)度、緯度、高度)分別為6 m，4 m和5 m，初始姿態(tài)誤差(航向角、俯仰角、橫滾角)分別為0.1°、0.1°和0.2°。選取粒子數(shù)為100，進(jìn)行50次Monte-Carlo仿真。

仿真時(shí)間為1800 s。采用標(biāo)準(zhǔn)UP、UPF和提出的LDSNAUPF算法計(jì)算得到的定位誤差如圖1～3所示。

由圖1～3可以看到，采用提出的LDSNAUPF算法計(jì)算得到的東向、北向和天向位置誤差相對(duì)最小，濾波后東向位置誤差控制在[-5 m, +7 m]，北向位置誤差控制在[-6 m, +6 m]，天向位置誤差控制在[-5 m, +6 m]，明顯優(yōu)于PF和UPF。

圖1 PF定位誤差Fig.1 Positioning error of PF

圖2 UPF定位誤差Fig.2 Positioning error of UPF

圖3 提出的LDSNAUPF定位誤差Fig.3 Positioning error of LDSNAUPF

2.2 單變量非靜態(tài)增長模型

采用單變量非靜態(tài)增長模型[10]，狀態(tài)模型和觀測(cè)模型如下：

式中，wk與vk均服從標(biāo)準(zhǔn)正態(tài)分布，過程噪聲方差為Q=10，量測(cè)噪聲方差R=1，初始狀態(tài)取x0=0.1，P0=3。

圖4為一次仿真實(shí)驗(yàn)中采用三種濾波算法對(duì)系統(tǒng)狀態(tài)估計(jì)對(duì)比曲線，橫坐標(biāo)表示運(yùn)行時(shí)間，縱坐標(biāo)表示狀態(tài)。圖5給出了三種濾波算法的RMSE曲線比較。LDSNAUPF算法與PF和UPF相比，均方誤差的均值和方差都有所減小，這是因?yàn)橐肓四芊从沉繙y(cè)噪聲信息統(tǒng)計(jì)性能的精度因子α，通過改變?chǔ)恋拇笮碜赃m應(yīng)地調(diào)整似然分布狀態(tài)，從而改變粒子對(duì)應(yīng)權(quán)值的分布，增加有用粒子的權(quán)值，從而提高算法估計(jì)性能。

圖4 采用三種濾波算法對(duì)系統(tǒng)狀態(tài)估計(jì)對(duì)比Fig.4 System state estimations by three filters

圖5 三種濾波算法的RMSE曲線Fig.5 Comparison on RMSEs by three filtering algorithms

3 結(jié) 論

為了提高組合導(dǎo)航系統(tǒng)的濾波精度，針對(duì)粒子濾波算法的實(shí)時(shí)性較差，提出了一種新的基于似然分布的粒子數(shù)自適應(yīng)UPF算法。該算法在每一步狀態(tài)方差估計(jì)中規(guī)定樣本數(shù)的下限，在重采樣階段嵌入似然采樣，根據(jù)反映量測(cè)噪聲實(shí)時(shí)統(tǒng)計(jì)性能的精度因子α的大小，自適應(yīng)調(diào)整似然分布狀態(tài)，增加先驗(yàn)和似然的重疊區(qū)，減少了粒子退化。利用UT變換獲得各個(gè)粒子的重要性密度函數(shù)，并將最新量測(cè)信息引入到粒子的重要性密度函數(shù)設(shè)計(jì)以及重采樣中，從而達(dá)到提高算法估計(jì)性能的目的。

將該算法應(yīng)用到SINS/SAR組合導(dǎo)航系統(tǒng)中進(jìn)行仿真驗(yàn)證，實(shí)驗(yàn)結(jié)果表明，提出的算法與PF和UPF算法相比，能有效改善濾波性能，提高濾波精度及穩(wěn)定性，適用于非線性非高斯系統(tǒng)。該方法雖然增大了似然分布的范圍，增強(qiáng)了濾波的魯棒性，但是在一定程度上增加了計(jì)算復(fù)雜度，下一步需要解決的是如何降低計(jì)算量的問題。

（References）：

[1] 趙琳, 王小旭, 丁繼成, 等. 組合導(dǎo)航系統(tǒng)非線性濾波算法綜述[J]. 中國慣性技術(shù)學(xué)報(bào), 2009, 17(1): 46-52. Zhao Lin, Wang Xiao-xu, Ding Ji-cheng, et al. Overview of nonlinear filter methods applied in integrated navigation system[J]. Journal of Chinese Inertial Technology, 2009, 17(1): 46-52.

[2] 高怡, 高社生, 馮志華. 一種新的非線性模型預(yù)測(cè)UPF算法[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2012, 30(5): 734-738. Gao Yi, Gao She-sheng, Feng Zhi-hua. Exploring an effective nonlinear model predictive unscented particle filtering (NMPUPF) algorithm[J]. Journal of Northwestern Polytechnical University, 2012, 30(5): 734-738.

[3] Fox D. KLD-sampling: Adaptive particle filters[J]. International Journal of Robotics Research, 2003, 22(12): 985-1003.

[4] Torma P, Szepesvari C. On using likelihood-adjusted proposals in particle filtering: local importance sampling [C]//Proceedings of the 4th International Symposium on Image and Signal Processing and Analysis, 2005: 58-63.

[5] 趙巖, 高社生, 楊一. 自適應(yīng)SDV-UPF算法及其在緊組合中的應(yīng)用[J]. 中國慣性技術(shù)學(xué)報(bào), 2014, 22(1): 83-88. Zhao Yan, Gao She-sheng, Yang Yi. Adaptive SVD-UPF algorithm and application to tightly-coupled integrated navigation[J]. Journal of Chinese Inertial Technology, 2014, 22(1): 83-88.

[6] 崔平遠(yuǎn), 鄭黎方, 裴福俊. 基于自調(diào)整粒子濾波的組合導(dǎo)航方法研究[J]. 計(jì)算機(jī)工程, 2008, 34(14): 185-187. Cui Ping-yuan, Zheng Li-fang, Pei Fu-jun. Research on integrated navigation system based on self adjust particle filter[J]. Computer Engineering, 2008, 34(14): 185-187.

[7] Doucet A, Johansen A M. A tutorial on particle filtering and smoothing: fifteen years later[M]//Handbook of Nonlinear Filtering. Cambridge University Press, 2009.

[8] 鄧小龍, 謝劍英, 郭為忠. 用于狀態(tài)估計(jì)的自適應(yīng)粒子濾波器研究[J]. 華南理工大學(xué)學(xué)報(bào), 2006, 34(1): 57-62. Deng Xiao-long, Xie Jian-ying, Guo Wei-zhong. Adaptive particle filtration for state estimation[J]. Journal of South China University of Technology, 2006, 34(1): 57-62.

[9] Zhong Y, Gao S, Li W. A quaternion-based method for SINS/SAR integrated navigation system[J]. IEEE Transactions on Aerospace and Electronic Systems, 2012, 48(1): 514-524.

[10] Gordon N J, Salmond D J, Smith A F M. Novel approach to nonlinear/non-Gaussian Bayesian state estimation[C]// IEEE Proceedings-F, 1993, 140(2): 107-113.

Adaptive sample-size unscented particle filter based on likelihood distribution

GAO Yi1, GAO Ya2, GAO She-sheng2
(1. School of Electronic Engineering, Xian ShiYou University, Xi’an 710065, China; 2. School of Electronic Information Engineering, Xian Technological University, Xi’an 710021, China; 3. Department of Automatics Control, Northwestern Polytechnical University, Xi’an 710072, China)

Aiming at the poor real-time performance of particle filtering and the computation amount’s exponentially increasing with particle numbers, this paper presents an adaptive sample size UPF (unscented particle filtering) algorithm, which takes the advantages of the adaptivities of likelihood distribution and sample number. In the state variance estimation, the lower limit of sample is set at each step, and the cases when state variance is too large or too small are taken into account. In the resampling phase, the likelihood samples are embedded, and the likelihood distribution state is adaptively adjusted based on the precision factor α which can reflect the real-time statistical performance of observational noises to increase the overlapping area of the prior and the likelihood and reduce particle degeneration. In addition, the method uses the unscented transformation to obtain the importance density function of each particle, and introduces the latest observational information to the importance density function and resample, thus effectively improves the estimation performance. By applying the proposed algorithm to the SINS/SAR integrated navigation system, the simulation results and their analysis demonstrate that, compared with the PF and UPF algorithms, the proposed algorithm can effectively improve filter performance and calculation precision.

unscented particle filter; sample number; adaptive filter; likelihood distribution; integrated navigation

TP301.6

：A

2015-06-15；

：2015-09-23

陜西省科技攻關(guān)項(xiàng)目（2013k09-18）；西安石油大學(xué)青年科技創(chuàng)新基金項(xiàng)目

高怡（1978—），女，博士，講師，從事控制理論與控制工程。E-mail：gaoyi_nwpu@163.com

1005-6734(2015)05-0648-05

10.13695/j.cnki.12-1222/o3.2015.05.016