進行序貫重要性采樣時，若小于設定的閾值，則應當采用重采樣的方法克 服粒子的權(quán)值退化，重采樣方法舍棄權(quán)值較小的粒子，代之以權(quán)值較大的粒子。重采樣 過程滿足/<·^ = <)= iif條件下，將粒子集合
更新為
?:本發(fā)明 中當/^<0.5時，采用基于重要性采樣的殘差重采樣更新粒子。殘差重采樣具有效率 高、實現(xiàn)方便的特點。設
其中|_」為取整操作。殘余重采樣采用新的權(quán)值
選擇剩余下的
個粒子，殘差重采樣的主要過程為：
[0053] (1)計算剩余粒子的權(quán)值累計量％.，/= V
[0054] (2)生成;^個在[0, 1]區(qū)間內(nèi)均勻分布的隨機數(shù)。
[0055] (3)對于每個μ \尋找歸一化權(quán)值累計量大于或者等于μ1的最小標號m，即 λ J μ 1Uni。當μ1落在區(qū)間[λ λ J時，<被復制一次。
[0056] 這樣，每個粒子.tf經(jīng)重采樣后的個數(shù)為步驟（3)中所選中的若干粒子數(shù)目與Ν1。
[0057] 步驟五：對慣性導航系統(tǒng)的狀態(tài)&進行估計
[0058] 貝葉斯估計利用概率分布的形式估計非線性系統(tǒng)的狀態(tài)Xk。根據(jù)現(xiàn)有的觀測量 Yk (Yk表示y i，y2，…，yk)，要估計出重力輔助慣導定位系統(tǒng)的位置信息Xk。依據(jù)貝葉斯公式 有：
[0060] 由式（19)可知，分母為一個與Xk無關(guān)的歸一化常量，它可以保證P(X kIYk)的積分 為1。由式（20)可知測量誤差％的概率密度函數(shù)p (yk I Xk)，即為先驗概率密度函數(shù)：
[0061] P (yk I Xk) = / δ (yk-hk (xk)-Vk) p (vk) d Vk (29)
[0062] 其中δ⑴為狄拉克函數(shù)。
[0063] 由于觀測值yk只與當前狀態(tài)X k有關(guān)，則P (X k I Xk Yk i) = P (xk |xk i)，根據(jù)全概率 公式可知：
[0064] p (xk，xk i I Yk D = p (xk I xk i，Yk D p (xk i I Yk D (30)
[0065] 化簡并進行積分可得CK方程：
[0066] p (xk I Yk !) = / P (xk I Xk !) P (xk ! I Yk !) d Xk ! (31)
[0067] 其中，p (xk ! I Yk D假設已知，p (xk I xk D為狀態(tài)的Markov過程。
[0068] 即根據(jù)最小均方差（MMSE)準則，將條件均值作為系統(tǒng)狀態(tài)Xk的最優(yōu)估計值，即
[0069]
[0070] 則上式（28) (29) (31)組成了貝葉斯估計系統(tǒng)狀態(tài)Xk的基本公式。式（29)為概 率密度函數(shù)，式（28)、（29)分別為位置更新和測量更新公式。由上述可知，貝葉斯估計需要 進行積分運算，非線性系統(tǒng)多采用近似算法解決積分問題，從而得到狀態(tài)的最優(yōu)估計?；?貝葉斯估計的粒子濾波利用所求狀態(tài)空間中的大量樣本點來近似逼近待估變量的后驗概 率分布，從而將積分問題轉(zhuǎn)換為有限樣本點的求和問題。
[0071] 粒子濾波就是尋找一組在狀態(tài)空間中傳播的隨機樣本對后驗概率密度函數(shù) P (xk I Yk)進行近似，以樣本均值代替條件均值E [xk IYJ。將服從p (xk I Yk)分布的隨機樣本稱 為粒子。
[0072] 設#為從后驗概率密度函數(shù)P (xk I Yk)中獲取的采樣粒子，則對于要估計的慣導 位置Xk，條件均值E [xk IYJ可以用求和方式逼近，即
[0074] 此時進行步驟（三）中的序貫重要性采樣，生成粒子，當采樣粒子數(shù)目很大時，式 (13)便可逼近真實的后驗概率密度。因此要估計的慣導位置Xk的條件均值E[xk|Y k]可表 示為：
[0076] 此時在進行步驟（四）重采樣，要估計的慣導位置Xk的條件均值E [xk IYJ可表示 為：
[0078] 輸出此時的慣導位置X1^估計值P k。
[0079] 步驟六：判斷單點匹配結(jié)果是否可信
[0080] 單點匹配結(jié)果也應滿足慣導相鄰點之間位相關(guān)性，則單點匹配結(jié)果 {P' k4，P' k3，P' k2，P' kl，P' k}(k 彡 4)中的
也應 滿足式（21)。以為例，若滿足，則將P',的匹配結(jié)果視為可信點，若不滿足，則在點 匹配過程中加入^的限定的條件得到新的重要性函數(shù)，根據(jù)@由式（21)所確 定的可信范圍重新計算狀態(tài)變量的轉(zhuǎn)移概率密度函數(shù)，得到，進行步驟（五），得 到新的匹配結(jié)果|?。
[0081] 步驟七：計算矢量匹配結(jié)果權(quán)值
[0082] 若I
都不滿足式（21)，則得到四個局部最優(yōu)解
利用加權(quán)最小二乘估計得到最終估計值3加權(quán)最小二乘估 計觀測模型為
[0083] βΡ+/ι (33)
[0084] 其中V表示采樣點{Pk 4, Pk 3, Pk 2, Pk J慣導系統(tǒng)輸出的速度IVk 4, Vk 3, Vk 2, Vk J，β 為系數(shù)矩陣[Pk 4, Pk3. Pk2, Pk In為觀測噪聲。取σ 1= Ivi-W1P1I,求加權(quán)偏差平方和 s最小的Wk，其中
[0086] 求得[wk 4, Wk 3, Wk 2, wk J，然后歸一化權(quán)值，求得Pk的最終估計值
[0087] 步驟八：按照步驟（一）中的Λ Uk更新p (X k I Xk i)，再根據(jù)步驟（四）更新粒子權(quán) 值，進行k+Ι時刻的慣導位置估計，再進行步驟（六）（七），得到矢量匹配結(jié)果。
[0088] 為了說明本發(fā)明的效果，采用桑迪亞算法在相同的實驗設置下進行試驗，采用本 發(fā)明后匹配軌跡在匹配區(qū)域內(nèi)對慣導軌跡起到了良好的校正作用，經(jīng)煒度誤差明顯減小， 經(jīng)過多次試驗，本發(fā)明的經(jīng)煒度誤差平均值與傳統(tǒng)桑迪亞算法比較的結(jié)果如表1所示。 [0089] 表1 :誤差結(jié)果
CN 105180938 A 說明書 8/8 頁
[0095] 其中若單點粒子濾波結(jié)果無須進行矢量匹配則視為可信。如表2所示，在重力異 常變化率大的區(qū)域，單點粒子濾波結(jié)果可信，在重力異常變化率相對小的區(qū)域，就要進行矢 量匹配。從仿真結(jié)果可以看出，在重力異常變化率較小的區(qū)域，單點粒子濾波的平均圓概率 誤差增量平均值比在重力異常變化率大的區(qū)域大，改進后的矢量匹配算法的平均圓概率誤 差比單點粒子濾波平均減小，并且在匹配區(qū)域增量平穩(wěn)，說明改進后的矢量匹配算法對重 力特征變化具有較強的魯棒性。
[0096] 以上所述僅是本發(fā)明的優(yōu)選實施方式，應當指出，對于本技術(shù)領(lǐng)域的普通技術(shù)人 員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進，或者對其中部分技術(shù)特征進 行等同替換，這些改進和替換也應視為本發(fā)明的保護范圍。
【主權(quán)項】
1. 一種基于粒子濾波的重力采樣矢量匹配定位方法，其特征在于，包括如下步驟： 步驟一、根據(jù)水下載體的運動規(guī)律，將運載體的經(jīng)煒度信息作為狀態(tài)變量，重力儀實時 測量重力異常值為觀測量，建立重力輔助慣導單點匹配模型：式中，Λ Xkik+1表示k時刻到k+Ι時刻輸出的潛艇的經(jīng)煒度差值，Λ U 慣導系統(tǒng)給出 k時刻的偏移增量，ek慣導系統(tǒng)的誤差，y k表示k時刻的重力儀測量的重力異常信息，h k (Xk) 表示在xk處在重力基準圖上讀取的重力異常值，vk表示重力異常測量誤差和重力基準圖誤 差； 步驟二、計算矢量匹配基準條件：設水下運載體為勻速航行，Pk表示運載體航行過程 中慣導系統(tǒng)輸出的點k的位置信息，則等間隔采樣點集{Pk 4, Pk 3, Pk 2, Pk i，PJ的矢量信息其中（mk，nk)表示k點慣導系統(tǒng)輸出的經(jīng)煒度 信息，感教表示無誤差慣導系統(tǒng)兩相鄰采樣點之間的歐氏距離，σ表示測量誤差的上 限； 步驟三、序貫重要性采樣：假設從后驗概率密度P(XkIYk)中抽出N個獨立同分布的隨機 樣本i = 1，···，Ν，則p(Xk|Yk)被表示成這些隨機樣本求和形式；引入一個已知、容易 采樣的重要性概率密度函數(shù)q(xk|Yk)，從中生成采樣粒子，實現(xiàn)求和逼近后驗概率密度函 數(shù)P (xk I Yk)，則后驗概率密度表示為從重要性密度函數(shù)采樣的隨機樣本點與每個樣本所對 應的權(quán)值乘積和的形式，粒子權(quán)值定義為重要性密度函數(shù)逼近后驗概率函數(shù)的程度；從重 要性概率密度采樣中生成采樣粒子，并隨著測量值的依次到來遞推求得相應的權(quán)值，進而 得到狀態(tài)估計； 步驟四：重采樣：根據(jù)步驟三得到的粒子權(quán)值，計算有效粒子數(shù)次#來衡量粒子權(quán)值的 退化程度； 步驟五、對慣性導航系統(tǒng)的狀態(tài)進行估計：根據(jù)現(xiàn)有的觀測其中量估計出重力輔助慣 導定位系統(tǒng)的位置信息； 步驟六、判斷單點匹配結(jié)果是否可信：單點匹配結(jié)果應滿足慣導相鄰點之間位相關(guān)性， 則單點匹配結(jié)果也應滿足公式（3);若滿足，則將匹配結(jié)果視為可信點，若不滿足，則在點 匹配過程中加入i的限定的條件得到新的重要性函數(shù)，根據(jù)由式（3)所確 定的可信范圍重新計算狀態(tài)變量的轉(zhuǎn)移概率密度函數(shù)，得到；進行步驟五，得到 新的匹配結(jié)果； 步驟七、計算矢量匹配結(jié)果權(quán)值：若匹配結(jié)果均不滿足式（3)，則得到四個局部最優(yōu) 解，利用加權(quán)最小二乘估計得到最終估計值 步驟八：按照步驟一中的Λ Uk更新p (Xk I Xk D，再根據(jù)步驟四更新的粒子權(quán)值，進行k+1 時刻的慣導位置估計，再進行步驟六、七，得到矢量匹配結(jié)果。2.如權(quán)利要求1所述的一種基于粒子濾波的重力采樣矢量匹配定位方法，其特征在 于，進一步地，所述的估計包含預測和更新兩個階段，預測過程利用建立的系統(tǒng)模型預測狀 態(tài)的先驗概率密度，更新過程則利用最新的測量值對先驗概率密度進行修正，得到后驗概 率密度。
【專利摘要】本發(fā)明提出一種基于粒子濾波的重力采樣矢量匹配定位方法，矢量匹配算法考慮匹配點之間的位置相關(guān)性，解決了傳統(tǒng)匹配算法匹配結(jié)果可信度不高的問題。本發(fā)明利用重力采樣點所組成的矢量進行匹配，對單點匹配結(jié)果的二次估計，利用基于貝葉斯估計的粒子濾波實現(xiàn)。在單點匹配之后，計算匹配結(jié)果相鄰點之間的歐氏距離，判斷其是否滿足無誤差慣導系統(tǒng)兩相鄰采樣點之間的歐氏距離所確定的范圍。若滿足，則該點匹配結(jié)果可信；若不滿足，則根據(jù)限定條件重新進行單點匹配。
【IPC分類】G01C21/20
【公開號】CN105180938
【申請?zhí)枴緾N201510300603
【發(fā)明人】王博, 于力, 肖烜, 鄧志紅
【申請人】北京理工大學
【公開日】2015年12月23日
【申請日】2015年6月3日