>[0038] (32)
[0039] 步驟三：重采樣
[0040] 為了得到正確的狀態(tài)估計，希望粒子權(quán)值的方差盡可能趨近于零。但在實際計算 中，經(jīng)過數(shù)次迭代，只有少數(shù)粒子的權(quán)值較大，其余粒子的權(quán)值可忽略不計。權(quán)值的方差隨 著時間增大，狀態(tài)空間的有效粒子數(shù)較少。隨著無效采樣粒子數(shù)目的增多，使得大量的計算 浪費在對估計后驗概率密度概率分布幾乎不起作用的粒子更新上，使得估計性能下降。采 樣有效粒子數(shù)乂.來衡量粒子權(quán)值的退化程度，即
[0041 ]
(23:)
[0042] 進行序貫重要性采樣時，若夂小于設(shè)定的閾值，則應(yīng)當(dāng)采用重采樣的方法克服粒 子的權(quán)值退化，重采樣方法舍棄權(quán)值較小的粒子，代之以權(quán)值較大的粒子。重采樣過程滿足
條件下，將粒子集合I#. 更新為
< 本發(fā)明中當(dāng)
，采用基于重 要性采樣的殘差重采樣更新粒子。殘差重采樣具有效率高、實現(xiàn)方便的特點。設(shè)
其中I丨為取整操作。殘余重采樣采用新的權(quán)值
選擇剩余下的

子，殘差重采樣的主要過程為：
[0043] (1)計算剩余粒子的權(quán)值累計量
[0044] (2)生成.?個在[0，1]區(qū)間內(nèi)均勻分布的隨機數(shù)例^
[0045] (3)對于每個μ \尋找歸一化權(quán)值累計量大于或者等于μ 1的最小標(biāo)號m，即λ " i < μ1; λ "。當(dāng)μ1落在區(qū)間[λ η1，λ」時，€被復(fù)制一次。
[0046] 這樣，每個粒子#經(jīng)重采樣后的個數(shù)為步驟（3)中所選中的若干粒子數(shù)目與Ν1。
[0047] 步驟四：對慣性導(dǎo)航系統(tǒng)的狀態(tài)&進行估計
[0048] 貝葉斯估計利用概率分布的形式估計非線性系統(tǒng)的狀態(tài)Xk。根據(jù)現(xiàn)有的觀測量 Yk (Yk表示yi，y2，…，yk)，要估計出重力輔助慣導(dǎo)定位系統(tǒng)的位置信息X k。依據(jù)貝葉斯公式 有：
[0049] (24)
[0050] 由式（16)可知，分母為一個與Xk無關(guān)的歸一化常量，它可以保證p(Xk|Y k)的積分 為1。由式（17)可知測量誤差％的概率密度函數(shù)p (yk I xk)，即為先驗概率密度函數(shù)：
[0051]
[0052] 其中δ (·)為狄拉克函數(shù)。
[0053] 由于觀測值yk只與當(dāng)前狀態(tài)X k有關(guān)，則ρ (X k I xk ρ Yk D = ρ (xk I xk D，根據(jù)全概率 公式可知：
[0054]
[0055] 化簡并進行積分可得CK方程：
[0056]
[0057] 其中，p (xk ! I Yk D假設(shè)已知，p (xk I xk D為狀態(tài)的Markov過程。
[0058] 即根據(jù)最小均方差（MMSE)準(zhǔn)則，將條件均值作為系統(tǒng)狀態(tài)Xk的最優(yōu)估計值，即
[0059]
[0060] 則上式（24) (26) (27)組成了貝葉斯估計系統(tǒng)狀態(tài)Xk的基本公式。式（26)為概 率密度函數(shù)，式（24)、（27)分別為位置更新和測量更新公式。由上述可知，貝葉斯估計需要 進行積分運算，非線性系統(tǒng)多采用近似算法解決積分問題，從而得到狀態(tài)的最優(yōu)估計?；?貝葉斯估計的粒子濾波利用所求狀態(tài)空間中的大量樣本點來近似逼近待估變量的后驗概 率分布，從而將積分問題轉(zhuǎn)換為有限樣本點的求和問題。
[0061] 粒子濾波就是尋找一組在狀態(tài)空間中傳播的隨機樣本對后驗概率密度函數(shù) P (Xk I Yk)進行近似，以樣本均值代替條件均值E [xk IYJ。將服從p (xk I Yk)分布的隨機樣本稱 為粒子。
[0062] 設(shè)#為從后驗概率密度函數(shù)p(Xk|Yk)中獲取的采樣粒子，則對于要估計的慣導(dǎo)位 置X k，條件均值E [xk IYJ可以用求和方式逼近，即
[0063]
[0064] 此時進行步驟（二）中的序貫重要性采樣，生成粒子，當(dāng)采樣粒子數(shù)目很大時，式 (13)便可逼近真實的后驗概率密度。因此要估計的慣導(dǎo)位置X k的條件均值E[xk|Yk]可表 示為：
[0065]
[0066] 此時在進行步驟（三），要估計的慣導(dǎo)位置Xk的條件均值E [X k IYJ可表示為：
[0067]
[0068] 輸出此時的慣導(dǎo)位置xk。
[0069] 步驟五：按照步驟（一）中的Δ Uk更新p (X k I xk J，再根據(jù)步驟（三）更新粒子權(quán) 值，進行k+Ι時刻的慣導(dǎo)位置估計。
[0070] 為了說明本發(fā)明的效果，采用桑迪亞算法在相同的實驗設(shè)置下進行試驗，采用本 發(fā)明后匹配軌跡在匹配區(qū)域內(nèi)對慣導(dǎo)軌跡起到了良好的校正作用，經(jīng)煒度誤差明顯減小， 經(jīng)過多次試驗，本發(fā)明的經(jīng)煒度誤差平均值與傳統(tǒng)桑迪亞算法比較的結(jié)果如表1所示。
[0071] 表1 :誤差結(jié)果
[0072]
[0073] 以上所述僅是本發(fā)明的優(yōu)選實施方式，應(yīng)當(dāng)指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人 員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進，或者對其中部分技術(shù)特征進 行等同替換，這些改進和替換也應(yīng)視為本發(fā)明的保護范圍。
【主權(quán)項】
1. 一種基于貝葉斯估計的粒子濾波重力輔助慣導(dǎo)匹配方法，其特征在于，包括如下步 驟： 步驟一、根據(jù)水下載體的運動規(guī)律，將運載體的經(jīng)煒度信息作為狀態(tài)變量，重力儀實時 測量重力異常值為觀測量，建立重力輔助慣導(dǎo)單點匹配模型：式中，Λ Xkik+1表示k時刻到k+Ι時刻輸出的潛艇的經(jīng)煒度差值，Λ U 慣導(dǎo)系統(tǒng)給出 k時刻的偏移增量，ek慣導(dǎo)系統(tǒng)的誤差，y k表示k時刻的重力儀測量的重力異常信息，h k (Xk) 表示在xk處在重力基準(zhǔn)圖上讀取的重力異常值，vk表示重力異常測量誤差和重力基準(zhǔn)圖誤 差； 步驟二、序貫重要性采樣：假設(shè)從后驗概率密度P (XkYk)中抽出N個獨立同分布的隨機 樣本,i = 1，···，Ν，則p(Xk|Yk)被表示成這些隨機樣本求和形式，引入一個已知、容易 采樣的重要性概率密度函數(shù)q(xk|Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度函 數(shù)P (xk I Yk)，則后驗概率密度表示為從重要性密度函數(shù)采樣的隨機樣本點與每個樣本所對 應(yīng)的權(quán)值乘積和的形式，粒子權(quán)值定義為重要性密度函數(shù)逼近后驗概率函數(shù)的程度，從重 要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求得相應(yīng)的權(quán)值，進而 得到狀態(tài)估計； 步驟三：重采樣：根據(jù)步驟二得到的粒子權(quán)值，計算有效粒子數(shù)來衡量粒子權(quán)值的退 化程度； 步驟四、對慣性導(dǎo)航系統(tǒng)的狀態(tài)Xk進行估計：根據(jù)現(xiàn)有的觀測其中量Y k，其中Yk表示 Y1, y2，…，yk，估計出重力輔助慣導(dǎo)定位系統(tǒng)的位置信息xk; 步驟五：按照步驟一中的AUk更新p(xk|xk D，再根據(jù)步驟三更新粒子權(quán)值，進行k+l時 刻的慣導(dǎo)位置估計。2. 如權(quán)利要求1所述的一種基于貝葉斯估計的粒子濾波重力輔助慣導(dǎo)匹配方法，其特 征在于，進一步地，貝葉斯估計包含預(yù)測和更新兩個階段，預(yù)測過程利用建立的系統(tǒng)模型預(yù) 測狀態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到后 驗概率密度。
【專利摘要】本發(fā)明提出一種基于貝葉斯估計的粒子濾波重力輔助慣導(dǎo)匹配方法，在解決非線性問題時，粒子濾波避免了線性化帶來的誤差，解決了傳統(tǒng)點匹配算法桑迪亞算法在重力異常變化大的匹配區(qū)域易發(fā)散的缺點。本發(fā)明以慣導(dǎo)的位置信息為狀態(tài)量以重力儀測量值為觀測量，利用基于貝葉斯估計的粒子濾波，以隨機養(yǎng)樣本均值代替概率密度函數(shù)的條件均值，估計慣導(dǎo)系統(tǒng)的狀態(tài)變量。
【IPC分類】G01C21/16, G01C21/20
【公開號】CN105157704
【申請?zhí)枴緾N201510300605
【發(fā)明人】王博, 于力, 鄧志紅, 肖烜
【申請人】北京理工大學(xué)
【公開日】2015年12月16日
【申請日】2015年6月3日