基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法
【專利摘要】本發(fā)明涉及基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法，主要解決現(xiàn)有地形輪廓匹配方法不能修正慣性導航系統(tǒng)速度誤差的問題。其實現(xiàn)步驟是：1)根據(jù)慣性導航系統(tǒng)指示在地形高程圖中找到航行器的位置；2)根據(jù)航行器的位置，獲取實測高程序列的起始坐標；3)根據(jù)實測高程序列的起始坐標，獲取實測地形高程序列，并構(gòu)造地形匹配起始點搜索區(qū)；4)根據(jù)地形匹配起始點搜索區(qū)的范圍讀取搜索區(qū)高程序列，計算搜索區(qū)各高程序列與實測高程序列的平方差，得到地形匹配位置坐標；6)根據(jù)實測高程序列起始坐標和地形匹配位置坐標對慣性導航系統(tǒng)進行速度修正。本發(fā)明減小了慣性導航系統(tǒng)的速度誤差和地形匹配位置誤差，可用于地形匹配輔助導航。
【專利說明】
基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法
技術領域
[0001] 本發(fā)明屬于數(shù)據(jù)處理技術領域，特別涉及一種慣性導航系統(tǒng)速度累積誤差修正方 法，可用于組合導航。
【背景技術】
[0002] 隨著科學技術的飛速發(fā)展，國內(nèi)外已先后研制出了慣性導航系統(tǒng)和GPS全球定位 導航系統(tǒng)等。GPS全球定位導航系統(tǒng)是一種高精度全球三維實時衛(wèi)星導航系統(tǒng)，具有精度 高，可用性好的優(yōu)點，但也存在動態(tài)環(huán)境中可靠性差，導航定位是非自主式，且GPS技術受美 國控制，因此關鍵技術受制于他人;慣性導航系統(tǒng)INS，具有自主性、隱蔽性、抗干擾性強，短 時間內(nèi)導航定位精度高，實時連續(xù)的提供導航信息等優(yōu)點，但也存在導航位置和速度信息 隨著時間累積而發(fā)生漂移產(chǎn)生較大誤差的缺點。由于慣性導航系統(tǒng)定位誤差隨時間積累， 必須進行補償和修正，解決這個問題的主要途徑之一是發(fā)展組合導航技術。因此出現(xiàn)了地 形輔助導航TAN技術，它被用來修正慣性導航系統(tǒng)的累積誤差。
[0003] 所謂地形輔助導航，實質(zhì)上是由慣性導航系統(tǒng)、雷達高度表、氣壓高度表和數(shù)字地 圖構(gòu)成的組合導航系統(tǒng)，該系統(tǒng)的最大特征就是能夠修正慣性導航系統(tǒng)的導航信息，消除 慣性導航系統(tǒng)的累積誤差，提高導航精度。
[0004] 現(xiàn)有的地形輔助導航方法是地形輪廓匹配TERC0M輔助導航方法。地形輪廓匹配方 法是通過控制航行器航行的速度和時間，間隔的進行地形高程采樣，從而獲得地形高程值 的實測數(shù)據(jù)。通過將實測高程序列與機載數(shù)字地形高程圖上的高程序列進行相關計算，獲 得當前地形匹配位置信息。
[0005] 上述這種形輪廓匹配輔助導航方法，所說在速度累積誤差較小的情況下可以消除 慣性導航系統(tǒng)的位置誤差，但對于慣性導航系統(tǒng)的速度累積誤差無法進行修正，導致航行 偏移。這是因為在慣性導航系統(tǒng)工作的初期，由于慣性導航系統(tǒng)工作時間較短，慣性導航系 統(tǒng)的速度累積誤差較小，對現(xiàn)有的地形輪廓匹配輔助導航方法影響較小；隨著慣性導航系 統(tǒng)工作時間的增加，慣性導航系統(tǒng)的速度產(chǎn)生較大的累積誤差，嚴重的影響了地形輪廓匹 配輔助導航的性能，最終導致輔助導航系統(tǒng)產(chǎn)生較大的定位導航誤差，產(chǎn)生航行偏移。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明的目的在于針對現(xiàn)有的形輪廓匹配輔助導航方法的不足，提出一種基于地 形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法，以減小慣性導航系統(tǒng)速度累積誤差， 提高了地形輪廓匹配的性能和穩(wěn)定性，避免航行偏移。
[0007] 本發(fā)明的技術關鍵是:利用地形輪廓匹配技術和卡爾曼濾波器技術，對慣性導航 系統(tǒng)的速度累積誤差進行修正，其實現(xiàn)步驟如下：
[0008] (1)讀取當前機載慣性導航系統(tǒng)指示的航行器位置坐標，根據(jù)此位置坐標，在機載 數(shù)字地形高程圖中找到當前航行器的位置；
[0009] (2)判斷航行器是否處在地形匹配區(qū)域中：
[0010] 若不在地形匹配區(qū)域中，則返回（1);
[0011] 若處在地形匹配區(qū)域中，則調(diào)整航行器的航行姿態(tài)和速度v進行勻速直線航行，并 把當前慣性導航系統(tǒng)指示位置的坐標作為實測高程序列的起始坐標；
[0012] (3)根據(jù)實測高程序列的起始坐標，獲取實測地形高程序列；
[0013] (4)根據(jù)實測高程序列的起始坐標，構(gòu)造地形匹配起始點搜索區(qū)；
[0014] (5)判斷地形起始點匹配搜索區(qū)是否完全在地形匹配區(qū)域中：
[0015] 若不完全在該地形匹配區(qū)域中，則不能進行本次地形匹配；
[0016] 若完全在地形匹配區(qū)域中，則執(zhí)行(6);
[0017] (6)分別計算搜索區(qū)各高程序列和實測高程序列的平方差值，并比較各平方差值， 得到地形匹配位置坐標；
[0018] (7)根據(jù)實測高程序列起始坐標和地形匹配位置坐標對慣性導航系統(tǒng)的速度進行 修正：
[0019] 7a)計算k時刻實測高程序列起始點位置坐標和k時刻地形匹配位置坐標的差值矩 陣Zk:
[0021] 其中，k為大于0的自然數(shù)，4sSk時刻實測高程序列起始點位置的東向坐標值，.^ 為k時刻實測高程序列起始點位置的北向坐標值，4^為k時刻地形匹配位置的東向坐標值， 時刻地形匹配位置的北向坐標值；
[0022] 7b)將差值矩陣Zk作為卡爾曼濾波器的測量值輸入給卡爾曼濾波器，并輸入卡爾 曼濾波器均方誤差矩陣初始值po和卡爾曼濾波器狀態(tài)估計值初始值毛，進行卡爾曼濾波遞 推計算，輸出卡爾曼濾波狀態(tài)估計值夂；
[0023] 7c)根據(jù)卡爾曼濾波器輸出濾波速度狀態(tài)估計值九^之，對慣性導航系統(tǒng)的 速度進行修正，得到修正后的速度h
[0024] f = v +
[0025]其中，v為前時刻慣性導航系統(tǒng)的速度，（?）T表示矩陣的轉(zhuǎn)置。
[0026] 本發(fā)明與現(xiàn)有技術相比具有如下優(yōu)點：
[0027] 本發(fā)明方法利用卡爾曼濾波器對慣性導航系統(tǒng)速度累積誤差進行了修正，較好的 抑制了由速度累積誤差引起的地形匹配位置誤差，從而提高了現(xiàn)有的地形輪廓匹配輔助導 航方法的性能和穩(wěn)定性。
【附圖說明】
[0028] 下面結(jié)合附圖和【具體實施方式】對本發(fā)明做進一步詳細說明。
[0029] 圖1是本發(fā)明的實現(xiàn)流程圖；
[0030] 圖2是分別使用現(xiàn)有地形輪廓匹配方法和本發(fā)明方法進行地形匹配后的航跡對比 圖；
[0031] 圖3是分別使用現(xiàn)有地形輪廓匹配方法和本發(fā)明方法的速度變化曲線對比圖。
【具體實施方式】
[0032] 參照圖1，本發(fā)明的基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法，實 現(xiàn)步驟如下：
[0033] 步驟1，讀取當前慣性導航系統(tǒng)指示航行器所處位置的坐標，并確定該坐標在機載 數(shù)字高程圖中的位置。
[0034] la)在航行器航行的過程中實時的讀取當前機載慣性導航系統(tǒng)指示航行器所處位 置的坐標；
[0035] lb)根據(jù)機載慣性導航系統(tǒng)指示的航行器位置坐標，在機載數(shù)字地形高程圖中找 到航行器的位置。
[0036] 步驟2,獲取實測高程序列的起始坐標。
[0037] 根據(jù)步驟1在機載數(shù)字地形高程圖中找到航行器所處位置的坐標，判斷航行器是 否在地形匹配區(qū)域中：
[0038] 若不在地形匹配區(qū)域中，則返回步驟1;
[0039] 若處在地形匹配區(qū)域中，則調(diào)整航行器的航行姿態(tài)和速度V，使其進行勻速直線航 行，并把當前慣性導航系統(tǒng)指示位置的坐標作為實測高程序列的起始坐標。
[0040] 步驟3,根據(jù)實測高程序列的起始坐標，獲取實測地形高程序列。
[0041] 3a)根據(jù)當前航行速度v和機載數(shù)字地形高程圖網(wǎng)格距離d，計算高度采樣時間間 隔A t:
[0043] 3b)在航行器航行過程中，以實測高程序列的起始坐標為起始點，以A t為時間間 隔，依次讀取i次雷達高度表實測航行器的離地高度組成離地高度序列Ha= [n，…，rm，~ ri]，同時依次讀取i次氣壓高度表實測航行器的海拔高度組成海拔高度序列Hb= [h，…， lm, ???li]；
[0044] 其中rm為第m個離地高度測量值，lm為第m個海拔高度測量值，m為整數(shù)且l〈m$i，H a 和Hb分別為1 X i維的矩陣；
[0045] 3c)根據(jù)雷達高度表實測航行器的離地高度序列&和氣壓高度表實測航行器的海 拔高度序列H b計算航行器航線下方實測高程序列A H:
[0046] AH=Hb_Ha=[ AHi，...，AHv AHi]，
[0047] 其中AHm為第m個地形高度測量值，m為整數(shù)且為實測高程序列矩陣中元 素的總數(shù)，ah為IX i維的矩陣。
[0048] 步驟4,根據(jù)實測高程序列的起始坐標，構(gòu)造地形匹配起始點搜索區(qū)。
[0049] 4a)根據(jù)慣性導航系統(tǒng)的性能設定當前慣性導航系統(tǒng)位置誤差估計值矩陣〇:
[0050] 〇 = [0x，0y]，
[0051] 其中，〇x和〇y分別為在東北天導航坐標系中東向位置誤差估計值和北向位置誤差 估計值；
[0052] 4b)根據(jù)實測高程序列的起始坐標和東向位置誤差估計值〇x和北向位置誤差估計 值％，構(gòu)造匹配高程序列起始點的搜索區(qū)域，該匹配高程序列起始點的搜索區(qū)域是以實測 高程序列的起始點坐標值為中心，其長為6〇x，寬為6〇y的矩形區(qū)域。
[0053] 步驟5,判斷地形起始點的匹配搜索區(qū)。
[0054] 判斷地形起始點匹配搜索區(qū)是否完全在地形匹配區(qū)域中：
[0055] 若地形起始點匹配搜索區(qū)不完全在地形匹配區(qū)域中，則不能進行本次地形匹配；
[0056] 若地形起始點匹配搜索區(qū)完全在地形匹配區(qū)域中，則執(zhí)行步驟6。
[0057] 步驟6,分別計算搜索區(qū)各高程序列和實測高程序列的平方差值，并比較各平方差 值，得到地形匹配位置坐標。
[0058] 6a)以匹配高程序列起始點搜索區(qū)域中的各網(wǎng)格坐標為匹配高程序列起始點的坐 標，然后沿航行器的速度方向依次在機載數(shù)字地形高程圖中讀取NX i個地形高程值h?; [0059] 其中，hnm為第n個匹配高程序列中第m個匹配地形高程值，n為整數(shù)且l〈n彡N，N為匹 配高程序列的總個數(shù)，m=l，2,…，i，m為整數(shù)且為實測高程序列矩陣中元素的總 數(shù)。
[0060] 6b)計算各匹配高程序列和實測高程序列的平方差值矩陣j :
[0061] j = [jl，."，jn，…，J.N]
[0062]
為第n個平方差值，AHm為實測高程序列矩陣H中第m個地形 高度測量值，n為整數(shù)且l〈n<N，N為匹配高程序列的總個數(shù)；
[0063] 6c)比較平方差值矩陣j中各元素的大小，得出平方差值矩陣j中元素的最小值 j_，該」_所對應匹配高程序列的起始點坐標即為地形匹配位置坐標。
[0064]步驟7,根據(jù)卡爾曼濾波器輸出的速度誤差估計值修正慣性導航系統(tǒng)的速度。
[0065] 7a)構(gòu)造卡爾曼濾波器的狀態(tài)變量x:
[0066] x=[8x,8y,8vx,8 vy]T
[0067] 其中Sx表示在導航坐標系中東向位置誤差，Sy表示在導航坐標系中北向位置誤差， 4表不在導航坐標系中東向速度誤差，\表不在導航坐標系中北向速度誤差，（?廠表不矩 陣的轉(zhuǎn)置；
[0068] 7b)構(gòu)造卡爾曼濾波器的狀態(tài)方程，其具體表達式如下：
[0069] xk = AkXk-i+Wk-i
[0070] 其中k表示時刻，取值為大于0的自然數(shù)，Xk為k時刻的狀態(tài)變量，
為k時刻的狀態(tài)轉(zhuǎn)換矩陣，dt為卡爾曼濾波器的濾波周期，xk-i為k-1時刻的狀態(tài)變量，cok_i 為k_l時刻的系統(tǒng)動態(tài)噪聲，且〇k-i是一個4 X 1維的列向量；
[0071] 7c)構(gòu)造卡爾曼濾波器的測量方程：
[0072] Zk = CkXk+Vk
[0073] 其中，Zk為k時刻卡爾曼濾波器的輸入差值矩陣，且Zk是一個2 X 1維的矩陣，
為k時刻的測量矩陣，vk為k時刻的測量噪聲，且vk是一個2 X 1維的列向量；
[0074] 7d)計算k時刻實測高程序列起始點位置坐標和k時刻地形匹配位置坐標的差值矩 陣Zk:
[0076] 其中，4,為k時刻實測高程序列起始點東向位置坐標jL為k時刻實測高程序列起 始點北向位置坐標，時刻地形匹配位置的東向坐標，為k時刻地形匹配位置的北向 坐標；
[0077] 7e)將差值矩陣Zk作為卡爾曼濾波器的測量值輸入給卡爾曼濾波器，并輸入卡爾 曼濾波器均方誤差矩陣初始值po和卡爾曼濾波器狀態(tài)估計值初始值通過如下遞推濾波 方程組進行卡爾曼濾波遞推計算，得到輸出卡爾曼濾波速度狀態(tài)估計值毛：
[0079]其中，為k時刻未考慮噪聲誤差的卡爾曼濾波均方誤差矩陣，Ak為k時刻的狀態(tài) 轉(zhuǎn)換矩陣，Pk-i為k-1時刻卡爾曼濾波輸出的均方誤差矩陣，（?）T表示矩陣的轉(zhuǎn)置，Qk-i為k-1時刻的系統(tǒng)動態(tài)噪聲協(xié)方差矩陣，Hk為k時刻卡爾曼濾波最小均方誤差條件下的增益矩 陣，ck為k時刻的測量矩陣，Rk為k時刻的測量噪聲協(xié)方差矩陣，p k為k時刻卡爾曼濾波輸出的 均方誤差矩陣，I為單位矩陣，4為k時刻卡爾曼濾波器輸出的濾波狀態(tài)估計值，爲、iSk-l時 刻卡爾曼濾波器輸出的濾波狀態(tài)估計值；
[0080] 7f)根據(jù)卡爾曼濾波器輸出濾波速度狀態(tài)估計值尤=&、，對慣性導航系統(tǒng)的 速度進行修正，得到修正后的速度I [0081 ] 為，.
[0082]其中，v為前時刻慣性導航系統(tǒng)的速度，（?）T表示矩陣的轉(zhuǎn)置。
[0083]本發(fā)明的效果可通過以下仿真進一步驗證。
[0084] 1.實驗場景
[0085] 航行器的航行速度為50m/s。選取的地形匹配區(qū)域是一塊40km X 40km地形起伏明 顯的地域，數(shù)字高程圖的網(wǎng)格距離為50m。慣性導航系統(tǒng)指示的東向和北向初始定位誤差均 為50m，指示的位置誤差增長率東向和北向分量均為0.14m/s，指示的速度初始誤差東向和 北向分量均為lm/s，指示的速度誤差增長率東向分量為0.06m/s、北向分量為0.04m/s。地形 高度測量誤差為零均值標準差為5m的高斯白噪聲，卡爾曼濾波周期為16s，仿真航行時間為 480s 〇
[0086] (2)實驗內(nèi)容與結(jié)果分析
[0087]實驗 1
[0088]根據(jù)實驗場景中參數(shù)設置，使航行器在選定的地形匹配區(qū)域上空模擬航行480s。 分別使用現(xiàn)有的地形輪廓匹配輔助導航方法和本發(fā)明的基于地形輪廓匹配的慣性導航系 統(tǒng)速度累積誤差修正方法進行地形匹配，并且畫出匹配定位后的航跡圖，結(jié)果如圖2所示。 [0089]從圖2可以看出隨著航行時間的增大現(xiàn)有的地形輪廓匹配輔助導航方法指示的航 跡會較大幅度的偏離真實航跡，而本發(fā)明方法的指示航跡就較好的和真實航跡重疊。說明 本發(fā)明方法的性能優(yōu)于現(xiàn)有的地形輪廓匹配輔助導航方法，能更好抑制地形輪廓匹配輔助 導航的匹配位置誤差。
[0090]實驗 2
[0091]根據(jù)實驗場景中參數(shù)設置，使航行器在選定的地形匹配區(qū)域上空模擬航行480s。 分別使用現(xiàn)有的地形輪廓匹配輔助導航方法和本發(fā)明的基于地形輪廓匹配的慣性導航系 統(tǒng)速度累積誤差修正方法進行地形匹配，并且畫出航行過程中的速度變化曲線圖，結(jié)果如 圖3所示。
[0092]從圖3可以看出，在使用現(xiàn)有的地形輪廓匹配輔助導航方法的時候不能對速度累 積誤差進行修正，隨著時間的增大慣性導航系統(tǒng)的速度誤差越來越大，而本發(fā)明方法增加 了速度誤差修正能力，使得慣性導航系統(tǒng)的速度誤差在航行過程中保持在較小的范圍內(nèi)， 能較好的抑制慣性導航系統(tǒng)的速度誤差。說明本發(fā)明方法在修正慣性導航系統(tǒng)速度誤差方 面性能優(yōu)于現(xiàn)有的方法。
【主權項】
1. 基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法，包括： (1) 讀取當前機載慣性導航系統(tǒng)指示的航行器位置坐標，根據(jù)此位置坐標，在機載數(shù)字 地形高程圖中找到當前航行器的位置； (2) 判斷航行器是否處在地形匹配區(qū)域中： 若不在地形匹配區(qū)域中，則返回（1); 若處在地形匹配區(qū)域中，則調(diào)整航行器的航行姿態(tài)和速度v進行勻速直線航行，并把當 前慣性導航系統(tǒng)指示位置的坐標作為實測高程序列的起始坐標； (3) 根據(jù)實測高程序列的起始坐標，獲取實測地形高程序列； (4) 根據(jù)實測高程序列的起始坐標，構(gòu)造地形匹配起始點搜索區(qū)； (5) 判斷地形起始點匹配搜索區(qū)是否完全在地形匹配區(qū)域中： 若不完全在該地形匹配區(qū)域中，則不能進行本次地形匹配； 若完全在地形匹配區(qū)域中，則執(zhí)行(6); (6) 分別計算搜索區(qū)各高程序列和實測高程序列的平方差值，并比較各平方差值，得到 地形匹配位置坐標； (7) 根據(jù)實測高程序列起始坐標和地形匹配位置坐標對慣性導航系統(tǒng)的速度進行修 正： 7a)計算k時刻實測高程序列起始點位置坐標和k時刻地形匹配位置坐標的差值矩陣Zk:其中，k為大于0的自然數(shù)，時刻實測高程序列起始點位置的東向坐標值，為讓 時刻實測高程序列起始點位置的北向坐標值，為k時刻地形匹配位置的東向坐標值， 為k時刻地形匹配位置的北向坐標值； 7b)將差值矩陣Zk作為卡爾曼濾波器的測量值輸入給卡爾曼濾波器，并輸入卡爾曼濾波 器均方誤差矩陣初始值P〇和卡爾曼濾波器狀態(tài)估計值初始值表，進行卡爾曼濾波遞推計 算，輸出卡爾曼濾波速度狀態(tài)估計值毛； 7c)根據(jù)卡爾曼濾波器輸出濾波速度狀態(tài)估計值元= 對慣性導航系統(tǒng)的速度進 行修正，得到修正后的速度々： V = V + xk. ， 其中，V為前時刻慣性導航系統(tǒng)的速度，（?）T表示矩陣的轉(zhuǎn)置。2. 根據(jù)權利要求1所述的基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法， 其中步驟(3)中根據(jù)實測高程序列的起始坐標，獲取實測地形高程序列，按如下步驟進行： 3a)根據(jù)當前航行速度v和機載數(shù)字地形高程圖網(wǎng)格距離d，計算高度采樣時間間隔A t ?3b)在航行器航行過程中，以實測高程序列的起始坐標為起始點，以A t為時間間隔，依 次讀取i次雷達高度表實測航行器的離地高度組成離地高度序列Ha= [n，…，rm，…^]，同 時依次讀取i次氣壓高度表實測航行器的海拔高度組成海拔高度序列Hb = [ h，…，lm，… h]; 其中rm為第m個離地高度測量值，lm為第m個海拔高度測量值，m為整數(shù)且l〈m< i，&和拖 分別為IX i維的矩陣； 3c)根據(jù)雷達高度表實測航行器的離地高度序列仏和氣壓高度表實測航行器的海拔高 度序列Hb計算航行器航線下方實測高程序列A H: AH=Hb-Ha=[ AHi,-'-, AHm,-" AHi], 其中AHm為第m個地形高度測量值，m為整數(shù)且l〈m<i，i為實測高程序列矩陣中元素的 總數(shù)，AH為IX i維的矩陣。3. 根據(jù)權利要求1所述的基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法， 其中步驟(4)中根據(jù)實測高程序列的起始坐標，構(gòu)造地形匹配起始點搜索區(qū)，是根據(jù)實測高 程序列的起始坐標和當前設定慣性導航系統(tǒng)的東向位置誤差估計值〇 x和北向位置誤差估 計值％，構(gòu)造匹配高程序列起始點的搜索區(qū)域，該匹配高程序列起始點的搜索區(qū)域是以實 測高程序列的起始點坐標值為中心，長為6〇 x，寬為6〇y的矩形區(qū)域。4. 根據(jù)權利要求1所述的基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法， 其中步驟(6)中分別計算搜索區(qū)各高程序列和實測高程序列的平方差值，并比較各平方差 值，得到地形匹配位置坐標，按如下步驟進行： 6a)以匹配高程序列起始點搜索區(qū)域中的各網(wǎng)格坐標為匹配高程序列起始點的坐標， 然后沿航行器的速度方向依次在機載數(shù)字地形高程圖中讀取NX i個地形高程值hnm，其中， hnm為第n個匹配高程序列中第m個匹配地形高程值，n為整數(shù)且l〈n<N，N為匹配高程序列的 總個數(shù)，m=l，2,…，i，m為整數(shù)且為實測高程序列矩陣中元素的總數(shù)； 6b)計算各匹配高程序列和實測高程序列的平方差值矩陣j: j=[jl,-'-, jn,-", J'n]其中，j為1 xN維矩陣，A仏為實測高程序列a H中第m個地形高度測量值，jr^n個平方 差值，n為整數(shù)且l〈n<N，N為匹配高程序列的總個數(shù)； 6c)比較平方差值矩陣j中各元素的大小，得出平方差值矩陣j中元素的最小值jmin，則 jmin所對應匹配高程序列的起始點坐標即為地形匹配位置坐標。5. 根據(jù)權利要求1所述的基于地形輪廓匹配的慣性導航系統(tǒng)速度累積誤差修正方法， 其中步驟(7b)中進行卡爾曼濾波遞推計算，通過如下遞推濾波方程組進行：其中，為k時刻未考慮噪聲誤差的卡爾曼濾波均方誤差矩陣，Ak為k時刻的狀態(tài)轉(zhuǎn)換 矩陣，Pk-i為k-1時刻卡爾曼濾波輸出的均方誤差矩陣，（? "表不矩陣的轉(zhuǎn)置，Qk-i為k-1時 刻的系統(tǒng)動態(tài)噪聲協(xié)方差矩陣，Hk為k時刻卡爾曼濾波最小均方誤差條件下的增益矩陣，ck 為k時刻的測量矩陣，Rk為k時刻的測量噪聲協(xié)方差矩陣，p k為k時刻卡爾曼濾波輸出的均方 誤差矩陣，I為單位矩陣，4為k時刻卡爾曼濾波器輸出的濾波狀態(tài)估計值，^ iSk-l時刻卡 爾曼濾波器輸出的濾波狀態(tài)估計值。
【文檔編號】G01C21/16GK106052688SQ201610644276
【公開日】2016年10月26日
【申請日】2016年8月8日 公開號201610644276.2, CN 106052688 A, CN 106052688A, CN 201610644276, CN-A-106052688, CN106052688 A, CN106052688A, CN201610644276, CN201610644276.2
【發(fā)明人】周宇, 萬俊, 張林讓, 楊慧婷, 蒙妍
【申請人】西安電子科技大學, 陜西長嶺電子科技有限責任公司