專利名稱：適用于非線性系統(tǒng)狀態(tài)的粒子估計(jì)方法
技術(shù)領(lǐng)域：
本發(fā)明屬于工程領(lǐng)域，具體涉及的是非線性系統(tǒng)狀態(tài)估計(jì)的技術(shù)領(lǐng)域。
技術(shù)背景非線性系統(tǒng)狀態(tài)估計(jì)問(wèn)題廣泛存在于信號(hào)處理及其相關(guān)領(lǐng)域中，例如視覺(jué)跟 蹤、語(yǔ)音識(shí)別、機(jī)器人定位、通信和故障檢測(cè)等。盡管經(jīng)過(guò)了近半個(gè)世紀(jì)的發(fā) 展，但非線性系統(tǒng)狀態(tài)估計(jì)方法還較不成熟。
一方面，這是由于非線性系統(tǒng)的復(fù)雜性、多樣性所造成的；另一方面，由于非線性系統(tǒng)中各種隨機(jī)噪聲的存在， 導(dǎo)致?tīng)顟B(tài)估計(jì)更加困難，影響了狀態(tài)估計(jì)的準(zhǔn)確率，為非線性系統(tǒng)狀態(tài)估計(jì)帶 來(lái)了較大的困難。20世紀(jì)80年代以來(lái)，非線性系統(tǒng)狀態(tài)估計(jì)方法受到越來(lái)越多的 關(guān)注，其中最常用的是擴(kuò)展卡爾曼濾波器。但擴(kuò)展卡爾曼濾波僅對(duì)某些特定的 非線性系統(tǒng)有效，對(duì)于一般的非線性系統(tǒng)擴(kuò)展卡爾曼濾波不能保證其收斂且濾 波誤差較大。近年來(lái)一種在非線性系統(tǒng)狀態(tài)估計(jì)方面^r有獨(dú)到優(yōu)勢(shì)的新濾波方 法一粒子濾波得到了迅速的發(fā)展，它有效克服了擴(kuò)展卡爾曼濾波的缺點(diǎn)，并已被 成功應(yīng)用于眾多非線性系統(tǒng)狀態(tài)估計(jì)問(wèn)題中，例如，美國(guó)國(guó)家航空和宇宙航行 局研制的"火星漫游者"機(jī)器人、艦船渦輪發(fā)動(dòng)機(jī)、渦輪噴氣飛機(jī)發(fā)動(dòng)機(jī)等。 粒子濾波是遞歸貝葉斯?fàn)顟B(tài)估計(jì)的一種模擬實(shí)現(xiàn)方法。它利用大量隨機(jī)樣本(或 稱為粒子)代表待估計(jì)狀態(tài)的后驗(yàn)概率密度函數(shù)。這些粒子通過(guò)系統(tǒng)模型按時(shí) 間順序向前傳播，從而得到大量系統(tǒng)狀態(tài)的路徑樣本。各時(shí)刻系統(tǒng)狀態(tài)的后驗(yàn) 概率密度函數(shù)用這些粒子來(lái)代表。目前比較有實(shí)用價(jià)值的粒子濾波方法有標(biāo)準(zhǔn) 粒子濾波方法(SIR)、輔助粒子濾波方法(APF)、正則化粒子濾波方法(RPF) 和高斯粒子濾波方法(GPF)。其中，輔助粒子濾波方法、正則化粒子濾波方法 和高斯粒子濾波方法均是標(biāo)準(zhǔn)粒子濾波方法的改進(jìn)方法。如果用j^表示/r時(shí)刻的系統(tǒng)狀態(tài)，用"表示j^的觀測(cè)值，用{^(/>:/=1，2，...》^}代 表A時(shí)刻的所有粒子，用/KvW表示觀測(cè)似然函數(shù)，則標(biāo)準(zhǔn)粒子濾波方法的步驟可描述為步驟一、粒子預(yù)測(cè)步驟一將A時(shí)刻系統(tǒng)狀態(tài)先驗(yàn)分布作為重要性密度函數(shù)并從中抽取隨機(jī)粒子^(0:—l，2，…A);步驟二、粒子權(quán)值更新步驟一得到A時(shí)刻系統(tǒng)狀態(tài)觀測(cè)值貝后，利用觀測(cè)似然函數(shù)計(jì)算^時(shí)刻上述各粒子的似然函數(shù)值并將其作為^時(shí)刻各粒子的權(quán)值即涵啡稀):—1，2"" W;步驟三、系統(tǒng)狀態(tài)估計(jì)步驟一利用這些粒子及其權(quán)值組成的離散概率分布作 為A時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似A時(shí)刻系統(tǒng)狀態(tài)的真實(shí)概率分布，并用 于A時(shí)刻系統(tǒng)狀態(tài)估計(jì)；步驟四、重采樣步驟一在上述離散概率分布中重新采樣得到新的粒子群，各 粒子的權(quán)值等于1/N，然后返回步驟一。輔助粒子濾波方法在標(biāo)準(zhǔn)粒子濾波方法的基礎(chǔ)上，采用輔助方法預(yù)先從前一 時(shí)刻的粒子樣本中選出較好的預(yù)測(cè)粒子，以期其粒子組成的離散概率分布接近 系統(tǒng)狀態(tài)真實(shí)概率分布的可能性更高。實(shí)際應(yīng)用中該方法的估計(jì)精度在系統(tǒng)噪 聲較小時(shí)好于標(biāo)準(zhǔn)粒子濾波方法，但當(dāng)系統(tǒng)噪聲較大時(shí)精度不如標(biāo)準(zhǔn)粒子濾波 方法。正則化粒子濾波方法改變了標(biāo)準(zhǔn)粒子濾波方法步驟四中的重采樣。不像 標(biāo)準(zhǔn)粒子濾波方法在離散概率分布上重采樣，正則化粒子濾波方法利用正則化 方法獲得一個(gè)連續(xù)概率分布并從中重采樣。該方法改善了標(biāo)準(zhǔn)粒子濾波方法中 存在的粒子多樣性匱乏問(wèn)題，但它有一個(gè)理論上的缺陷，即所得的粒子離散概 率分布不再能保證是漸進(jìn)地收斂到系統(tǒng)狀態(tài)真實(shí)概率分布。實(shí)際應(yīng)用中當(dāng)粒子 多樣性匱乏問(wèn)題較嚴(yán)重時(shí)該方法好于標(biāo)準(zhǔn)粒子濾波方法。高斯粒子濾波方法在 標(biāo)準(zhǔn)粒子濾波方法的基礎(chǔ)上，采用一個(gè)高斯分布來(lái)近似系統(tǒng)狀態(tài)真實(shí)概率分布， 并且沒(méi)有重采樣步驟。當(dāng)系統(tǒng)狀態(tài)真實(shí)概率分布服從高斯分布時(shí)該方法的狀態(tài) 估計(jì)精度優(yōu)于標(biāo)準(zhǔn)粒子濾波方法。綜上所述，以上四種方法都具有各自的優(yōu)點(diǎn)，在其所適合的應(yīng)用范圍內(nèi)都是 比較有效的。然而在實(shí)際非線性系統(tǒng)狀態(tài)估計(jì)中，系統(tǒng)狀態(tài)觀測(cè)似然函數(shù)常常 具有雙峰特性，這通常導(dǎo)致上述方法的狀態(tài)估計(jì)精度較差。 發(fā)明內(nèi)容本發(fā)明是為解決現(xiàn)有粒子濾波方法在對(duì)非線性系統(tǒng)狀態(tài)進(jìn)行估計(jì)時(shí)，因系 統(tǒng)狀態(tài)觀測(cè)似然函數(shù)具有雙峰特性而導(dǎo)致估計(jì)精度較差的問(wèn)題，進(jìn)而提供的一 種適用于非線性系統(tǒng)狀態(tài)的粒子估計(jì)方法。本發(fā)明的步驟為步驟一、粒子預(yù)測(cè)步驟將A時(shí)刻系統(tǒng)狀態(tài)先驗(yàn)分布作為重要性密度函數(shù) 并從中抽取隨機(jī)粒子{^(/):/=1，2，...^}; 步驟二、粒子權(quán)值更新步驟(1) 得到A時(shí)刻系統(tǒng)狀態(tài)觀測(cè)值力后，利用觀測(cè)似然函數(shù)計(jì)算/^時(shí)刻各粒子 的似然函數(shù)值為(/K^^(,W，其中—l，2，…，N;(2) 計(jì)算各粒子觀測(cè)向量F④和系統(tǒng)狀態(tài)觀測(cè)向量Kt間的夾角^i)，其中/=1，2，.."N;(3) 計(jì)算A時(shí)刻各粒子的權(quán)值^②如下^(/)=,"')Xjp(h I ^ (/)),/ = 1,2，."iV計(jì)算/^￡+/時(shí)刻各粒子的權(quán)值^^+/,)如下』)= x /7d+, I &_』))，/ = 1，2，." iV 其中，"是一個(gè)預(yù)先指定的比例因子; 步驟三、系統(tǒng)狀態(tài)估計(jì)步驟(7J A時(shí)刻系統(tǒng)狀態(tài)的濾波估計(jì)一利用/t時(shí)刻粒子及其權(quán)值組成的離散概 率分布作為&時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似/t時(shí)刻系統(tǒng)狀態(tài) 的真實(shí)概率分布，并用于A時(shí)刻系統(tǒng)狀態(tài)估計(jì)；時(shí)刻系統(tǒng)狀態(tài)的平滑估計(jì)--利用時(shí)刻粒子及其權(quán)值組成 的離散概率分布作為時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似 /^+/時(shí)刻系統(tǒng)狀態(tài)的真實(shí)概率分布，并用于時(shí)刻系統(tǒng)狀態(tài)估 計(jì)；O返回步驟一；用JCA表示A時(shí)刻的系統(tǒng)狀態(tài)，用力表示A的觀測(cè)值，用^(/):/=1，2，...^} 代表A時(shí)刻的所有粒子，用p(vlA:)表示觀測(cè)似然函數(shù)，用{~(/):戶/^:+1，...，/^代 表^￡+1時(shí)刻到A時(shí)刻的所有粒子，用^^=0^+/0，>^+^,...，^朋表示粒子觀測(cè)向量，用「={^4+1，^+2，...，^}表示系統(tǒng)狀態(tài)觀測(cè)向量，用6Y,)表示這兩種 向量的夾角。這里，1和/是兩個(gè)預(yù)先指定的常量且1 《是一個(gè)預(yù)先指 定的比例因子。本發(fā)明的粒子權(quán)值更新步驟和系統(tǒng)狀態(tài)估計(jì)步驟不同于現(xiàn)有的粒子濾波方法，本發(fā)明方法在粒子權(quán)值更新步驟中將粒子觀測(cè)向量和系統(tǒng)狀態(tài)觀測(cè)向量的 夾角作為計(jì)算粒子權(quán)值的參數(shù)，并在系統(tǒng)狀態(tài)估計(jì)步驟中加入了平滑操作。這 些特征都是現(xiàn)有的粒子濾波方法所不具備的。本發(fā)明適用于一般非線性系統(tǒng)狀 態(tài)估計(jì)問(wèn)題的求解，在系統(tǒng)狀態(tài)觀測(cè)似然函數(shù)具有雙峰特性時(shí)該方法優(yōu)于現(xiàn)有 粒子濾波方法，能有效地提高非線性系統(tǒng)狀態(tài)的估計(jì)精度。


圖1是本發(fā)明的方法與SIR、 APF、 RPF和GPF算法的性能對(duì)比實(shí)驗(yàn)效 果圖。
具體實(shí)施方式
具體實(shí)施方式
一本發(fā)明基于下述理論基礎(chǔ)如果一個(gè)粒子的觀測(cè)路徑靠近系統(tǒng)狀態(tài)的觀測(cè) 路徑，則該粒子的路徑應(yīng)靠近系統(tǒng)狀態(tài)的路徑。如果將一個(gè)粒子的一系列觀測(cè) 值和系統(tǒng)狀態(tài)的一系列觀測(cè)值分別看作一個(gè)粒子觀測(cè)向量和系統(tǒng)狀態(tài)觀測(cè)向 量，則可用這兩個(gè)向量的夾角來(lái)度量它們之間的相似程度。當(dāng)該夾角較小時(shí)就 表示該粒子靠近系統(tǒng)狀態(tài)。用A表示A時(shí)刻的系統(tǒng)狀態(tài)，用"表示J^的觀測(cè)值，用^(/):/=1，2，...^}代 表A時(shí)刻的所有粒子，用jKvW表示觀測(cè)似然函數(shù)，用{~(/):7'=^丄+1，...，/^代表yn+i時(shí)刻到&時(shí)刻的所有粒子，用ni:)={^i+^,》-i+/o，...,j^:n表示粒子觀領(lǐng)U向量，用^={^./:+1，>^:+2，...，力}表示系統(tǒng)狀態(tài)觀測(cè)向量，用0④表示這兩種向 量的夾角。這里，￡和/是兩個(gè)預(yù)先指定的常量且1^/<￡， 《是一個(gè)預(yù)先指定 的比例因子。本實(shí)施方式的非線性系統(tǒng)狀態(tài)粒子估計(jì)方法步驟為步驟一、粒子預(yù)測(cè)步驟將/t時(shí)刻系統(tǒng)狀態(tài)先驗(yàn)分布作為重要性密度函數(shù) 并從中抽取隨機(jī)粒子{^/):/=1，2，...>^;步驟二、粒子權(quán)值更新步驟(1) 得到A時(shí)刻系統(tǒng)狀態(tài)觀測(cè)值力后，利用觀測(cè)似然函數(shù)計(jì)算A時(shí)刻各粒子 的似然函數(shù)值為(p(v4^(/)》，其中l(wèi)M，2，.,.，N;(2) 計(jì)算各粒子觀測(cè)向量K②和系統(tǒng)狀態(tài)觀測(cè)向量Kt間的夾角^i)，其中,.=1，2，""N;(3)計(jì)算A時(shí)刻各粒子的權(quán)值<formula>formula see original document page 8</formula>如下<formula>formula see original document page 8</formula>計(jì)算<formula>formula see original document page 8</formula>時(shí)刻各粒子的權(quán)值<formula>formula see original document page 8</formula>如下<formula>formula see original document page 8</formula>其中，《是一個(gè)預(yù)先指定的比例因子； 步驟三、系統(tǒng)狀態(tài)估計(jì)步驟-(7JA時(shí)刻系統(tǒng)狀態(tài)的濾波估計(jì)一利用A時(shí)刻粒子及其權(quán)值組成的離散概 率分布作為A時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似A時(shí)刻系統(tǒng)狀態(tài)的真實(shí) 概率分布，并用于A時(shí)刻系統(tǒng)狀態(tài)估計(jì)；時(shí)刻系統(tǒng)狀態(tài)的平滑估計(jì)一利用時(shí)刻粒子及其權(quán)值組成 的離散概率分布作為時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似時(shí) 刻系統(tǒng)狀態(tài)的真實(shí)概率分布，并用于時(shí)刻系統(tǒng)狀態(tài)估計(jì)；(3)返回步驟一。本具體實(shí)施方式
的非線性系統(tǒng)狀態(tài)粒子估計(jì)方法步驟二中第(2)子步驟所 述的向量K(T^和F之間的夾角0④的計(jì)算步驟為 步驟1、計(jì)算K②和K的內(nèi)積如下步驟2、分別計(jì)算K④和F的范數(shù)如下<formula>formula see original document page 8</formula>遵步驟3、計(jì)算K②和F之間的夾角e②如下廣歐、、<formula>formula see original document page 8</formula>其中，arccos"是反余弦函數(shù)。本方法在被應(yīng)用于如下具有代表性的觀測(cè)似然函數(shù)有雙峰特性的非線性系統(tǒng)中時(shí)，本方法的誤差遠(yuǎn)小于SIR、 APF、 RPF和GPF方法。<formula>formula see original document page 9</formula>使用粒子總數(shù) =100、 W-廣A/(0，1)、 W~A/(0，1)， xQ=ai; —次實(shí)驗(yàn)運(yùn)行時(shí) 間為5000個(gè)時(shí)間點(diǎn)，共進(jìn)行100次實(shí)驗(yàn)運(yùn)行r取/_=5、 /=2和gf1.8;采用誤差均方根值(賜SE)評(píng)價(jià)算法性能。實(shí)驗(yàn)結(jié)果顯示在圖1中，其中PE代表本 方法結(jié)果，SIR代表標(biāo)準(zhǔn)粒子濾波方法，APF代表輔助粒子濾波方法，RPF代表 正則化粒子濾波方法，GPF代表高斯粒子濾波方法。
權(quán)利要求
1、適用于非線性系統(tǒng)狀態(tài)的粒子估計(jì)方法，其特征是它通過(guò)下述步驟實(shí)現(xiàn)步驟一、粒子預(yù)測(cè)步驟將k時(shí)刻系統(tǒng)狀態(tài)先驗(yàn)分布作為重要性密度函數(shù)并從中抽取隨機(jī)粒子{x(i)i＝1，2，...，N}；步驟二、粒子權(quán)值更新步驟(1)得到k時(shí)刻系統(tǒng)狀態(tài)觀測(cè)值yk后，利用觀測(cè)似然函數(shù)計(jì)算k時(shí)刻各粒子的似然函數(shù)值為{p(yk|xk(i))}，其中i＝1，2，...，N；(2)計(jì)算各粒子觀測(cè)向量V(i)和系統(tǒng)狀態(tài)觀測(cè)向量V之間的夾角θ(i)，其中i＝1，2，...，N；(3)計(jì)算k時(shí)刻各粒子的權(quán)值qk(i)如下qk(i)＝eα/θ(i)×p(yk|xk(i))，i＝1，2，..，N計(jì)算k-L+l時(shí)刻各粒子的權(quán)值qk-L+l(i)如下qk-L+l(i)＝eα/θ(i)×p(yk-L+l|xk-L+l(i))，i＝1，2，..，N其中，α是一個(gè)預(yù)先指定的比例因子；步驟三、系統(tǒng)狀態(tài)估計(jì)步驟(1)k時(shí)刻系統(tǒng)狀態(tài)的濾波估計(jì)一利用k時(shí)刻粒子及其權(quán)值組成的離散概率分布作為k時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似k時(shí)刻系統(tǒng)狀態(tài)的真實(shí)概率分布，并用于k時(shí)刻系統(tǒng)狀態(tài)估計(jì)；(2)k-L+l時(shí)刻系統(tǒng)狀態(tài)的平滑估計(jì)一利用k-L+l時(shí)刻粒子及其權(quán)值組成的離散概率分布作為k-L+l時(shí)刻系統(tǒng)狀態(tài)后驗(yàn)概率分布，去近似k-L+l時(shí)刻系統(tǒng)狀態(tài)的真實(shí)概率分布，并用于k-L+l時(shí)刻系統(tǒng)狀態(tài)估計(jì)；(3)返回步驟一；用xk表示k時(shí)刻的系統(tǒng)狀態(tài)，用yk表示xk的觀測(cè)值，用{x(i)i＝1，2，...，N}代表k時(shí)刻的所有粒子，用p(y|x)表示觀測(cè)似然函數(shù)，用{xj(i)j＝k-L+1，...，k}代表k-L+1時(shí)刻到k時(shí)刻的所有粒子，用V(i)＝{yk-L+1(i)，yk-L+2(i)，...，yk(i)}表示粒子觀測(cè)向量，用V＝{yk-L+1，yk-L+2，...，yk}表示系統(tǒng)狀態(tài)觀測(cè)向量，用θ(i)表示這兩種向量的夾角；這里，L和l是兩個(gè)預(yù)先指定的常量且1≤l＜L，α是一個(gè)預(yù)先指定的比例因子。
2、根據(jù)權(quán)利要求1所述適用于非線性系統(tǒng)狀態(tài)的粒子估計(jì)方法， 其特征是步驟二中第(2)子步驟的向量r②和F之間的夾角0②的 計(jì)算步驟為步驟1、計(jì)算F④和K的內(nèi)積如下步驟2、分別計(jì)算F②和K的范數(shù)如下步驟3、計(jì)算F②和K之間的夾角0②如下 W.、 「 ( 、其中，arccos(J是反余弦函數(shù)
全文摘要
適用于非線性系統(tǒng)狀態(tài)的粒子估計(jì)方法。本發(fā)明屬于工程領(lǐng)域，具體涉及到非線性系統(tǒng)狀態(tài)估計(jì)的技術(shù)領(lǐng)域。它是為解決對(duì)非線性系統(tǒng)狀態(tài)進(jìn)行估計(jì)時(shí)，因系統(tǒng)狀態(tài)觀測(cè)似然函數(shù)具有雙峰特性使?fàn)顟B(tài)估計(jì)精度較差的問(wèn)題。它在標(biāo)準(zhǔn)粒子濾波方法的基礎(chǔ)上，在粒子權(quán)值更新步驟中將粒子觀測(cè)向量和系統(tǒng)狀態(tài)觀測(cè)向量的夾角作為計(jì)算粒子權(quán)值的參數(shù)，并在系統(tǒng)狀態(tài)估計(jì)步驟中加入了平滑操作。本發(fā)明適用于一般非線性系統(tǒng)的狀態(tài)估計(jì)問(wèn)題，當(dāng)系統(tǒng)狀態(tài)觀測(cè)似然函數(shù)具有雙峰特性時(shí)，該方法的狀態(tài)估計(jì)精度較高。
文檔編號(hào)G06N7/00GK101261691SQ200810064328
公開日2008年9月10日 申請(qǐng)日期2008年4月18日 優(yōu)先權(quán)日2008年4月18日
發(fā)明者彭喜元, 軍 梁 申請(qǐng)人:哈爾濱工業(yè)大學(xué)