式的結(jié)果根據(jù)公式(8)計(jì)算變換隨機(jī)變量XWlk的協(xié)方差:
[0043] 進(jìn)一步地,步驟四所述的基于步驟四的狀態(tài)預(yù)測和測量方程,使用MIS計(jì)算狀態(tài)預(yù) 測的量測的分布特征可分為W下二個(gè)步驟:
[0044] 步驟(一):根據(jù)上一步的狀態(tài)估計(jì)隨機(jī)變量分布特征,即均值兩協(xié)方差Pk+i|k, .一知+1 克 Tjr 利用公式(9)確定Sigm a樣本點(diǎn)義…,對(duì)應(yīng)的權(quán)重為巧C = 1,2,…C,
[0046] 步驟(二):隨機(jī)變量量測方程變換的分布特征計(jì)算:根據(jù)變換函數(shù),計(jì)算Sigma點(diǎn) 經(jīng)過量測方程變換后的變換Sigma點(diǎn)>對(duì)應(yīng)的權(quán)重為巧。,W。根據(jù)測量方程,使用公式 (10)計(jì)算方變換點(diǎn):
[0047] 產(chǎn)1:'=々(<:'!')……狂媒
[0048] 然后,使用公式(11)計(jì)算變換隨機(jī)變量ZWlk的均值向量:
[0050]利用公式結(jié)果使用公式(12)計(jì)算變換隨機(jī)變量ZW的協(xié)方差:
[0052]根據(jù)公式、樣本點(diǎn)使用公式(13)計(jì)算變換隨機(jī)變量XWlk與ZWlk的協(xié)方差:
[0054] 進(jìn)一步地,步驟六所述的使用卡曼增益融合狀態(tài)預(yù)測W及測量數(shù)據(jù)計(jì)算最優(yōu)狀態(tài) 的分布特征,完成非線性系統(tǒng)一步估計(jì)任務(wù)的具體過程為:
[0055] 分別使用公式和公式(15)計(jì)算XW的均值與協(xié)方差:
[005引 A+"i-+1 二、i+i|/' +.吟+1.(?" -&+.恥)(化)
[0化7] 巧+胖+.1二與:擇-馬+1巧;片1巾A+1 (巧)
[005引式中馬,1 = 恥堪;U,為卡曼增益;ZW是實(shí)際巧慢數(shù)據(jù)值。
[0059] 更進(jìn)一步地,在上述步驟=、步驟四和步驟五中W及其進(jìn)一步描述中是使用相同 的樣本半徑、樣本類別數(shù)W及權(quán)重,即在濾波進(jìn)程中,樣本半徑和權(quán)重是不變的。因此,該樣 本半徑和權(quán)重可W離線計(jì)算存儲(chǔ),W簡化濾波復(fù)雜度和計(jì)算量。
[0060] 另外,本發(fā)明還提供了一種高斯濾波器,其通過上述基于多層重要性采樣的高斯 濾波方法來實(shí)現(xiàn)濾波。
[0061] 如上所述,本發(fā)明具有W下有益效果:本發(fā)明充分利用協(xié)方差楠圓的幾何特征,將 重要性采樣和多層采樣結(jié)合,獲得合理的重要性要本和非負(fù)權(quán)重,使得濾波穩(wěn)定性W及濾 波精度顯著提高。同時(shí),該方法還可W根據(jù)計(jì)算復(fù)雜度需要,調(diào)整采樣的層數(shù)和樣本類別, 得到所需的濾波性能。
【附圖說明】
[0062] 圖1為本發(fā)明一種基于多層重要性采樣的高斯濾波方法的實(shí)現(xiàn)流程圖。
[0063] 圖2為基于本發(fā)明提供的MISKF方法與基于UKF、CKF方法的目標(biāo)跟蹤應(yīng)用中對(duì)位置 估計(jì)的均方誤差曲線圖。
[0064] 圖3為基于圖本發(fā)明提供的MISKF方法與基于UKF、CKF方法的目標(biāo)跟蹤應(yīng)用中對(duì)目 標(biāo)速度估計(jì)的均方誤差曲線圖。
[0065] 圖4為基于圖本發(fā)明提供的MISKF方法與基于UKF、CKF方法的目標(biāo)跟蹤應(yīng)用中對(duì)目 標(biāo)角速度估計(jì)的均方誤差曲線圖。
[0066] 元件標(biāo)號(hào)說明
[0067] SlOl ~Sll 步驟
【具體實(shí)施方式】
[0068] W下通過特定的具體實(shí)例說明本發(fā)明的實(shí)施方式,本領(lǐng)域技術(shù)人員可由本說明書 所掲露的內(nèi)容輕易地了解本發(fā)明的其他優(yōu)點(diǎn)與功效。本發(fā)明還可W通過另外不同的具體實(shí) 施方式加 W實(shí)施或應(yīng)用,本說明書中的各項(xiàng)細(xì)節(jié)也可W基于不同觀點(diǎn)與應(yīng)用,在沒有背離 本發(fā)明的精神下進(jìn)行各種修飾或改變。需說明的是,在不沖突的情況下,W下實(shí)施例及實(shí)施 例中的特征可W相互組合。
[0069] 需要說明的是,W下實(shí)施例中所提供的圖示僅W示意方式說明本發(fā)明的基本構(gòu) 想,遂圖式中僅顯示與本發(fā)明中有關(guān)的組件而非按照實(shí)際實(shí)施時(shí)的組件數(shù)目、形狀及尺寸 繪制,其實(shí)際實(shí)施時(shí)各組件的型態(tài)、數(shù)量及比例可為一種隨意的改變,且其組件布局型態(tài)也 可能更為復(fù)雜。
[0070]下面將結(jié)合附圖,對(duì)本發(fā)明的優(yōu)選實(shí)施例進(jìn)行詳細(xì)的描述。
[0071 ]實(shí)施例一
[0072] 在目標(biāo)追蹤過程中,觀測站能夠獲得包含噪聲的目標(biāo)方位信息,而方位信息與待 估的目標(biāo)位置信息間關(guān)系為非線性,通常應(yīng)用EKF、二階UKF、3階CKF或5階CK巧自線性濾波方 法來獲得目標(biāo)的運(yùn)動(dòng)狀態(tài)。但在目標(biāo)定位要求較高的場合上述濾波方法不能滿足要求。本 發(fā)明提供的方法比現(xiàn)有的方法具備更高的估計(jì)精度,能夠提高目標(biāo)跟蹤的精度。下面W具 體實(shí)施例子來說明本發(fā)明的優(yōu)越性。請(qǐng)參見圖1,為具體如下:
[0073] SlOl:依據(jù)純方位目標(biāo)跟蹤問題,建立如下描述目標(biāo)跟蹤系統(tǒng)的狀態(tài)方程和觀測 方程:
[0076] 其中,第k步的狀態(tài)方程中= [.V.Vn 0''i,巧r,Xk和yk分別表示X坐標(biāo)軸和y坐 標(biāo)軸平面內(nèi)(笛卡爾坐標(biāo)系)目標(biāo)的位置,馬和知分別表示X坐標(biāo)軸和y坐標(biāo)軸方向的速度; Ok表示目標(biāo)的角速度;隨機(jī)系統(tǒng)噪聲向量Wk服從均值為零,方差為Qk的高斯分布;隨機(jī)量測 噪聲向量Vk服從均值為0,方差為化的高斯分布。而且有:
[007引其中,At為采樣間隔,本次實(shí)例中為Is。
[0079] S103:初始狀態(tài):確定系統(tǒng)初始狀態(tài),即初始狀態(tài)的隨機(jī)分布特征,包括其均值、協(xié) 方差:
[0080] 初始狀態(tài):XO= [ 1000 m,300m/s,1000 m,Om/s,-3。/s ]T;
[0081 ]初始協(xié)方差矩陣P〇 = diag[ 100m2,10m2/s2,100m2,10m^s2,IOOmra少/s2] ,Po表征了 系統(tǒng)初始位置的不確定性。
[0082] S105:根據(jù)計(jì)算復(fù)雜度、濾波精度W及穩(wěn)定性的要求,初始化樣本層數(shù)L = 4和樣本 類別數(shù)C=I,然后基于樣本層數(shù)和樣本類別數(shù),計(jì)算每層樣本點(diǎn)的權(quán)重和矩匹配參數(shù)。
[0083] 具體為,根據(jù)S105確定的層數(shù)L和公式(2),計(jì)算出每層預(yù)樣本的半徑然后根 據(jù)公式(3),將\心7代入求解每層樣本的權(quán)重巧。根據(jù)公式(4),求解出匹配參數(shù)0。
[0084] S107:基于上一時(shí)刻的狀態(tài)估計(jì)和狀態(tài)方程,使用MIS計(jì)算一步狀態(tài)預(yù)測的隨機(jī)變 量的分布特征。
[0085] 假設(shè)在k時(shí)刻,狀態(tài)的隨機(jī)分布特征為均值馬I*、協(xié)方差Pklk,則將上述參數(shù)代入公 式(5),得到所采集的樣本>對(duì)應(yīng)權(quán)重為巧^ U。
[0086] 隨機(jī)變量狀態(tài)方程變換的分布特征計(jì)算:根據(jù)變換函數(shù),根據(jù)公式(5)計(jì)算得到的 樣本點(diǎn)經(jīng)過狀態(tài)方程變換后的變換樣本點(diǎn)對(duì)應(yīng)的權(quán)重為巧。,。然后,根據(jù)公式(7) 計(jì)算變換隨機(jī)變量XWlk的均值向量^叫*,:根據(jù)公式(8)計(jì)算變換隨機(jī)變量XWlk的協(xié)方差 Pk+l |k〇
[0087] S109:-步量測預(yù)測:基于S105的狀態(tài)預(yù)測和測量方程,使用MIS計(jì)算狀態(tài)預(yù)測的 量測的分布特征。
[0088] 具體為,根據(jù)S105的狀態(tài)估計(jì)隨機(jī)變量的特征,即馬+M-、協(xié)方差Pk+i|k,根據(jù)公式(9) 確定樣本點(diǎn)對(duì)應(yīng)的權(quán)重為巧。
[0089] 根據(jù)公式(10),計(jì)算Sigma點(diǎn)經(jīng)過量測方程變換后的變換Sigma點(diǎn)I對(duì)應(yīng)的權(quán) 重為巧。如。然后根據(jù)公式(11)和(12),分別計(jì)算變換隨機(jī)變量Zbi Ik的均值向量耳*蝸和協(xié) 方差矩陣Pzz,k+l|k。同時(shí),根據(jù)公式(13),計(jì)算變換隨機(jī)變量Xk+l|k與Zk+l|k的互協(xié)方差Pxz,k+l|k。
[0090] sill:狀態(tài)濾波更新:使用卡曼增益化alman Gain)融合狀態(tài)預(yù)測W及測量數(shù)據(jù)計(jì) 算最優(yōu)狀態(tài)的分布特征,完成非線性系統(tǒng)一步估計(jì)任務(wù),并迭代回到S105,進(jìn)行下一時(shí)刻估 計(jì)任務(wù)。
[0091 ] 具體為:根據(jù)公式