[發(fā)明專利]一種基于集合卡爾曼濾波的粒子濾波方法

無效

• 申請?zhí)枺?/td>: 201110048138.5
• 申請日: 2011-02-28
• 公開（公告）號: CN102082560A
• 公開（公告）日: 2011-06-01
• 發(fā)明（設(shè)計(jì)）人: 杜航原;趙玉新;李剛;沈志峰
• 申請（專利權(quán)）人: 哈爾濱工程大學(xué)
• 主分類號: H03H21/00
• 分類號: H03H21/00
• 代理公司: 北京永創(chuàng)新實(shí)專利事務(wù)所 11121
• 代理人: 官漢增

1.一種基于集合卡爾曼濾波的粒子濾波方法，其特征在于：包括以下幾個步驟：

步驟一：在k＝0時刻初始化粒子，利用先驗(yàn)概率p(x₀)進(jìn)行隨機(jī)采樣，獲得初始采樣粒子初始化每個采樣粒子的權(quán)值為1/N，i代表粒子序號，i＝1∶N，N為采樣粒子個數(shù)，k為非線性系統(tǒng)的運(yùn)行時刻，為k＝0時刻第i個采樣粒子的狀態(tài)值；對于k＝0時刻每一個采樣粒子定義初始的分析集合為其中表示k＝0時刻第i個粒子對應(yīng)的分析集合中第j個樣本，j代表分析集合中樣本序號，表示以k＝0時刻第i個粒子狀態(tài)值為均值，以系統(tǒng)過程噪聲協(xié)方差Q為方差的高斯分布；

步驟二：(1)在k＝1時刻，對每一個采樣粒子分別定義一個背景集合為和一個分析集合為其中上標(biāo)a和b分別代表分析集合和背景集合，j代表分析集合或背景集合中樣本序號，n表示分析集合或背景集合中樣本的個數(shù)，表示k時刻背景集合中第j個樣本，表示k時刻分析集合中第j個樣本；

(2)利用非線性系統(tǒng)的狀態(tài)方程由k-1時刻的分析集合對當(dāng)前背景集合中每個樣本進(jìn)行預(yù)測，得到公式(3)

xk,j(i),b=f(xk-1,j(i),a)---(3)]]>

為非線性系統(tǒng)狀態(tài)方差，表示k-1時刻分析集合中第j個樣本。

計(jì)算k時刻背景集合的中所有樣本的均值非線性系統(tǒng)關(guān)于狀態(tài)變量的協(xié)方差以及觀測方程的協(xié)方差得到公式(4)、(5)和(6)：

x^k(i),b=1nΣj-1nxk,j(i),b---(4)]]>

P^xh(i),k=1n-1Σj=1n(xk,j(i),b-x^k(i),b)(h(xk,j(i),b)-h(x^k(i),b))T---(5)]]>

P^hh(i)k=1n-1Σj=1n(h(xk,j(i),b)-h(x^k(i),b))(h(xk,j(i),b)-h(x^k(i),b))T---(6)]]>

h(·)表示非線性系統(tǒng)的觀測方程函數(shù)；

(3)根據(jù)公式(7)計(jì)算卡爾曼增益

Kk(i)=P^xh(i),k(P^hh(i),k+Rk)-1---(7)]]>

其中為卡爾曼增益，R_k為觀測噪聲的協(xié)方差矩陣；

(4)融合最近觀測信息，對k時刻背景集合進(jìn)行更新，計(jì)算得到k時刻的分析集合：

xk,j(i),a=xk,j(i),b+Kk(i)(yk,j-h(xk,j(i),b))---(8)]]>

其中y_k，j是以觀測值y_k為均值、R_k為方差的高斯分布的采樣；

根據(jù)公式(9)和(10)，計(jì)算分析集合的均值和協(xié)方差

x^k(i)a=1nΣj-1nxk,j(i)a---(9)]]>

P^k(i)a=1n-1Σj=1n(xk,j(i)a-x^k(i)a)(xk,j(i)a-x^k(i)a)T---(10)]]>

這個過程利用上一時刻的狀態(tài)變量的值和當(dāng)前時刻的最近觀測信息，通過集合卡爾曼濾波方法對建議分布更新后的分析集合均值和方差進(jìn)行傳遞，得到該時刻的高斯建議分布函數(shù)其中表示k時刻第i個粒子，表示k-1時刻第i個粒子，z_k表示k時刻觀測向量；

步驟三：從步驟二得到的高斯建議分布函數(shù)中進(jìn)行采樣，根據(jù)公式(11)得到當(dāng)前時刻的二次采樣粒子，表示k時刻第i個樣本的二次采樣樣本，按照公式(12)計(jì)算每個二次采樣粒子的權(quán)值對權(quán)值進(jìn)行歸一化處理，得到歸一化后的權(quán)值

x^k(i)~q(xk(i)|xk-1(i),zk)=N(x^k(i),a,P^k(i),a)]]>

ωk(i)=ωk-1(i)p(zk|x^k(i))p(x^k(i)|xk-1(i))q(x^k(i)|x0:k-1(i),z1:k)---(12)]]>

步驟四：依照二次采樣粒子的權(quán)值大小對步驟三中得到的二次粒子進(jìn)行重采樣，得到三次采樣粒子；

步驟五：待估計(jì)非線性離散系統(tǒng)的運(yùn)行時間為M，若k＝M則輸出濾波結(jié)果，若k＜M，則迭代過程未完成，進(jìn)入k+1時刻，重復(fù)步驟二至步驟四，直至非線性系統(tǒng)運(yùn)行結(jié)束，使用采樣加權(quán)平均值E(x_0:k)作為每一時刻算法輸出值：

E(x0:k)=∫x0:k·p(x0:k|z1:k)dx0:k≈1NΣi=1Nx0:k(i)---(13)]]>

輸出值E(x_0:k)是對非線性系統(tǒng)進(jìn)行濾波后的狀態(tài)變量的估計(jì)值，其中0:k表示從0時刻到k時刻；x_0:k表示非線性系統(tǒng)從0到k時刻的狀態(tài)值；p(x_0:k|z_1:k)表示非線性系統(tǒng)從0到k時刻的后驗(yàn)概率密度；表示第i個樣本從0到k時刻的狀態(tài)變量的值。