本發(fā)明涉及導(dǎo)航和定位技術(shù)領(lǐng)域，特別涉及一種自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航方法和系統(tǒng)。

背景技術(shù)：

地形輔助導(dǎo)航(Terrain Aided Navigation，TAN)由于具有自主性、隱蔽性、抗干擾能力強(qiáng)和全天候工作的優(yōu)點(diǎn)，其在組合導(dǎo)航系統(tǒng)中受到廣泛的重視和研究，并已在巡航導(dǎo)彈、飛機(jī)和艦船上成功應(yīng)用。TAN的定位思想是利用實(shí)測的運(yùn)動(dòng)軌跡下方的地形高程數(shù)據(jù)和存儲(chǔ)的數(shù)字地形高程模型(Digital Elevation Model，DEM)為信息，采用地形匹配算法來達(dá)到定位的目的。地形輔助導(dǎo)航系統(tǒng)的定位精度和實(shí)時(shí)性不僅與地形匹配算法有關(guān)還受地形特征的影響。在地形特征較明顯的山地和丘陵地區(qū)定位準(zhǔn)確度較高，在平原或海平面這些地形特征不明顯的區(qū)域匹配就不適合執(zhí)行匹配定位。由于受地形匹配算法和地形特征的限制，目前地形輔助導(dǎo)航系統(tǒng)的應(yīng)用，大都需要事先根據(jù)飛行任務(wù)規(guī)劃飛行路線、選取匹配區(qū)域，使得載體機(jī)動(dòng)飛行和實(shí)時(shí)定位的能力受限。如何實(shí)現(xiàn)載體在飛行過程中自主選擇匹配區(qū)域并根據(jù)飛行狀態(tài)和所規(guī)劃區(qū)域的地形特征自動(dòng)執(zhí)行合適的地形匹配算法，將極大的提高載體機(jī)動(dòng)飛行和實(shí)時(shí)地形輔助定位的能力從而擺脫受事先規(guī)劃的飛行軌跡的束縛，這也將有利于地形輔助導(dǎo)航的推廣和應(yīng)用。

技術(shù)實(shí)現(xiàn)要素：

本發(fā)明提供了一種實(shí)時(shí)機(jī)載地形輔助導(dǎo)航方法和系統(tǒng)，以解決目前地形輔助導(dǎo)航系統(tǒng)需要事先根據(jù)飛行任務(wù)規(guī)劃飛行路線、選取匹配區(qū)域，使得載體機(jī)動(dòng)飛行和實(shí)時(shí)定位的能力受限的問題。

本發(fā)明的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航方法，包括以下步驟：

步驟S1：根據(jù)主慣性導(dǎo)航系統(tǒng)輸出的姿態(tài)、速度和位置信息，通過地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的地形匹配區(qū)域自動(dòng)規(guī)劃算法連續(xù)規(guī)劃匹配區(qū)域；

步驟S2：根據(jù)地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的匹配區(qū)域適應(yīng)性分析準(zhǔn)則自動(dòng)分析規(guī)劃區(qū)域的匹配適應(yīng)性；

步驟S3：根據(jù)步驟S2中的匹配適應(yīng)性的分析結(jié)果來決策是否執(zhí)行地形匹配算法，如果所規(guī)劃區(qū)域?yàn)槠ヅ鋮^(qū)域，則執(zhí)行地形匹配算法并實(shí)現(xiàn)匹配定位；如果所規(guī)劃區(qū)域?yàn)榉瞧ヅ鋮^(qū)域，則不執(zhí)行地形匹配算法。

進(jìn)一步地，步驟S1中的根據(jù)主慣性導(dǎo)航系統(tǒng)輸出的姿態(tài)、速度和位置信息，通過地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的地形匹配區(qū)域自動(dòng)規(guī)劃算法連續(xù)規(guī)劃匹配區(qū)域，具體包括：

利用主慣性導(dǎo)航系統(tǒng)提供的載體即時(shí)位置信息經(jīng)度λ和緯度L、航向角ψ、東向位置誤差的估計(jì)值σ_E和北向的位置誤差的估計(jì)值σ_N對(duì)匹配區(qū)域進(jìn)行規(guī)劃，匹配區(qū)域規(guī)化的邊界模型為

式中，E₁、E₂、N₁、N₂分別為以載體即時(shí)位置經(jīng)度λ和緯度L為坐標(biāo)原點(diǎn)，東向?yàn)閄軸、北向?yàn)閅軸所建立的臨時(shí)坐標(biāo)系下的矩形區(qū)域在西向、東向、南向和北向的邊界坐標(biāo)值；l為匹配序列長度。

進(jìn)一步地，所述地形匹配算法包括搜索模式和跟蹤模式；搜索模式下的地形匹配算法采用基于相關(guān)思想的地形輪廓匹配算法，跟蹤模式下的地形匹配算法采用基于最優(yōu)濾波原理的地形輔助導(dǎo)航算法。

進(jìn)一步地，所述地形輪廓匹配算法所采用的性能指標(biāo)有三種：交叉相關(guān)COR算法、平均絕對(duì)差MAD算法和均方差MSD算法；搜索模式下的所述地形匹配算法為：利用COR、MAD和MSD算法分別進(jìn)行三次獨(dú)立的地形匹配，若每次執(zhí)行匹配時(shí)3種算法其中2種的定位誤差在1個(gè)網(wǎng)格內(nèi)則認(rèn)為本次表決通過，如果連續(xù)三次均通過則認(rèn)為定位可靠。

進(jìn)一步地，所述根據(jù)步驟S2中的匹配適應(yīng)性的分析結(jié)果來決策是否執(zhí)行地形匹配算法的具體決策過程為：

當(dāng)所規(guī)劃的區(qū)域?yàn)槠ヅ鋮^(qū)域，且載體為初次進(jìn)入匹配區(qū)域或者從非匹配區(qū)域進(jìn)入匹配區(qū)域，地形匹配算法執(zhí)行搜索模式；

當(dāng)載體處于連續(xù)匹配區(qū)域且執(zhí)行搜索模式時(shí)，且定位誤差減小到規(guī)定的閾值范圍內(nèi)，地形匹配算法執(zhí)行跟蹤模式；

當(dāng)載體處于連續(xù)匹配區(qū)域且執(zhí)行跟蹤模式時(shí)，根據(jù)可靠性判斷機(jī)制判斷定位結(jié)果的可靠性，如果定位結(jié)果可靠，則繼續(xù)執(zhí)行跟蹤模式的地形匹配算法；如果定位結(jié)果不可靠，將地形匹配算法切換到搜索模式，重新開始搜索定位；

當(dāng)載體處于非匹配區(qū)或從匹配區(qū)域離開時(shí)，地形匹配算法停止執(zhí)行。

進(jìn)一步地，所述可靠性判斷機(jī)制的實(shí)現(xiàn)過程為：

根據(jù)慣性器件在短時(shí)間內(nèi)漂移小的特性，利用當(dāng)前時(shí)刻之前的位置序列推算當(dāng)前時(shí)刻的位置信息得到當(dāng)前位置估計(jì)值，并分別與通過擴(kuò)展卡爾曼濾波得到的當(dāng)前時(shí)刻位置的濾波值進(jìn)行比較，如果估計(jì)值與濾波值差的絕對(duì)值均小于1個(gè)網(wǎng)格，則表明定位結(jié)果可靠，否則，定位結(jié)果不可靠。

進(jìn)一步地，步驟S2中根據(jù)地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的匹配區(qū)域適應(yīng)性分析準(zhǔn)則自動(dòng)分析規(guī)劃區(qū)域的匹配適應(yīng)性的具體步驟：

步驟1)：計(jì)算規(guī)化的區(qū)域內(nèi)的地形特征參數(shù)，包括高程標(biāo)準(zhǔn)差σ_h、測高信噪比σ、地形粗糙度σ_z和地形相關(guān)長度l_m，并根據(jù)所述地形特征參數(shù)構(gòu)建地形匹配區(qū)域適應(yīng)性分析的邏輯判斷函數(shù)；

步驟2)：根據(jù)地形匹配區(qū)域適應(yīng)性分析的所述邏輯判斷函數(shù)，對(duì)規(guī)劃區(qū)域的適應(yīng)性做初步判斷，如果規(guī)劃區(qū)域的匹配適應(yīng)性不滿足所述邏輯判斷函數(shù)，則舍棄該區(qū)域；如果規(guī)劃區(qū)域的匹配適應(yīng)性滿足所述邏輯判斷函數(shù)，則利用地形剖面相關(guān)性檢驗(yàn)算法做進(jìn)一步的驗(yàn)證。

進(jìn)一步地，所述利用地形剖面相關(guān)性檢驗(yàn)算法做進(jìn)一步的驗(yàn)證，具體為：在規(guī)劃區(qū)域內(nèi)隨機(jī)選取n個(gè)地形剖面，將每個(gè)剖面與其余的n-1個(gè)剖面根據(jù)剖面相關(guān)性計(jì)算公式做相關(guān)性分析，相關(guān)性計(jì)算公式為

式中，T和F為兩個(gè)地形剖面，M為剖面的長度；

在得到的相關(guān)值R中選取次最大值和最大值的比值小于0.9作為判斷條件，在所有的相關(guān)面中滿足條件的個(gè)數(shù)大于總的相關(guān)面的90％，將該區(qū)域保留，否則將該區(qū)域舍棄。

相應(yīng)地，本發(fā)明還提供一種實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航系統(tǒng)，

包括：主慣性導(dǎo)航系統(tǒng)，地形輔助導(dǎo)航系統(tǒng)；

所述地形輔助導(dǎo)航系統(tǒng)包括地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊、基準(zhǔn)地形高程數(shù)據(jù)庫、傳感器單元、地形匹配導(dǎo)航算法模塊；

所述基準(zhǔn)地形高程數(shù)據(jù)庫，提供飛行區(qū)域的地形高程基準(zhǔn)圖；

所述傳感器單元提供載體飛行軌跡下方的地形高程測量信息；

所述地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊的輸入?yún)?shù)由主慣性導(dǎo)航系統(tǒng)和基準(zhǔn)地形高程數(shù)據(jù)庫提供，所述地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊的輸出參數(shù)提供給所述地形匹配導(dǎo)航算法模塊；

所述地形匹配導(dǎo)航算法模塊，根據(jù)所述地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊的輸出參數(shù)決策是否執(zhí)行匹配算法。

進(jìn)一步地，所述地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的地形匹配區(qū)域自動(dòng)規(guī)劃算法連續(xù)規(guī)劃匹配區(qū)域，所述地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊中的匹配區(qū)域適應(yīng)性分析準(zhǔn)則自動(dòng)分析規(guī)劃區(qū)域的匹配適應(yīng)性。

本發(fā)明的優(yōu)點(diǎn)在于：對(duì)于傳統(tǒng)的地形輔助慣性導(dǎo)航系統(tǒng)，引入匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊，可實(shí)現(xiàn)載體在飛行過程中自主規(guī)劃選取匹配區(qū)域，并根據(jù)不同地形區(qū)域和飛行狀態(tài)選擇合適的地形匹配算法進(jìn)行匹配定位，從而提高載體機(jī)動(dòng)飛行和實(shí)時(shí)定位的能力。為確保匹配定位算法在跟蹤模式下定位結(jié)果的可靠性，采用基于位置序列估值與濾波值比較的方法確保定位結(jié)果的可靠性。本發(fā)明可以提高載體機(jī)動(dòng)飛行和實(shí)時(shí)定位的能力，從而解決目前地形輔助導(dǎo)航系統(tǒng)需要事先根據(jù)飛行任務(wù)規(guī)劃飛行路線和匹配區(qū)域?qū)е螺d體機(jī)動(dòng)飛行和實(shí)時(shí)定位能力受限的問題。

附圖說明

圖1為本發(fā)明實(shí)施例中的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航系統(tǒng)結(jié)構(gòu)示意圖。

圖2為本發(fā)明實(shí)施例中的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航方法的流程圖。

圖3為本發(fā)明實(shí)施例中的自主實(shí)時(shí)機(jī)載地形輔助慣性匹配區(qū)域規(guī)劃的示意圖。

圖4為本發(fā)明實(shí)施例中的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航的跟蹤模式下的可靠性判斷機(jī)制原理示意圖。

具體實(shí)施方式

下面將結(jié)合本發(fā)明的附圖，對(duì)本發(fā)明的技術(shù)方案進(jìn)行清楚、完整地描述，顯然，所描述的實(shí)施例僅僅是本發(fā)明一部分實(shí)施例，而不是全部的實(shí)施例?；诒景l(fā)明的實(shí)施例，本領(lǐng)域普通技術(shù)人員在沒有做出創(chuàng)造性勞動(dòng)前提下所獲得的所有其他實(shí)施例，都屬于本發(fā)明保護(hù)的范圍。

一個(gè)實(shí)施例，如圖1所示的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航系統(tǒng)，包括主慣性導(dǎo)航系統(tǒng)1和地形輔助導(dǎo)航系統(tǒng)2。其中主慣性導(dǎo)航系統(tǒng)1提供載體的實(shí)時(shí)姿態(tài)、速度和位置信息(位置信息包含位置誤差的估計(jì)值)；地形輔助導(dǎo)航系統(tǒng)2作為主慣性導(dǎo)航系統(tǒng)1的輔助導(dǎo)航系統(tǒng)，包括地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21、基準(zhǔn)地形高程數(shù)據(jù)庫22、傳感器單元23、地形匹配導(dǎo)航算法模塊24。

地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21，用于載體飛行過程中連續(xù)規(guī)劃匹配區(qū)域并對(duì)所規(guī)劃的匹配區(qū)域進(jìn)行匹配適應(yīng)性分析，地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21的輸入?yún)?shù)由主慣性導(dǎo)航系統(tǒng)1和基準(zhǔn)地形高程數(shù)據(jù)庫22提供，地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21的輸出參數(shù)提供給地形匹配導(dǎo)航算法模塊24。

基準(zhǔn)地形高程數(shù)據(jù)庫22，提供飛行區(qū)域的地形高程基準(zhǔn)圖；

傳感器單元23包括氣壓高度表231和雷達(dá)高度表232，提供載體飛行軌跡下方的地形高程測量信息；

地形匹配導(dǎo)航算法模塊24，根據(jù)地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21的輸出參數(shù)決策是否執(zhí)行匹配算法；

地形匹配導(dǎo)航算法模塊24，利用傳感器單元23提供的地形高程測量信息和基準(zhǔn)地形高程數(shù)據(jù)庫22提供的整個(gè)區(qū)域的地形高程信息實(shí)現(xiàn)地形匹配定位，對(duì)主慣性導(dǎo)航系統(tǒng)1輸出的姿態(tài)、速度和位置信息3進(jìn)行修正。

另一個(gè)實(shí)施例，如圖2所示的自主實(shí)時(shí)機(jī)載地形輔助慣性導(dǎo)航方法，包括以下步驟：

步驟S1：根據(jù)主慣性導(dǎo)航系統(tǒng)輸出的姿態(tài)、速度和位置信息，通過地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21中的地形匹配區(qū)域自動(dòng)規(guī)劃算法連續(xù)規(guī)劃匹配區(qū)域；

具體的，利用主慣性導(dǎo)航系統(tǒng)1提供的載體即時(shí)位置信息經(jīng)度λ和緯度L、航向角ψ、東向位置誤差的估計(jì)值σ_E和北向的位置誤差的估計(jì)值σ_N對(duì)匹配區(qū)域進(jìn)行規(guī)劃，匹配區(qū)域規(guī)化的邊界模型為

式中，E₁、E₂、N₁、N₂分別為以載體即時(shí)位置經(jīng)度λ和緯度L為坐標(biāo)原點(diǎn)，東向?yàn)閄軸、北向?yàn)閅軸所建立的臨時(shí)坐標(biāo)系下的矩形區(qū)域在西向、東向、南向和北向的邊界坐標(biāo)值；l為匹配序列長度。

為了更好的說明匹配區(qū)域規(guī)劃的流程，給出如圖3所示的匹配區(qū)域規(guī)劃的示意圖。載體在基準(zhǔn)地形高程數(shù)據(jù)庫22提供的飛行區(qū)域的地形高程基準(zhǔn)圖所描述的地形區(qū)域沿著軌跡221飛行時(shí)，k時(shí)刻飛出前一個(gè)匹配區(qū)222，則以當(dāng)前時(shí)刻載體所在的位置經(jīng)度λ和緯度L為參考點(diǎn)O執(zhí)行下一個(gè)匹配區(qū)的規(guī)劃。以O(shè)點(diǎn)為坐標(biāo)原點(diǎn)，建立如圖3所示的坐標(biāo)系，坐標(biāo)軸的X軸指向東向和Y軸指向北向。根據(jù)匹配區(qū)域規(guī)化的邊界模型得到的匹配區(qū)域223的長寬與航向角ψ、匹配序列長度l、東向位置誤差的估計(jì)值σ_E和北向的位置誤差的估計(jì)值σ_N的關(guān)系如圖3所示。

步驟S2：根據(jù)地形匹配區(qū)域規(guī)劃與適應(yīng)性分析模塊21中的匹配區(qū)域適應(yīng)性分析準(zhǔn)則自動(dòng)分析規(guī)劃區(qū)域的匹配適應(yīng)性；

具體的，匹配區(qū)域的適應(yīng)分析準(zhǔn)則包含以下步驟：

步驟1)：根據(jù)地形特征參數(shù)高程標(biāo)準(zhǔn)差σ_h、測高信噪比σ、地形粗糙度σ_z和地形相關(guān)長度l_m構(gòu)建地形匹配區(qū)域適應(yīng)性分析的邏輯判斷函數(shù)，根據(jù)邏輯判斷函數(shù)，對(duì)規(guī)劃區(qū)域的適應(yīng)性做初步判斷，邏輯判斷函數(shù)取為：

高程標(biāo)準(zhǔn)差σ_h，用于描述地形高程基準(zhǔn)圖中逐網(wǎng)格點(diǎn)的離散程度和整個(gè)區(qū)域地形總的起伏程度，其計(jì)算公式為：

式中，M_h為地形均值，其計(jì)算公式為：

式中，m為網(wǎng)格高程基準(zhǔn)圖的行數(shù)，n為網(wǎng)格高程基準(zhǔn)圖的列數(shù)，h(i，j)為網(wǎng)格高程基準(zhǔn)圖的第i行、第j列網(wǎng)格的地形高程值。

地形粗糙度σ_z，描述整個(gè)地形區(qū)域的區(qū)平均光滑程度，刻劃較細(xì)微的局部起伏，其計(jì)算公式為：

式中，Q_x為x方向相鄰網(wǎng)格的地形粗糙度，Q_y為y方向相鄰網(wǎng)格的地形粗糙度，其計(jì)算公式分別為：

步驟2)：如果所規(guī)劃的區(qū)域滿足邏輯判斷函數(shù)，則通過地形剖面相關(guān)性檢驗(yàn)做進(jìn)一步的驗(yàn)證，在進(jìn)行地形相關(guān)性檢驗(yàn)時(shí)，在規(guī)劃區(qū)域內(nèi)隨機(jī)選取n(n可取100，也可取其他合適的值，例如80、120、130等)個(gè)地形剖面，將每個(gè)剖面與其余的n-1個(gè)剖面根據(jù)剖面相關(guān)性計(jì)算公式做相關(guān)性分析，計(jì)算公式為

式中，T和F為兩個(gè)地形剖面，M為剖面的長度(其值可取匹配序列長度l)。

在得到的相關(guān)值R中選取次最大值和最大值的比值小于0.9作為判斷條件，在所有的相關(guān)面中滿足條件的個(gè)數(shù)大于總的相關(guān)面的90％，將該區(qū)域保留；否則，將該區(qū)域舍棄。

步驟S3：根據(jù)步驟S2中的匹配適應(yīng)性分析結(jié)果來決策是否執(zhí)行地形匹配算法，如果所規(guī)劃的區(qū)域?yàn)槠ヅ鋮^(qū)域，則執(zhí)行地形匹配算法并實(shí)現(xiàn)匹配定位；如果所規(guī)劃的區(qū)域?yàn)榉瞧ヅ鋮^(qū)域，則不執(zhí)行地形匹配算法。

具體的，地形匹配算法包括搜索模式和跟蹤模式。

搜索模式，用于在載體剛進(jìn)入匹配區(qū)域或者從非匹配區(qū)域進(jìn)行匹配區(qū)域時(shí)，由于慣性導(dǎo)航系統(tǒng)長時(shí)間積累的誤差使得此時(shí)的位置誤差較大，搜索模式可以快速的定位到一個(gè)較小的誤差范圍。

搜索模式下的地形匹配算法采用基于相關(guān)思想的地形輪廓匹配算法，所采用的性能指標(biāo)有三種：交叉相關(guān)COR算法、平均絕對(duì)差MAD算法和均方差MSD算法。搜索模式下的地形匹配算法為：利用COR、MAD和MSD相關(guān)算法分別進(jìn)行三次獨(dú)立的地形匹配，若每次執(zhí)行匹配時(shí)3種算法其中2種的定位誤差在1個(gè)網(wǎng)格內(nèi)則認(rèn)為本次表決通過，如果連續(xù)三次均通過則認(rèn)為定位可靠。

交叉相關(guān)COR算法，根據(jù)相關(guān)性強(qiáng)度實(shí)現(xiàn)匹配搜索，相關(guān)性越大則表明搜索位置與實(shí)際位置越接近，計(jì)算公式為：

式中，l為匹配序列的長度，h_t(i)為第i個(gè)點(diǎn)的測量地形高度(由傳感器單元提供)，h_m(x+iδx，y+iδy)為基準(zhǔn)地形高程數(shù)據(jù)庫的第i個(gè)點(diǎn)的地形高程(由基準(zhǔn)地形高程數(shù)據(jù)庫提供)，x和y為選擇匹配點(diǎn)的起始點(diǎn)坐標(biāo)，δx和δy為相對(duì)于起始點(diǎn)坐標(biāo)的網(wǎng)格偏移量。

平均絕對(duì)差MAD算法，利用搜索位置與實(shí)際位置的高程差的絕對(duì)值求平均得到，平均絕對(duì)差越小則表明搜索位置與實(shí)際位置越接近，計(jì)算公式為：

均方差MSD算法，根據(jù)搜索位置與實(shí)際位置的高程的均方差最小作為評(píng)判標(biāo)準(zhǔn)實(shí)現(xiàn)搜索定位，計(jì)算公式為：

跟蹤模式，用于載體的位置誤差在一個(gè)較小的范圍時(shí)通過跟蹤模式獲取載體的精確位置。

具體的，跟蹤模式下的地形匹配算法采用基于最優(yōu)濾波原理的地形輔助導(dǎo)航算法。地形輔助導(dǎo)航算法具體為：狀態(tài)變量取東北天坐標(biāo)系下的載體三維位置誤差Δx、Δy和Δz以及二維速度誤差dv_x和dv_y，向量表達(dá)式為：

線性最優(yōu)濾波的狀態(tài)方程的離散形式為：

X_k＝Φ_kk-1X_k-1+Γ_k-1W_k-1 (13)

式中，Φ_k，k-1為一步狀態(tài)轉(zhuǎn)移矩陣；Γ_k-1為系統(tǒng)噪聲轉(zhuǎn)移矩陣；W_k-1為系統(tǒng)噪聲向量。

一步狀態(tài)轉(zhuǎn)移矩陣和系統(tǒng)噪聲轉(zhuǎn)移矩陣的表達(dá)式為：

式中，T為采樣周期。

取實(shí)際測量的地形高度和估算的地形高度之差作為一維量測值Z，離散形式的量測方程為：

Z_k＝H_kX_k+γ_k (15)

式中，為量測矩陣；γ_k為量測噪聲；h_x和h_y是通過地形隨機(jī)線性化技術(shù)得到的地形x和y方向上的斜率。

根據(jù)匹配適應(yīng)性分析結(jié)果執(zhí)行地形匹配算法的具體決策過程為：

當(dāng)所規(guī)劃的區(qū)域?yàn)槠ヅ鋮^(qū)域，且載體為初次進(jìn)入匹配區(qū)域或者從非匹配區(qū)域進(jìn)入匹配區(qū)域，地形匹配算法執(zhí)行搜索模式；

當(dāng)載體處于連續(xù)匹配區(qū)域且執(zhí)行搜索模式時(shí)，且定位誤差減小到規(guī)定的閾值范圍內(nèi)，地形匹配算法執(zhí)行跟蹤模式；

當(dāng)載體處于連續(xù)匹配區(qū)域且執(zhí)行跟蹤模式時(shí)，根據(jù)可靠性判斷機(jī)制判斷定位結(jié)果的可靠性，如果定位結(jié)果可靠，則繼續(xù)執(zhí)行跟蹤模式算法；如果定位結(jié)果不可靠，將地形匹配算法切換到搜索模式，重新開始搜索定位；

當(dāng)載體處于非匹配區(qū)或從匹配區(qū)域離開時(shí)，地形匹配算法停止執(zhí)行。

跟蹤模式下的可靠性判斷機(jī)制用于檢測跟蹤定位結(jié)果的可靠性，可靠性判斷機(jī)制的原理如圖4所示：根據(jù)慣性器件在短時(shí)間內(nèi)漂移小的特性，利用t_k的位置序列根據(jù)推算公式推算當(dāng)前時(shí)刻t_n的位置信息得到當(dāng)前位置估計(jì)值，推算公式為

式中，(λ(t_k)，L(t_k))為t_k時(shí)刻的位置，(λ(t_n)，L(t_n))為t_n時(shí)刻的位置，和分別為東向和北向的在t_k～t_n時(shí)間段內(nèi)的平均速度，R_N和R_M分別為地球的子午圈半徑和卯酉圈半徑。

獲得推算值后，分別與通過擴(kuò)展卡爾曼濾波得到的當(dāng)前時(shí)刻位置的濾波值進(jìn)行比較，如果估計(jì)值與濾波值差的絕對(duì)值均小于1個(gè)網(wǎng)格，則表明定位結(jié)果可靠；否則，定位結(jié)果不可靠。實(shí)現(xiàn)跟蹤模式下的可靠性判斷機(jī)制的具體步驟為：

步驟1)：在執(zhí)行搜索模式時(shí)初始化位置序列；

位置序列的長度一般選取5～10個(gè)采樣點(diǎn)。

步驟2)：進(jìn)入跟蹤模式時(shí)，根據(jù)位置序列的初始值和時(shí)間間隔T估計(jì)當(dāng)前時(shí)刻的位置并與跟蹤模式下濾波的位置進(jìn)行比較，如果位置差的絕對(duì)值均小于1個(gè)網(wǎng)格的誤差范圍則表示定位結(jié)果可靠，否則定位結(jié)果不可靠。時(shí)間間隔T的選取的準(zhǔn)則為：

式中，RES為高程數(shù)據(jù)庫網(wǎng)格分辨率的大小，和分別為東向和北向的加速度計(jì)零偏。

本領(lǐng)域普通技術(shù)人員可以理解：實(shí)現(xiàn)上述實(shí)施例的全部或部分，可以通過程序指令或相關(guān)的硬件來完成，前述的程序可以存儲(chǔ)于計(jì)算機(jī)可讀取存儲(chǔ)介質(zhì)中，該程序在執(zhí)行時(shí)，執(zhí)行包括上述實(shí)施例的步驟；而前述的存儲(chǔ)介質(zhì)包括：移動(dòng)存儲(chǔ)設(shè)備、只讀存儲(chǔ)器(Read-Only Memory，ROM)、隨機(jī)存取存儲(chǔ)器(Random Access Memory，RAM)、磁碟或者光盤等各種可以存儲(chǔ)程序代碼的介質(zhì)。

本發(fā)明適用于地形輔助導(dǎo)航系統(tǒng)。以上所述，僅為本發(fā)明的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。

以上所述，僅為本發(fā)明的具體實(shí)施方式，但本發(fā)明的保護(hù)范圍并不局限于此，任何熟悉本技術(shù)領(lǐng)域的技術(shù)人員在本發(fā)明揭露的技術(shù)范圍內(nèi)，可輕易想到變化或替換，都應(yīng)涵蓋在本發(fā)明的保護(hù)范圍之內(nèi)。因此，本發(fā)明的保護(hù)范圍應(yīng)所述以權(quán)利要求的保護(hù)范圍為準(zhǔn)。