亚洲狠狠干,亚洲国产福利精品一区二区,国产八区,激情文学亚洲色图

迭代平方根中心差分卡爾曼粒子濾波方法

文檔序號(hào):7517020閱讀:967來(lái)源:國(guó)知局
專利名稱:迭代平方根中心差分卡爾曼粒子濾波方法
技術(shù)領(lǐng)域
本發(fā)明涉及一種卡爾曼粒子濾波方法,特別是迭代平方根中心差分卡爾曼粒子濾波方法。

背景技術(shù)
文獻(xiàn)“閃爍噪聲環(huán)境下基于粒子濾波的目標(biāo)跟蹤,聲學(xué)技術(shù),2009,Vol.28(4),p29-32”公開(kāi)了一種粒子濾波方法Sigma粒子濾波SPPF。該方法是利用平方根無(wú)跡卡爾曼濾波(SRUKF)來(lái)更新粒子均值和方差

Pki,從而用高斯近似的方法作為建議分布密度函數(shù)

其中N(·)表示高斯函數(shù)。該方法雖然可以取得較快的濾波收斂性和較高的濾波精度,但是需要說(shuō)明的是,在非線性、非高斯系統(tǒng)的目標(biāo)跟蹤中,該方法存在跟蹤精度較低,均值為0.4689、方差為0.1664、效率較低、穩(wěn)定性較差等問(wèn)題。


發(fā)明內(nèi)容
為了克服現(xiàn)有的卡爾曼濾波方法跟蹤精度低的不足,本發(fā)明提供一種迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)。該方法利用迭代平方根中心差分卡爾曼濾波方法(ISRCDKF)來(lái)產(chǎn)生建議分布,可以使其更好的逼近后驗(yàn)概率密度,達(dá)到很高的跟蹤精度,同時(shí)具有很好的穩(wěn)定性。
本發(fā)明解決其技術(shù)問(wèn)題所采用的技術(shù)方案一種迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF),其特點(diǎn)是包括下述步驟 (a)根據(jù)非線性、非高斯系統(tǒng)目標(biāo)跟蹤的特點(diǎn),采用Levenberg-Marquardt優(yōu)化方法,修正狀態(tài)協(xié)方差; 系統(tǒng)方程 量測(cè)方程 式中,f和h為系統(tǒng)狀態(tài)和量測(cè)的非線性變換,xk,yk分別是系統(tǒng)k時(shí)刻的狀態(tài)值和量測(cè)值,wk,vk分別是系統(tǒng)k時(shí)刻的狀態(tài)噪聲和測(cè)量噪聲; 對(duì)基于線性化方法的卡爾曼濾波,定義代價(jià)函數(shù)C(xk)為 (b)利用迭代的平方根CDKF和步驟(a)共同來(lái)產(chǎn)生建議分布,建立迭代平方根中心差分卡爾曼粒子濾波方法; 1)初始值 2)構(gòu)造2L+1維Sigma點(diǎn) 3)時(shí)間更新 χk|k-1=f(χk-1) (6) 4)測(cè)量更新 for i=1m yk|k-1=g(χk|k-1) (13) end 式中,Rw為狀態(tài)過(guò)程噪聲協(xié)方差矩陣,Rv為測(cè)量噪聲協(xié)方差矩陣;h≥1為中心差分區(qū)間長(zhǎng)度,w1=1/(4h2)和w2=(h2-1)/(4h4),L是狀態(tài)向量的維數(shù),Levenberg-Marquardt參數(shù)μi的選為0.1; 迭代的平方根CDKF粒子濾波方法(ISPPF), 1)當(dāng)k=0時(shí),采樣粒子

i=1,...,N。
2)重要性采樣 將初始粒子代入迭代的平方根CDKF中產(chǎn)生重要性函數(shù)分布,并采樣粒子 3)重要性權(quán)值 當(dāng)i=2,...,N時(shí), 歸一化權(quán)值 4)重采樣 從xki中根據(jù)重要性權(quán)值重新采樣得到新的N個(gè)粒子

并重新分配權(quán)值 本發(fā)明的有益效果是由于利用迭代的平方根CDKF和引入Levenberg-Marquardt優(yōu)化方法共同來(lái)產(chǎn)生建議分布,粒子濾波(PF)、迭代擴(kuò)展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF、迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)粒子數(shù)均為100,迭代平方根中心差分卡爾曼粒子濾波方法可以使跟蹤精度提高至少30多倍,均值為0.0147,方差為0.00000394,還具有很好的穩(wěn)定性。
下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明作詳細(xì)說(shuō)明。



圖1是利用本發(fā)明方法對(duì)目標(biāo)狀態(tài)估計(jì)的曲線圖。
圖2是利用本發(fā)明方法進(jìn)行100次蒙特卡羅仿真的RMSE曲線圖。

具體實(shí)施例方式 以下詳細(xì)說(shuō)明迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF) (a)根據(jù)非線性、非高斯系統(tǒng)目標(biāo)跟蹤的特點(diǎn),引入Levenberg-Marquardt優(yōu)化方法,修正狀態(tài)協(xié)方差; 系統(tǒng)方程 量測(cè)方程 其中f和h為系統(tǒng)狀態(tài)和量測(cè)的非線性變換,xk,yk分別是系統(tǒng)k時(shí)刻的狀態(tài)值和量測(cè)值,wk,vk分別是系統(tǒng)k時(shí)刻的狀態(tài)噪聲和測(cè)量噪聲。
對(duì)基于線性化方法的卡爾曼濾波,定義代價(jià)函數(shù)C(xk)為 觀測(cè)更新的迭代過(guò)程可看作是利用Gauss-Newton方法迭代求解函數(shù)C(xk)的極小值點(diǎn)。然而,由于線性化等誤差的引入,狀態(tài)空間模型與觀測(cè)數(shù)據(jù)未必完全一致,因此只能求出函數(shù)C(xk)的非零極值點(diǎn),這等價(jià)于將Gauss-Newton方法應(yīng)用于小殘差問(wèn)題的迭代優(yōu)化。對(duì)于這類問(wèn)題,Gauss-Newton方法往往性能不穩(wěn)定,主要表現(xiàn)為對(duì)狀態(tài)的觀測(cè)更新不能保障估計(jì)誤差的一致減少,對(duì)協(xié)方差陣的估計(jì)值比真實(shí)值偏低,進(jìn)而影響對(duì)觀測(cè)信息的有效利用。為了增強(qiáng)算法的穩(wěn)定性,本發(fā)明利用Levenberg-Marquardt方法調(diào)整預(yù)測(cè)協(xié)方差陣,以保證算法具有全局收斂性。該方法的核心是在每次迭代過(guò)程中,使用參數(shù)μi對(duì)預(yù)測(cè)協(xié)方差陣進(jìn)行修正,即調(diào)整協(xié)方差陣為

然后,以修正的協(xié)方差陣

進(jìn)行迭代觀測(cè)更新。
(b)利用迭代的平方根CDKF和步驟(a)共同來(lái)產(chǎn)生建議分布,建立迭代平方根中心差分卡爾曼粒子濾波方法; 基于Levenberg-Marquardt優(yōu)化方法的迭代SRCDKF濾波方法(ISRCDKF),具體步驟如下 1)初始值 2)構(gòu)造2L+1維Sigma點(diǎn) 3)時(shí)間更新 χk|k-1=f(χk-1) (6) 4)測(cè)量更新 for i=1m yk|k-1=g(χk|k-1) (13) end 其中Rw為狀態(tài)過(guò)程噪聲協(xié)方差矩陣,Rv為測(cè)量噪聲協(xié)方差矩陣。h≥1為中心差分區(qū)間長(zhǎng)度,w1=1/(4h2)和w2=(h2-1)/(4h4),L是狀態(tài)向量的維數(shù),Levenberg-Marquardt參數(shù)μi的選為0.1。
由以上推導(dǎo)可知,迭代的平方根CDKF能得到狀態(tài)的最大后驗(yàn)概率估計(jì),其性能要優(yōu)于傳統(tǒng)的UKF、IEKF。所以用迭代的平方根CDKF產(chǎn)生建議分布,將更符合狀態(tài)變量的實(shí)際后驗(yàn)概率分布。
于是迭代的平方根CDKF粒子濾波方法(ISPPF),具體步驟如下 1)當(dāng)k=0時(shí),采樣粒子

i=1,...,N。
2)重要性采樣 將初始粒子代入迭代的平方根CDKF中產(chǎn)生重要性函數(shù)分布,并采樣粒子 3)重要性權(quán)值 當(dāng)i=2,...,N時(shí), 歸一化權(quán)值 4)重采樣 從xki中根據(jù)重要性權(quán)值重新采樣得到新的N個(gè)粒子

并重新分配權(quán)值 (c)在非線性、非高斯模型中,運(yùn)用迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)進(jìn)行實(shí)驗(yàn),給出該方法與粒子濾波(PF)、迭代擴(kuò)展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF方法的仿真結(jié)果,分析其跟蹤性能和均方根誤差。
考慮一個(gè)非線性、非高斯模型 xk=1+sin(0.4πk)+0.5xk-1+vk-1 (25) 這里vk服從Gamma(3,2)分布,觀測(cè)噪聲nk服從高斯分布N(0,10-5),目標(biāo)的初始狀態(tài)x0=1,經(jīng)過(guò)100次蒙特卡羅仿真,每次仿真時(shí)間是60s,采樣間隔為1s。1次獨(dú)立實(shí)驗(yàn)的均方根誤差定義為 其中粒子數(shù)均為100。不同濾波方法對(duì)目標(biāo)狀態(tài)的估計(jì)曲線圖如圖1所示。100次蒙特卡羅仿真的RMSE曲線圖如圖2所示。
從圖1、圖2可以看出,在非線性、非高斯環(huán)境中,由于PF中沒(méi)有包含最新的測(cè)量信息,所以它受噪聲的干擾比較大,算法不穩(wěn)定,跟蹤性能不好。SPPF生成的建議分布中有效地利用了測(cè)量信息,具有較高的精確度。而本發(fā)明中提出的ISPPF由迭代的平方根CDKF和步驟(a)共同來(lái)產(chǎn)生建議分布,它比其它的方法更能有效的利用測(cè)量信息。表1給出了重復(fù)進(jìn)行100次蒙特卡羅實(shí)驗(yàn)得到的ISPPF與擴(kuò)展卡爾曼濾波EKF、無(wú)跡卡爾曼濾波UKF、粒子濾波PF、Sigma粒子濾波SPPF性能對(duì)比統(tǒng)計(jì)結(jié)果。可以看出它們對(duì)狀態(tài)估計(jì)的均方根誤差(RMSE)均值和方差的變化。顯然,粒子濾波方法(ISPPF)具有很好的穩(wěn)定性,是一種效率高、性能好的跟蹤方法。
表1 100次蒙特卡羅仿真的RMSE的均值和方差
權(quán)利要求
1.一種迭代平方根中心差分卡爾曼粒子濾波方法,其特征在于包括下述步驟
(a)根據(jù)非線性、非高斯系統(tǒng)目標(biāo)跟蹤的特點(diǎn),采用Levenberg-Marquardt優(yōu)化方法,修正狀態(tài)協(xié)方差;
系統(tǒng)方程
量測(cè)方程
式中,f和h為系統(tǒng)狀態(tài)和量測(cè)的非線性變換,xk,yk分別是系統(tǒng)k時(shí)刻的狀態(tài)值和量測(cè)值,wk,vk分別是系統(tǒng)k時(shí)刻的狀態(tài)噪聲和測(cè)量噪聲;
對(duì)基于線性化方法的卡爾曼濾波,定義代價(jià)函數(shù)C(xk)為
(b)利用迭代的平方根CDKF和步驟(a)共同來(lái)產(chǎn)生建議分布,建立迭代平方根中心差分卡爾曼粒子濾波方法;
1)初始值
2)構(gòu)造2L+1維Sigma點(diǎn)
3)時(shí)間更新
χk|k-1=f(χk-1)(6)
4)測(cè)量更新
for i=1:m
uk|k-1=g(χk|k-1) (13)
end
式中,Rw為狀態(tài)過(guò)程噪聲協(xié)方差矩陣,Rv為測(cè)量噪聲協(xié)方差矩陣;h≥1為中心差分區(qū)間長(zhǎng)度,w1=1/(4h2)和w2=(h2-1)/(4h4),L是狀態(tài)向量的維數(shù),Levenberg-Marquardt參數(shù)μi的選為0.1;
迭代的平方根CDKF粒子濾波方法(ISPPF),
1)當(dāng)k=0時(shí),采樣粒子
i=1,...,N。
2)重要性采樣
將初始粒子代入迭代的平方根CDKF中產(chǎn)生重要性函數(shù)分布,并采樣粒子
3)重要性權(quán)值
當(dāng)i=2,...,N時(shí),
歸一化權(quán)值
4)重采樣
從xki中根據(jù)重要性權(quán)值重新采樣得到新的N個(gè)粒子
并重新分配權(quán)值
全文摘要
本發(fā)明公開(kāi)了一種迭代平方根中心差分卡爾曼粒子濾波方法,其目的是解決現(xiàn)有的卡爾曼濾波方法跟蹤精度低的技術(shù)問(wèn)題。技術(shù)方案是根據(jù)非線性、非高斯系統(tǒng)目標(biāo)跟蹤的特點(diǎn),引入Levenberg-Marquardt優(yōu)化方法,修正狀態(tài)協(xié)方差;利用迭代的平方根CDKF和所引入Levenberg-Marquardt優(yōu)化方法共同來(lái)產(chǎn)生建議分布,建立迭代平方根中心差分卡爾曼粒子濾波方法。由于利用迭代的平方根CDKF和引入Levenberg-Marquardt優(yōu)化方法共同來(lái)產(chǎn)生建議分布,粒子濾波(PF)、迭代擴(kuò)展卡爾曼粒子濾波(IEKFPF)、Sigma粒子濾波SPPF、迭代平方根中心差分卡爾曼粒子濾波方法(ISPPF)粒子數(shù)均為100,迭代平方根中心差分卡爾曼粒子濾波方法可以使跟蹤精度提高至少30多倍,均值為0.0147,方差為0.00000394,還具有很好的穩(wěn)定性。
文檔編號(hào)H03H21/00GK101820269SQ20101014227
公開(kāi)日2010年9月1日 申請(qǐng)日期2010年4月8日 優(yōu)先權(quán)日2010年4月8日
發(fā)明者李國(guó)輝, 李亞安, 何健, 楊宏, 崔琳 申請(qǐng)人:西北工業(yè)大學(xué)
網(wǎng)友詢問(wèn)留言 已有0條留言
  • 還沒(méi)有人留言評(píng)論。精彩留言會(huì)獲得點(diǎn)贊!
1