均方根容積代價(jià)參考粒子濾波算法

武青海 ，曲朝陽(yáng)

(1.吉林農(nóng)業(yè)科技學(xué)院電氣與信息工程學(xué)院，吉林 吉林 132101； 2.東北電力大學(xué)信息工程學(xué)院，吉林 吉林 132012)

均方根容積代價(jià)參考粒子濾波算法

武青海1，曲朝陽(yáng)2

(1.吉林農(nóng)業(yè)科技學(xué)院電氣與信息工程學(xué)院，吉林 吉林 132101； 2.東北電力大學(xué)信息工程學(xué)院，吉林 吉林 132012)

為提高復(fù)雜噪聲的濾波精度，基于均方根容積卡爾曼濾波(SCKF)和代價(jià)參考粒子濾波(CRPF)，提出一種新的均方根容積代價(jià)參考粒子濾波算法(SCCRPF)。算法采用SCKF和最新量測(cè)信息更新先驗(yàn)分布函數(shù)，生成CRPF的重要密度函數(shù)，保留了SCKF對(duì)非線性系統(tǒng)的濾波精度，同時(shí)獲取了CRPF對(duì)噪聲假設(shè)未知系統(tǒng)的濾波精度。仿真結(jié)果表明，對(duì)于噪聲假設(shè)未知系統(tǒng)，SCCRPF的濾波精度高于均方根容積粒子濾波(SCPF)；對(duì)于噪聲假設(shè)已知系統(tǒng)，SCCRPF的濾波精度高于CRPF。

狀態(tài)估計(jì)； 非線性系統(tǒng)； 非高斯系統(tǒng)； 代價(jià)參考粒子濾波

0 引言

狀態(tài)估計(jì)問(wèn)題在視景跟蹤、機(jī)器人定位、航海以及其他領(lǐng)域得到廣泛研究。對(duì)于非線性系統(tǒng)，常用的估計(jì)方法是擴(kuò)展卡爾曼濾波(EKF)[1-2]、不敏卡爾曼濾波(UKF)[3-4]、均方根容積卡爾曼濾波(Square-root Cubature Kalman Filter,SCKF)[5-6]。但以上都是假設(shè)過(guò)程噪聲和量測(cè)噪聲為高斯白噪聲，在非高斯噪聲條件下的濾波效果就會(huì)很差。對(duì)于非線性非高斯系統(tǒng)，基于貝葉斯理論和蒙特卡羅方法的粒子濾波(Particle Filter,PF)[7-8]能夠獲取好的濾波效果。

為提高復(fù)雜噪聲系統(tǒng)的濾波精度，學(xué)者們提出了不依賴任何分布函數(shù)先驗(yàn)知識(shí)的代價(jià)參考粒子濾波(Cost-Reference Particle Filter,CRPF)[9-10]。但在量測(cè)信息較為精確的條件下，CRPF的濾波精度可以通過(guò)融合最新量測(cè)信息進(jìn)行改進(jìn)。

通過(guò)將SCKF引入CRPF框架下提出了SCCRPF，利用最新量測(cè)信息，通過(guò)SCKF來(lái)產(chǎn)生重要性密度函數(shù)(Importance Density Function,IDF)，在量測(cè)信息相對(duì)準(zhǔn)確條件下，IDF更加接近真實(shí)的后驗(yàn)概率分布。SCCRPF不僅能夠處理噪聲假設(shè)未知的系統(tǒng)，同時(shí)獲取了SCKF對(duì)非線性系統(tǒng)的濾波精度。仿真結(jié)果表明，對(duì)噪聲條件復(fù)雜的系統(tǒng)，SCCRPF具有更高的濾波精度。

1 SCCRPF

CRPF的粒子從IDF里采樣，IDF的確定對(duì)CRPF十分重要，目前較為流行的方法是采用狀態(tài)轉(zhuǎn)移先驗(yàn)分布，因?yàn)槠溆?jì)算較為簡(jiǎn)單，但這種方法未能包含最新的量測(cè)信息。在量測(cè)信息相對(duì)準(zhǔn)確條件下，采用SCKF等方法對(duì)粒子進(jìn)行傳遞，可進(jìn)一步提升CRPF的濾波精度，如擴(kuò)展粒子濾波(EPF)[11]和不敏粒子濾波(UPF)[12]的精度要比傳統(tǒng)PF高。

以SCKF作為子濾波器，完成CRPF中粒子傳遞過(guò)程，提出新的SCCRPF，其過(guò)程如下。

1.1 初始化

在k=0時(shí)刻，從初始分布中均勻抽取N個(gè)粒子，即

xj0～u(I0)j=1,2,…,N。

(1)

為保證粒子多樣性，I0的選取應(yīng)足夠大。計(jì)算各個(gè)粒子初始協(xié)方差的開(kāi)方Sj0，即

(2)

(3)

Pj0=Sj0(Sj0)T。

(4)

對(duì)各粒子初始代價(jià)賦值

cj0=0 。

(5)

得到初始樣本集合

(6)

1.2 重采樣

已知k時(shí)刻的粒子集合為

(7)

結(jié)合k+1時(shí)刻的量測(cè)zk+1，計(jì)算各個(gè)粒子的風(fēng)險(xiǎn)，即

δjk+1=λcjk+Δc(xjk|zk+1)

(8)

式中：λ為遺忘因子，主要作用平衡歷史信息和當(dāng)前信息的權(quán)重，其值越大歷史信息占比重越大，反之歷史信息比重越小;增量部分的計(jì)算形式為

Δc(xjk|zk+1)=‖zk+1-h(f(xjk))‖p

(9)

式中,p可取1或2。當(dāng)?shù)玫礁鱾€(gè)粒子的風(fēng)險(xiǎn)后，計(jì)算粒子的概率混合密度函數(shù)(Probability Mass Function,PMF),即

(10)

式中,u(δjk+1)的計(jì)算式為

u(δjk+1)=1/(δjk+1-minj{δjk+1}+α)β

(11)

式中，β=2，且有

(12)

(13)

1.3 利用SCKF完成粒子傳遞

(14)

傳遞各粒子的容積點(diǎn)

xj+l,k=f(xjl,k) 。

(15)

估計(jì)各粒子的預(yù)測(cè)狀態(tài)

(16)

估計(jì)各粒子

(17)

式中:Tria()為求預(yù)測(cè)協(xié)方差的開(kāi)方的操作，具體操作見(jiàn)文獻(xiàn)[5-6];SjQ為第j個(gè)粒子的狀態(tài)協(xié)方差的開(kāi)方，且有

(18)

量測(cè)更新：由于Sjk+1|k已知，直接計(jì)算各粒子的容積點(diǎn)(l=1,2,…,2n),即

(19)

傳遞各粒子的容積點(diǎn)

zj+l,k+1|k=h(xjl,k+1|k) 。

(20)

估計(jì)各粒子的預(yù)測(cè)量測(cè)

(21)

估計(jì)各粒子新息協(xié)方差的開(kāi)方

(22)

式中,SjR為第j個(gè)粒子量測(cè)協(xié)方差的開(kāi)方，且有

(23)

估計(jì)各粒子的交叉協(xié)方差為

Pjxz,k+1|k=xj++k+1|k(zj+k+1|k)T。

(24)

且有

(25)

計(jì)算各粒子的增益為

Φjk+1=Pjxz,k+1|k(Sjzz,k+1|k(Sjzz,k+1|k)T)-1。

(26)

估計(jì)各粒子的狀態(tài)為

(27)

估計(jì)各粒子協(xié)方差的開(kāi)方為

(28)

經(jīng)過(guò)SCKF傳遞后，新的粒子包含了最新的量測(cè)信息，基于傳遞后的狀態(tài)和協(xié)方差開(kāi)方，產(chǎn)生粒子

(29)

對(duì)各粒子的代價(jià)進(jìn)行更新，有

(30)

式中,

Δc(xjk+1|zk+1)=‖zk+1-h(xjk+1)‖p。

(31)

經(jīng)過(guò)粒子傳遞過(guò)程，得到更新后的粒子及其代價(jià)集合

(32)

1.4 狀態(tài)估計(jì)

對(duì)每個(gè)粒子分配PMF

為使A相剩磁穩(wěn)定，需使合閘時(shí)刻外加電源A相虛擬磁鏈正好處在對(duì)應(yīng)方向的峰值，若剩磁為負(fù)，則為負(fù)峰值，反之亦然。假設(shè)剩磁為某一值X，則要求剩磁滿足：

(33)

式中,

u(cjk+1)=1/(cjk+1-minj{cjk+1}+α)β。

(34)

(35)

狀態(tài)估計(jì)為

(36)

2 仿真結(jié)果與分析

為了驗(yàn)證所提SCCRPF的有效性，進(jìn)行仿真并與CRPF,SCPF比較。所采用的系統(tǒng)的狀態(tài)方程為

(37)

量測(cè)方程為

(38)

初始狀態(tài)為x0～N(10,0.8),所有的粒子數(shù)目為500,采用的重采樣方法為殘差重采樣，蒙特卡羅仿真次數(shù)為50次,遺忘因子λ=0.85，且p=2。

首先，在噪聲假設(shè)已知條件下，將CRPF與SCCRPF進(jìn)行比較，以說(shuō)明利用SCKF進(jìn)行CRPF粒子傳遞的效果；其次，在噪聲假設(shè)未知條件下，將SCPF與SCCRPF進(jìn)行比較，說(shuō)明后二者兼顧著CRPF對(duì)噪聲未知條件的處理能力。

2.1 噪聲假設(shè)已知

噪聲假設(shè)為

(39)

分別采用CRPF和SCCRPF對(duì)系統(tǒng)進(jìn)行跟蹤，其中，CRPF與SCCRPF的跟蹤結(jié)果對(duì)比如圖1所示。

圖1 CRPF與SCCRPF的比較Fig.1 Comparison of CRPF with SCCRPF

從圖1可以看出，CRPF和SCCRPF均可有效跟蹤目標(biāo)，在噪聲假設(shè)已知條件下，SCCRPF的精度同樣優(yōu)于CRPF，對(duì)其跟蹤誤差的RMSE數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分別為1.852和1.653。

2.2 噪聲假設(shè)未知

狀態(tài)噪聲假設(shè)與噪聲假設(shè)已知情況相同，量測(cè)噪聲由已知部分和未知部分組成，即

V2k=V2k,known+V2k,unknown

(40)

(41)

同理，將SCCRPF與采用SCKF完成粒子傳遞的SCPF進(jìn)行比較，結(jié)果如圖2所示。從圖2可以看出，SCCRPF的濾波精度高于SCPF。對(duì)其跟蹤誤差RMSE數(shù)據(jù)進(jìn)行統(tǒng)計(jì)分別為8.687和6.228。

圖2 SCPF與SCCRPF的比較Fig.2 Comparison of SCPF with SCCRPF

從仿真實(shí)驗(yàn)可以看出，利用SCKF傳遞CRPF的粒子，提升了對(duì)已知噪聲濾波精度，同時(shí)，SCCRPF獲得了CRPF對(duì)未知噪聲的處理能力，新的非線性非高斯濾波在處理復(fù)雜噪聲方面具有更好的整體性能。

3 結(jié)論

通過(guò)對(duì)SCKF和CRPF的融合，提出了一種SCCRPF，新算法采用了最新量測(cè)信息完成CRPF粒子的傳遞，提升了IDF對(duì)真實(shí)后驗(yàn)概率分布的逼真度，提高了濾波精度。仿真結(jié)果亦驗(yàn)證了SCCRPF的有效性。

[1] ZHANG K W,CHEN J C,CHANG Y F,et al.EKF-based LQR tracking control of a quadrotor helicopter subject to uncertainties[C]//IECON 42nd Annual Conference of IEEE Industrial Electronics Society,2016:5426-5431.

[2] YANG S K,LI H X.Application of EKF and UKF in target tracking problem[C]//The 8th International Conference on Intelligent Human-Machine Systems and Cybernetics,2016:116-120.

[3] CHEN J,SONG J,LI L,et al.UKF-based adaptive variable structure observer for vehicle sideslip with dynamic correction[J].IET Control Theory and Applications, 2016,10(14):1641-1652.

[4] DIAZ A O C,SANTOS A I G.State estimation of the switched reluctance motor MFR 132.5 with MHE and UKF estimators[J].IEEE Latin America Transactions, 2016,14(1):147-152.

[5] LI C,GE Q B.SCKF for MAV attitude estimation[C]//International Conference on Machine Learning and Cybernetics,2011:1313-1318.

[6] 林浩申，黃璜，劉剛，等.基于SCKF算法的空間機(jī)動(dòng)目標(biāo)跟蹤研究[J].電光與控制,2015,22(3):64-68.

[7] SONG Z Q,FU P,LI J C,et al.Prototype design and test of ITER PF converter unit[J].IEEE Transactions on Plasma Science,2016,44(9):1677-1683.

[8] YOUN H S,LEE J B,BAEK J,et al.A digital phase leading filter current compensation (PLFCC) technique for CCM boost PFC converter to improved PF in high line voltage and light load conditions[J].IEEE Transactions on Power Electronics,2016,31(9):6596-6606.

[9] LIM J,HONG D.Cost reference particle filtering approach to high-bandwidth tilt estimation[J].IEEE Transaction on Industrial Electronics,2010,57(11):3830-3839.

[10] 董唯光,李言俊,張科.改進(jìn)的CRPF在自主天文導(dǎo)航中的應(yīng)用[J].計(jì)算機(jī)測(cè)量與控制,2008,16(11):1682-1684,1688.

[11] 王秋平,周原,康順,等.改進(jìn)權(quán)值計(jì)算的EPF算法及在目標(biāo)跟蹤中的應(yīng)用[J].電光與控制,2011,18(4):10-12,25.

[12] 申正義,王晴晴,許誠(chéng),等.UPF改進(jìn)算法在單站無(wú)源定位中的應(yīng)用[J].雷達(dá)科學(xué)與技術(shù),2014,12(6):592-596,603.

Square-RootCubatureCost-ReferenceParticleFilterAlgorithm

WU Qing-hai1, QU Zhao-yang2

(1.School of Electrical and Information Engineering,Jilin Agricultural Science and Technology University,Jilin 132101,China;2.School of Information Engineering,Northeast Dianli University,Jilin 132012,China)

To improve the filtering precision under complex noise condition,a new Square-root Cubature Cost-Reference Particle Filter (SCCRPF) is proposed based on the Square-root Cubature Kalman Filter (SCKF) and the Cost-Reference Particle Filter (CRPF).The proposed filter updates the prior distribution function with the latest measured information and SCKF,and thereby generates the importance density function for CRPF.The new filter not only reserves the precision advantage of SCKF in filtering nonlinear systems,but also possesses the filtering precision of CRPF for dealing with systems with an unknown noise assumption.Simulation results show that:the filtering precision of SCCRPF is higher than that of Square-root Cubature Particle Filter (SCPF) for a system with unknown noise assumption,and is higher than that of CRPF for a system with known noise assumption.

state estimation; nonlinear system; non-Gaussian system; cost-reference particle filter

武青海，曲朝陽(yáng).均方根容積代價(jià)參考粒子濾波算法[J].電光與控制，2017，24( 11) : 28-30，42.WU Q H，QU Z Y.Square-root cubature cost-reference particle filter algorithm[J].Electronics Optics & Control，2017，24( 11) : 28-30，42．

2017-01-03

2017-01-16

吉林省教育科學(xué)重點(diǎn)項(xiàng)目(ZD115088)

武青海(1978—),男，黑龍江雙城人，碩士，講師，研究方向?yàn)樗惴ǚ治雠c設(shè)計(jì)、軟件工程。

TP391.41

A

10.3969/j.issn.1671-637X.2017.11.006