專利名稱：：一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法
技術(shù)領(lǐng)域：
：本發(fā)明涉及一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，適用于各類慣性導(dǎo)航系統(tǒng)的初始對準(zhǔn)，屬于慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)
技術(shù)領(lǐng)域：
。
背景技術(shù)：
：慣性導(dǎo)航系統(tǒng)是復(fù)雜的高精度機電綜合系統(tǒng)，由于具有完全自主性的優(yōu)點而廣泛應(yīng)用于陸?？仗祛I(lǐng)域。慣性導(dǎo)航系統(tǒng)在轉(zhuǎn)入導(dǎo)航狀態(tài)前需要進(jìn)行初始對準(zhǔn)以提供必要的初始值，依據(jù)載體的運動情況劃分，對準(zhǔn)分為靜基座對準(zhǔn)和運動對準(zhǔn)。同靜基座對準(zhǔn)相比，運動對準(zhǔn)在縮短對準(zhǔn)時間、提高對準(zhǔn)精度和機動性等方面具有明顯的優(yōu)勢。為實現(xiàn)慣性導(dǎo)航系統(tǒng)在運動過程中的對準(zhǔn)，必須引入外部觀測信息(包括速度、位置等)。對于陸用車輛而言，通常采用全球定位系統(tǒng)(GPS)作為輔助導(dǎo)航設(shè)備來實現(xiàn)慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)。目前已有的GPS輔助慣性導(dǎo)航系統(tǒng)實現(xiàn)運動對準(zhǔn)的方法，即傳統(tǒng)卡爾曼濾波方法，其原理是根據(jù)GPS輸出的位置或速度與慣性導(dǎo)航系統(tǒng)的位置或速度之差作為觀測量，在慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的基礎(chǔ)上，采用傳統(tǒng)卡爾曼濾波方法對慣性導(dǎo)航系統(tǒng)的誤差狀態(tài)進(jìn)行估計，對姿態(tài)矩陣進(jìn)行開環(huán)或閉環(huán)方式反饋，實現(xiàn)運動對準(zhǔn)。這種方法的缺點是在實際應(yīng)用中，由于車輛的隨機振動和行駛環(huán)境(如山區(qū)、隧道等)的影響，GPS的速度輸出帶有各種干擾噪聲，且干擾信號統(tǒng)計特性不完全已知。對這些受干擾的觀測量如果不采取有效的處理方法而直接進(jìn)行卡爾曼濾波，則不僅會降低當(dāng)前的估計精度，對于后續(xù)的估計效果也會產(chǎn)生影響，嚴(yán)重時會引起濾波發(fā)散，導(dǎo)致對準(zhǔn)失敗。
發(fā)明內(nèi)容本發(fā)明的目的是為了克服已有技術(shù)存在的不足，提出一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法。本發(fā)明的基本原理是在慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型和觀測方程的基礎(chǔ)上，以GPS輸出速度作為觀測量，采用卡爾曼濾波對載車的加加速度進(jìn)行估計，并判別GPS速度輸出是否為野值。若GPS速度輸出為野值，采用卡爾曼濾波平滑方法對當(dāng)前狀態(tài)進(jìn)行遞推；否則，采用主、從卡爾曼濾波器來實現(xiàn)慣性導(dǎo)航系統(tǒng)狀態(tài)和噪聲統(tǒng)計特性的同步估計，主濾波器對慣性導(dǎo)航系統(tǒng)的狀態(tài)進(jìn)行估計，其新息和方差作為從濾波器的觀測量；從濾波器對主濾波器觀測噪聲的均值和方差進(jìn)行估計，估計結(jié)果為下一次主濾波器的濾波提供噪聲特性輸入；從而實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)的運動對準(zhǔn)。本發(fā)明的目的是通過下述技術(shù)方案實現(xiàn)的。一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，包括如下步驟步驟一、建立包括位置誤差、速度誤差、失準(zhǔn)角和慣性器件誤差漂移的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型；動態(tài)誤差模型為Φ角誤差方程或Ψ角誤差方程；在東北天坐標(biāo)系下，動態(tài)誤差模型表示如公式1所示。i:(0=F(0-v:(0+H;(0(1)式中，t為時間值，是正實數(shù)；χ(t)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量，由位置誤差SP、速度誤差δVn、失準(zhǔn)角陀螺儀零偏￡g和加速度計零偏▽組成；w(t)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲；F(t)為轉(zhuǎn)移矩陣；表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量的變化量。對該動態(tài)誤差模型進(jìn)行離散化處理，可得Xk=FHXkJwk(2)式中，k表示時間值，為正整數(shù)Jlri表示離散化的慣性導(dǎo)航系統(tǒng)的一步轉(zhuǎn)移矩陣，Xk為離散化后的k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)向量，Wk表示k時刻的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲，是均值為零，方差為Q的白噪聲序列，Q值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Q為正實數(shù)。步驟二、建立步驟一所述的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程，如公式3所示。式中，Yk表示k時刻的觀測量；&_表示慣性導(dǎo)航系統(tǒng)的速度輸出；廠/#表示經(jīng)桿臂補償后GPS的速度輸出；Hk為k時刻的觀測矩陣，Hk=；O3x3表示3階時刻的慣性導(dǎo)航系統(tǒng)v『，計算水平合速度噪聲序列，2值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，β為正實數(shù)。步驟三、對GPS輸出進(jìn)行實時野值檢測。通過GPS測量得到的當(dāng)前時刻載車東向速度和北向速度Zk，并以此作為觀測量，建立加加速度跟蹤模型，如公式4所示。時刻水平加速度和水平加加速ζk表示離散后加加速度跟蹤模式中，t為時間值，為正實數(shù)；v(t)表示水平合速度，a(t)和々(0分別表示水平加速度和水平加加速度；ζ(t)表示加加速度跟蹤模型的系統(tǒng)噪聲，是均值為零，方差為。的白對公式4進(jìn)行離散化，離散化后的系統(tǒng)方程表示為式中，Vk表示k時刻水平合速度，ak和分別表示度，Gk^1表示離散化的加加速度跟蹤模型的一步轉(zhuǎn)移矩陣，型的系統(tǒng)噪聲。加加速度跟蹤模型的觀測方程表示為0矩陣；I3x3表示3階單位陣，O3x9表示3行9列的0矩陣；nk表示k動態(tài)誤差模型的觀測方程噪聲。式中，Zk表示k時刻水平合速度的觀測量，其計算公式為:Zk=V(vD2+(ν,Γ)2，Ik表示加加速度跟蹤模型的觀測方程噪聲，是均值為零，方差為及的白噪聲序列，及值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，及為正實數(shù)。根據(jù)公式5和公式6，采用卡爾曼濾波器即可估計出k時刻水平合速度vk、相應(yīng)的加速度ak和加加速度和。根據(jù)載車的類型和對準(zhǔn)動態(tài)情況，預(yù)先設(shè)定閾值Jmax，如果μμ成立，則判斷當(dāng)前的觀測量為有效值，執(zhí)行步驟四；否則，判斷其為野值，在該時刻做卡爾曼平滑，同時將k+Ι的值賦給k，然后重復(fù)步驟三。步驟四、對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲ηk的均值Mf和方差Cf進(jìn)行估計；根據(jù)步驟二中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程得到的k時刻的觀測量Yk以及觀測矩陣Hk，采用主、從兩個卡爾曼濾波器分別對k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)xk、k時刻的觀測噪聲nk的均值Mf和方差Cf進(jìn)行估計；具體為Mf和Cf的狀態(tài)方程為表示主對角線元素；該狀態(tài)方程的系統(tǒng)噪聲h\r，是均值為零，方差為(^的白噪聲序列，Qk值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Qk為正實數(shù);Mi'%Hi1行1列的向量；I1為HI1行HI1列的單位陣；diag(Cf)為m2行1列的向量；I2為m2行m2列的單位陣。公式7對應(yīng)的觀測方程為式中，量測噪聲是均值為零，方差為Rk的白噪聲序列，Rk值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Rk為正實數(shù)。根據(jù)公式7和公式9，米用從卡爾曼濾波器即可估計出k時刻的觀測噪聲Ilk的均值Mf和方差Cf；從卡爾曼濾波器為經(jīng)典卡爾曼濾波器。步驟五、對慣性導(dǎo)航系統(tǒng)的狀態(tài)Xk進(jìn)行估計；根據(jù)步驟一建立的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型及步驟二中的觀測方程，結(jié)合步驟四給出的觀測噪聲ηk的均值Mf和方差Cf的估計值，采用主卡爾曼濾波器對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk進(jìn)行估計。具體為主卡爾曼濾波器為經(jīng)典卡爾曼濾波器的改進(jìn)，計算過程如公式1014所示Pk＝Pk|k-1－KkHkPk|k-1(14)其中，^1W表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk的一步預(yù)測；Pkllri表示一步預(yù)測方差；么表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk的估計值；Pk表示估計方差；Kk表示濾波增益；if(l)表示步驟四中M^的估計值；對(2)表示Cf的估計值，其主對角線元素等于步驟四中diag(Cf)的估計值，非主對角線的元素為0。經(jīng)過上述步驟即可得到k時刻慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型狀態(tài)向量Xk的估計值L·，包含位置誤差(δP)k，速度誤差(δVn)k和失準(zhǔn)角辨。利用這些誤差估計結(jié)果對慣性導(dǎo)航系統(tǒng)的位置輸出/Tw、速度輸出Waw和姿態(tài)矩陣輸出進(jìn)行校正，通過公式1517即可得到修正后的載車的位置A、速度A、姿態(tài)矩陣(々>。其中，/Tv、和O分別表示k時刻慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)矩陣，是已知量；I3x3表示3階單位陣；辨χ表示由辦構(gòu)成的斜負(fù)對稱陣。步驟六、對步驟一中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型一步轉(zhuǎn)移矩陣Flri進(jìn)行更新，同時將k+Ι的值賦給k，然后返同到步驟二。有益效果與已有的GPS輔助慣性導(dǎo)航系統(tǒng)實現(xiàn)運動對準(zhǔn)的方法比較，本發(fā)明方法放寬了傳統(tǒng)卡爾曼濾波方法對噪聲特性的苛刻要求，解決了車輛運動過程中由于GPS包含各類未知噪聲而造成對準(zhǔn)精度不高及對準(zhǔn)時間延長的缺陷；野值檢測方法充分利用了運動對準(zhǔn)過程中車輛的行駛特點，采用卡爾曼濾波方法實現(xiàn)了實時檢測；噪聲特性同樣采用卡爾曼濾波實現(xiàn)實時估計，與現(xiàn)有技術(shù)相比，不需要對當(dāng)前時刻之前的若干個時刻殘差進(jìn)行存儲，使其更加適用于慣性導(dǎo)航系統(tǒng)在環(huán)境惡劣下的初始對準(zhǔn)。圖1為本發(fā)明的具體實施例中的載車運行軌跡示意圖；圖2為本發(fā)明的具體實施例中的俯仰角誤差比較示意圖；圖3為本發(fā)明的具體實施例中的橫滾角誤差比較示意圖；圖4為本發(fā)明的具體實施例中的航向角誤差比較示意圖。具體實施例方式下面結(jié)合附圖和具體實施例對本發(fā)明進(jìn)行詳細(xì)說明。本實施例中，將慣性導(dǎo)航系統(tǒng)固聯(lián)在載車上，將GPS接收器安裝在車頂。載車靜止30秒進(jìn)行粗對準(zhǔn)后開始運動，運動軌跡如圖1所示，其橫坐標(biāo)為緯度，縱坐標(biāo)為經(jīng)度。3個陀螺儀的隨機漂移均為0.01/h,常值漂移均為0.02/h；3個加速度計隨機漂移均為50μg，常值漂移均為100μg；初始緯度為39.800343；初始經(jīng)度為116.166874；初始高程為40.87m；野值檢測閾值.Jfflax=0.5；慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量知為零向量；慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲nk的均值Mf和方差Cf的狀態(tài)方程中的初始值i。R為零向量。其過程如下步驟一、建立包括位置誤差、速度誤差、失準(zhǔn)角和慣性器件誤差漂移的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型；動態(tài)誤差模型為Φ角誤差方程或Ψ角誤差方程；在東北天坐標(biāo)系下，動態(tài)誤差模型表示如公式1所示。動態(tài)誤差模型為Φ角誤差方程，F(xiàn)(t)表示為各個矩陣塊的形式表示為式中，[PEPnPu]表示地球坐標(biāo)系相對地理坐標(biāo)系的轉(zhuǎn)動角速率；L和h分別表示載體所處緯度和高程；Rm和Rt分別表示卯酉圈和子午圈的半徑；R_表示卯酉圈的半徑關(guān)于緯度求導(dǎo)，可表示如下Rmm=SRm/dL=6Roesin(Z)cos(Z)(20)其中，Rtl為地球半徑，e為橢圓率。式中，[ωΕωηω]表示地球自轉(zhuǎn)角速率在東北天方向的分量；[νΕvNVu]表示載體在東北天方向的運動速度；Rtt表示子午圈的半徑關(guān)于緯度求導(dǎo)，可表示如下Ru=dR1/dL=2Roesin(I)cos(I)式中，[fEfNfU]表示加速度計測量的東向、北向和天向的比力,F25=Cnh式中，Cnb表示姿態(tài)矩陣.對該動態(tài)誤差模型進(jìn)行離散化處理，可得公式2⑵步驟二、建立步驟一所述的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程，如公式3所不。式中，Yk表示k時刻的觀測量；FTs'表示慣性導(dǎo)航系統(tǒng)的速度輸出；表示經(jīng)桿臂補償后GPS的速度輸出；Hk為k時刻的觀測矩陣，Hk=；O3x3表示3階ΠΦΕ眸.T豐宗q階你眸_η豐宗qτQ萬Il的ΠΦΕ眸.η豐宗時刻的慣性導(dǎo)航系統(tǒng)。步驟三、對GPS輸出進(jìn)行實時野值檢測。通過GPS測量得到的當(dāng)前時刻載車東向速度和北向速度νZk，并以此作為觀測量，建立加加速度跟蹤模型，如公式4所示。式中，t為時間值，為正實數(shù)；v(t)表示水平合速度，a(t)和表示主對角線元素；該狀態(tài)方程的系統(tǒng)噪聲rRH^r，是均值為零，方差為(^的白噪聲序列，Qk值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Qk為正實數(shù);Mf為Hi1行1列的向量；I1為HI1行HI1列的單位陣；diag(Cf)為m2行1列的向量；I2為m2行m2列的單位陣。公式7對應(yīng)的觀測方程為式中，Zk表示k時刻水平合速度的觀測量，其計算公式為-M=V(vD2+(νΓ')2，Ik表示加加速度跟蹤模型的觀測方程噪聲，是均值為零，方差為及的白噪聲序列，及值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，及為正實數(shù)。根據(jù)公式5和公式6，采用卡爾曼濾波器即可估計出k時刻水平合速度vk、相應(yīng)的加速度ak和加加速度和&。根據(jù)載車的類型和對準(zhǔn)動態(tài)情況，預(yù)先設(shè)定閾值Jmax，如果|么|^/_成立，則判斷當(dāng)前的觀測量為有效值，執(zhí)行步驟四；否則，判斷其為野值，在該時刻做卡爾曼平滑，同時將k+l的值賦給k，然后重復(fù)步驟三。步驟四、對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲ηk的均值Mf和方差C^進(jìn)行估計；根據(jù)步驟二中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程得到的k時刻的觀測量Yk以及觀測矩陣Hk，采用主、從兩個卡爾曼濾波器分別對k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)xk、k時刻的觀測噪聲nk的均值Mf和方差Cf進(jìn)行估計；具體為M〖和Cf的狀態(tài)方程為對公式4進(jìn)行離散化，離散化后的系統(tǒng)方程表示為式中，Vk表示k時刻水平合速度，ak和&分別表示k時刻水平加速度和水平加加速度，Gk—表示離散化的加加速度跟蹤模型的一步轉(zhuǎn)移矩陣，ζk表示離散后加加速度跟蹤模型的系統(tǒng)噪聲。加加速度跟蹤模型的觀測方程表示為式中，Ck表示殘差序列的方差，滿足Gif]TJdPPkllri的初始值分別為知和Po，均為人為設(shè)定值；此后，h和PklH由主卡爾曼濾波器提供；公式8可進(jìn)一步整理為式中，量測噪聲是均值為零，方差為Rk的白噪聲序列，Rk值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Rk為正實數(shù)。根據(jù)公式7和公式9，采用從卡爾曼濾波器即可估計出k時刻的觀測噪聲Ilk的均值Mf和方差C^；從卡爾曼濾波器為經(jīng)典卡爾曼濾波器。步驟五、對慣性導(dǎo)航系統(tǒng)的狀態(tài)Xk進(jìn)行估計；根據(jù)步驟一建立的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型及步驟二中的觀測方程，結(jié)合步驟四給出的觀測噪聲ηk的均值Mf和方差Cf的估計值，采用主卡爾曼濾波器對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk進(jìn)行估計。具體為主卡爾曼濾波器為經(jīng)典卡爾曼濾波器的改進(jìn)，計算過程如公式1014所示(12)(13)Pk=Pk丨H-KkHkPk丨Η(14)其中，JblM表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk的一步預(yù)測；Pkllri表示一步預(yù)測方差；;b表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk的估計值；Pk表示估計方差；Kk表示濾波增益；對(1)表示步驟四中Mf的估計值；對(2)表示C的估計值，其主對角線元素等于步驟四中diag(Cf)的估計值，非主對角線的元素為0。經(jīng)過上述步驟即可得到k時刻慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型狀態(tài)向量Xk的估計值L·，包含位置誤差(δP)k，速度誤差(δVn)k和失準(zhǔn)角辦。利用這些誤差估計結(jié)果對慣性導(dǎo)航系統(tǒng)的位置輸出Zfw、速度輸出’和姿態(tài)矩陣輸出進(jìn)行校正，通過公式1517即可得到修正后的載車的位置A、速度A、姿態(tài)矩陣(々>。(15)(16)其中，Cw、PTs'和(0分別表示k時刻慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)矩陣，是已知量；I3x3表示3階單位陣；辦χ表示由辦構(gòu)成的斜負(fù)對稱陣。步驟六、對步驟一中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型一步轉(zhuǎn)移矩陣Flri進(jìn)行更新，同時將k+Ι的值賦給k，然后返回到步驟二。為了說明本發(fā)明的效果，采用傳統(tǒng)卡爾曼濾波方法在相同的實驗設(shè)置下進(jìn)行試驗，得到俯仰角誤差比較圖如圖2所示，其橫坐標(biāo)為時間，縱坐標(biāo)為俯仰角誤差值；橫滾角誤差比較圖如圖3所示，其橫坐標(biāo)為時間，縱坐標(biāo)為橫滾角誤差值；航向角誤差比較圖如圖4所示，其橫坐標(biāo)為時間，縱坐標(biāo)為航向角誤差值。從這三幅圖中可以看出，采用本發(fā)明后對準(zhǔn)結(jié)果受環(huán)境擾動的影響較小，自收斂后(100秒-600秒)的均方差結(jié)果如表1所示。表1兩種方法估計結(jié)果的均方差比較表中均方差的計算結(jié)果表明本發(fā)明在抑制野值及噪聲特性的估計中能夠起到有效的作用。以上所述僅是本發(fā)明的優(yōu)選實施方式，應(yīng)當(dāng)指出，對于本
技術(shù)領(lǐng)域：
的普通技術(shù)人員來說，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)，或者對其中部分技術(shù)特征進(jìn)行等同替換，這些改進(jìn)和替換也應(yīng)視為本發(fā)明的保護(hù)范圍。權(quán)利要求一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，其特征在于包括如下步驟步驟一、建立包括位置誤差、速度誤差、失準(zhǔn)角和慣性器件誤差漂移的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型；在東北天坐標(biāo)系下，動態(tài)誤差模型表示如公式1所示；<mrow><mover><mi>x</mi><mo>&CenterDot;</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>=</mo><mi>F</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mi>x</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>+</mo><mi>w</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow>式中，t為時間值，是正實數(shù)；x(t)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量，由位置誤差δP、速度誤差δVn、失準(zhǔn)角陀螺儀零偏δg和加速度計零偏組成；w(t)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲；F(t)為轉(zhuǎn)移矩陣；表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量的變化量；對該動態(tài)誤差模型進(jìn)行離散化處理，可得xk＝Fk1xk1+wk(2)式中，k表示時間值，為正整數(shù)；Fk1表示離散化的慣性導(dǎo)航系統(tǒng)的一步轉(zhuǎn)移矩陣，xk為離散化后的k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)向量，wk表示k時刻的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲，是均值為零，方差為Q的白噪聲序列，Q值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Q為正實數(shù)；步驟二、建立步驟一所述的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程，如公式3所示；<mrow><msub><mi>Y</mi><mi>k</mi></msub><mo>=</mo><msubsup><mi>V</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msubsup><mi>V</mi><mi>k</mi><mi>GPS</mi></msubsup><mo>=</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>x</mi><mi>k</mi></msub><mo>+</mo><msub><mi>&eta;</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>3</mn><mo>)</mo></mrow></mrow>式中，Yk表示k時刻的觀測量；表示慣性導(dǎo)航系統(tǒng)的速度輸出；表示經(jīng)桿臂補償后GPS的速度輸出；Hk為k時刻的觀測矩陣，Hk＝[O3×3I3×3O3×9]；O3×3表示3階O矩陣；I3×3表示3階單位陣，O3×9表示3行9列的O矩陣；ηk表示k時刻的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲；步驟三、對GPS輸出進(jìn)行實時野值檢測；通過GPS測量得到的當(dāng)前時刻載車東向速度和北向速度計算水平合速度Zk，并以此作為觀測量，建立加加速度跟蹤模型，如公式4所示；<mrow><mfrac><mi>d</mi><mi>dt</mi></mfrac><mfencedopen='['close=']'><mtable><mtr><mtd><mi>v</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mover><mi>a</mi><mo>&CenterDot;</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>1</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mfencedopen='['close=']'><mtable><mtr><mtd><mi>v</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mi>a</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr><mtr><mtd><mover><mi>a</mi><mo>&CenterDot;</mo></mover><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>+</mo><mi>&zeta;</mi><mrow><mo>(</mo><mi>t</mi><mo>)</mo></mrow><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow>式中，t為時間值，為正實數(shù)；v(t)表示水平合速度，a(t)和分別表示水平加速度和水平加加速度；ζ(t)表示加加速度跟蹤模型的系統(tǒng)噪聲，是均值為零，方差為的白噪聲序列，值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，為正實數(shù)；對公式4進(jìn)行離散化，離散化后的系統(tǒng)方程表示為<mrow><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>&CenterDot;</mo></mover><mi>k</mi></msub></mtd></mtr></mtable></mfenced><mo>=</mo><msub><mi>G</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>&CenterDot;</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>+</mo><msub><mi>&zeta;</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow></mrow>式中，vk表示k時刻水平合速度，ak和分別表示k時刻水平加速度和水平加加速度，Gk1表示離散化的加加速度跟蹤模型的一步轉(zhuǎn)移矩陣，ζk表示離散后加加速度跟蹤模型的系統(tǒng)噪聲；加加速度跟蹤模型的觀測方程表示為<mrow><msub><mi>Z</mi><mi>k</mi></msub><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mn>1</mn></mtd><mtd><mn>0</mn></mtd><mtd><mn>0</mn></mtd></mtr></mtable></mfenced><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>v</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mi>a</mi><mi>k</mi></msub></mtd></mtr><mtr><mtd><msub><mover><mi>a</mi><mo>&CenterDot;</mo></mover><mi>k</mi></msub></mtd></mtr></mtable></mfenced><mo>+</mo><msub><mi>&xi;</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>6</mn><mo>)</mo></mrow></mrow>式中，Zk表示k時刻水平合速度的觀測量，其計算公式為ξk表示加加速度跟蹤模型的觀測方程噪聲，是均值為零，方差為的白噪聲序列，值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，為正實數(shù)；根據(jù)公式5和公式6，采用卡爾曼濾波器即可估計出k時刻水平合速度vk、相應(yīng)的加速度ak和加加速度和根據(jù)載車的類型和對準(zhǔn)動態(tài)情況，預(yù)先設(shè)定閾值Jmax，如果成立，則判斷當(dāng)前的觀測量為有效值，執(zhí)行步驟四；否則，判斷其為野值，在該時刻做卡爾曼平滑，同時將k+1的值賦給k，然后重復(fù)步驟三；步驟四、對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲ηk的均值和方差進(jìn)行估計；根據(jù)步驟二中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程得到的k時刻的觀測量Yk以及觀測矩陣Hk，采用主、從兩個卡爾曼濾波器分別對k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)xk、k時刻的觀測噪聲ηk的均值和方差進(jìn)行估計；具體為和的狀態(tài)方程為<mrow><msubsup><mi>x</mi><mrow><mi>k</mi><mo>+</mo><mn>1</mn></mrow><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>I</mi><mn>1</mn></msub></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><msub><mi>I</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced><msubsup><mi>x</mi><mi>k</mi><mi>R</mi></msubsup><mo>+</mo><msubsup><mi>w</mi><mi>k</mi><mi>R</mi></msubsup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>7</mn><mo>)</mo></mrow></mrow>式中，表示主對角線元素；該狀態(tài)方程的系統(tǒng)噪聲是均值為零，方差為QR的白噪聲序列，QR值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，QR為正實數(shù)；為m1行1列的向量；I1為m1行m1列的單位陣；為m2行1列的向量；I2為m2行m2列的單位陣；公式7對應(yīng)的觀測方程為<mrow><msubsup><mi>z</mi><mi>k</mi><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><msub><mi>Y</mi><mi>k</mi></msub><mo>-</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub></mtd></mtr><mtr><mtd><mi>diag</mi><mrow><mo>(</mo><msub><mi>C</mi><mi>k</mi></msub><mo>-</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><mo>)</mo></mrow></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>8</mn><mo>)</mo></mrow></mrow>式中，Ck表示殘差序列的方差，滿足和Pk|k1的初始值分別為和P0，均為人為設(shè)定值；此后，和Pk|k1由主卡爾曼濾波器提供；公式8可進(jìn)一步整理為<mrow><msubsup><mi>z</mi><mi>k</mi><mi>R</mi></msubsup><mo>=</mo><mfencedopen='['close=']'><mtable><mtr><mtd><mi>I</mi></mtd><mtd><mn>0</mn></mtd></mtr><mtr><mtd><mn>0</mn></mtd><mtd><mi>I</mi></mtd></mtr></mtable></mfenced><msubsup><mi>x</mi><mi>k</mi><mi>R</mi></msubsup><mo>+</mo><msubsup><mi>&eta;</mi><mi>k</mi><mi>R</mi></msubsup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>9</mn><mo>)</mo></mrow></mrow>式中，量測噪聲是均值為零，方差為RR的白噪聲序列，RR值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，RR為正實數(shù)；根據(jù)公式7和公式9，采用從卡爾曼濾波器即可估計出k時刻的觀測噪聲ηk的均值和方差從卡爾曼濾波器為經(jīng)典卡爾曼濾波器；步驟五、對慣性導(dǎo)航系統(tǒng)的狀態(tài)xk進(jìn)行估計；根據(jù)步驟一建立的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型及步驟二中的觀測方程，結(jié)合步驟四給出的觀測噪聲ηk的均值和方差的估計值，采用主卡爾曼濾波器對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量xk進(jìn)行估計；具體為主卡爾曼濾波器為經(jīng)典卡爾曼濾波器的改進(jìn)，計算過程如公式10～14所示<mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>10</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>=</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow><mi>T</mi></msubsup><mo>+</mo><msub><mi>Q</mi><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>11</mn><mo>)</mo></mrow></mrow><mrow><msub><mi>K</mi><mi>k</mi></msub><mo>=</mo><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><msup><mrow><mo>[</mo><msub><mi>H</mi><mi>k</mi></msub><msub><mi>P</mi><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msubsup><mi>H</mi><mi>k</mi><mi>T</mi></msubsup><mo>+</mo><msubsup><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi><mi>R</mi></msubsup><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow><mo>]</mo></mrow><mrow><mo>-</mo><mn>1</mn></mrow></msup><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>12</mn><mo>)</mo></mrow></mrow><mrow><msub><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi></msub><mo>=</mo><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>+</mo><msub><mi>K</mi><mi>k</mi></msub><mo>[</mo><msub><mi>Y</mi><mi>k</mi></msub><mo>-</mo><msub><mi>F</mi><mrow><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><msub><mover><mi>x</mi><mo>^</mo></mover><mrow><mi>k</mi><mo>|</mo><mi>k</mi><mo>-</mo><mn>1</mn></mrow></msub><mo>-</mo><msubsup><mover><mi>x</mi><mo>^</mo></mover><mi>k</mi><mi>R</mi></msubsup><mrow><mo>(</mo><mn>2</mn><mo>)</mo></mrow><mo>]</mo><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>13</mn><mo>)</mo></mrow></mrow>Pk＝Pk|k1KkHkPk|k1(14)其中，表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量xk的一步預(yù)測；Pk|k1表示一步預(yù)測方差；表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量xk的估計值；Pk表示估計方差；Kk表示濾波增益；表示步驟四中的估計值；表示的估計值，其主對角線元素等于步驟四中的估計值，非主對角線的元素為0；經(jīng)過上述步驟即可得到k時刻慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型狀態(tài)向量xk的估計值包含位置誤差(δP)k，速度誤差(δVn)k和失準(zhǔn)角利用這些誤差估計結(jié)果對慣性導(dǎo)航系統(tǒng)的位置輸出速度輸出和姿態(tài)矩陣輸出進(jìn)行校正，通過公式15～17即可得到修正后的載車的位置速度姿態(tài)矩陣<mrow><msub><mover><mi>P</mi><mo>~</mo></mover><mi>k</mi></msub><mo>=</mo><msubsup><mi>P</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msub><mrow><mo>(</mo><mi>&delta;P</mi><mo>)</mo></mrow><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>15</mn><mo>)</mo></mrow></mrow><mrow><msub><mover><mi>V</mi><mo>~</mo></mover><mi>k</mi></msub><mo>=</mo><msubsup><mi>V</mi><mi>k</mi><mi>INS</mi></msubsup><mo>-</mo><msub><mrow><mo>(</mo><msup><mi>&delta;V</mi><mi>n</mi></msup><mo>)</mo></mrow><mi>k</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>16</mn><mo>)</mo></mrow></mrow>其中，和分別表示k時刻慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)矩陣，是已知量；I3×3表示3階單位陣；表示由構(gòu)成的斜負(fù)對稱陣；步驟六、對步驟一中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型一步轉(zhuǎn)移矩陣Fk1進(jìn)行更新，同時將k+1的值賦給k，然后返回到步驟二。FSA00000190917000012.tif,FSA00000190917000013.tif,FSA00000190917000014.tif,FSA00000190917000015.tif,FSA00000190917000017.tif,FSA00000190917000018.tif,FSA00000190917000021.tif,FSA00000190917000022.tif,FSA00000190917000024.tif,FSA00000190917000025.tif,FSA00000190917000026.tif,FSA00000190917000027.tif,FSA00000190917000029.tif,FSA000001909170000211.tif,FSA000001909170000212.tif,FSA000001909170000213.tif,FSA000001909170000214.tif,FSA000001909170000215.tif,FSA00000190917000031.tif,FSA00000190917000032.tif,FSA00000190917000033.tif,FSA00000190917000034.tif,FSA00000190917000035.tif,FSA00000190917000036.tif,FSA00000190917000037.tif,FSA00000190917000039.tif,FSA000001909170000310.tif,FSA000001909170000311.tif,FSA000001909170000312.tif,FSA000001909170000313.tif,FSA000001909170000314.tif,FSA000001909170000316.tif,FSA000001909170000317.tif,FSA000001909170000318.tif,FSA000001909170000319.tif,FSA00000190917000041.tif,FSA00000190917000042.tif,FSA00000190917000043.tif,FSA00000190917000044.tif,FSA00000190917000045.tif,FSA000001909170000410.tif,FSA000001909170000411.tif,FSA000001909170000412.tif,FSA000001909170000413.tif,FSA000001909170000414.tif,FSA000001909170000415.tif,FSA000001909170000416.tif,FSA000001909170000417.tif,FSA000001909170000418.tif,FSA000001909170000419.tif,FSA000001909170000420.tif,FSA000001909170000421.tif,FSA000001909170000422.tif,FSA000001909170000423.tif,FSA000001909170000424.tif,FSA00000190917000053.tif,FSA00000190917000054.tif,FSA00000190917000055.tif,FSA00000190917000056.tif,FSA00000190917000057.tif1.一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，其特征在于包括如下步驟步驟一、建立包括位置誤差、速度誤差、失準(zhǔn)角和慣性器件誤差漂移的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型；在東北天坐標(biāo)系下，動態(tài)誤差模型表示如公式1所示；x(t)=F(t)x(t)+w(t)(1)式中，t為時間值，是正實數(shù)；功)=[評TSV"T(pJe/V/yqa)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量，由位置誤差SP、速度誤差、失準(zhǔn)角9、陀螺儀零偏5g和加速度計零偏▽組成；w(t)表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲；F(t)為轉(zhuǎn)移矩陣表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量的變化量；對該動態(tài)誤差模型進(jìn)行離散化處理，可得xk=Fk-iXk-i+wk(2)式中，k表示時間值，為正整數(shù)；Fh表示離散化的慣性導(dǎo)航系統(tǒng)的一步轉(zhuǎn)移矩陣，xk為離散化后的k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)向量，wk表示k時刻的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的系統(tǒng)噪聲，是均值為零，方差為Q的白噪聲序列，Q值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Q為正實數(shù)；步驟二、建立步驟一所述的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程，如公式3所示；Yk=VlNS一VkGPS=HkXk+rjk(3)式中，Yk表示k時刻的觀測量；K/w表示慣性導(dǎo)航系統(tǒng)的速度輸出；表示經(jīng)桿臂補償后GPS的速度輸出；Hk為k時刻的觀測矩陣，Hk=;03X3表示3階0矩眸.t豐宗qRfr^/ffrE^n豐宗■^千Qm的n妬眸.n豐宗t時刻的慣性導(dǎo)航系統(tǒng)云力態(tài)序列，0值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，2為正實數(shù)；對公式4進(jìn)行離散化，離散化后的系統(tǒng)方程表示為式中，vk表示k時刻水平合速度，ak和&分別表示Gh表示離散化的加加速度跟蹤模型的一步轉(zhuǎn)移矩陣，系統(tǒng)噪聲；誤差模型的觀測方程噪聲；步驟三、對GPS輸出進(jìn)行實時野值檢測；通過GPS測量得到的當(dāng)前時刻載車東向速度和北向速度力并以此作為觀測量，建立加加速度跟蹤模型，如公式4所示；式中，t為時間值，為正實數(shù)；v(t)表示水平合速度，a(t)和WO加加速度跟蹤模型的觀測方程表示為式中，zk表示k時刻水平合速度的觀測量，其計算公式為:，ξk表示加加速度跟蹤模型的觀測方程噪聲，是均值為零，方差為及的白噪聲序列，及值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，及為正實數(shù)；根據(jù)公式5和公式6，采用卡爾曼濾波器即可估計出k時刻水平合速度vk、相應(yīng)的加速度ak和加加速度和&；根據(jù)載車的類型和對準(zhǔn)動態(tài)情況，預(yù)先設(shè)定閾值J_，如果<成立，則判斷當(dāng)前的觀測量為有效值，執(zhí)行步驟四；否則，判斷其為野值，在該時刻做卡爾曼平滑，同時將k+1的值賦給k，然后重復(fù)步驟三；步驟四、對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程噪聲nk的均值Mf和方差Cf進(jìn)行估根據(jù)步驟二中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的觀測方程得到的k時刻的觀測量Yk以及觀測矩陣Hk，采用主、從兩個卡爾曼濾波器分別對k時刻慣性導(dǎo)航系統(tǒng)的狀態(tài)xk、k時刻的觀測噪聲nk的均值Mf和方差Cf進(jìn)行估計；具體為Mf和Cf的狀態(tài)方程為式中，表示主對角線元素；該狀態(tài)方程的系統(tǒng)噪聲是均值為零，方差為Qk的白噪聲序列，Qe值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，Qk為正實數(shù)-M為mi行1列的向量山為mi行mi列的單位陣；diag(CAR)為m2行1列的向量；12為m2行m2列的單位陣；公式7對應(yīng)的觀測方程為式中，Ck表示殘差序列的方差，滿足GiJPPk^的初始值分別為h和P。，均為人為設(shè)定值；此后，h和Pkh由主卡爾曼濾波器提供；公式8可進(jìn)一步整理為式中，量測噪聲是均值為零，方差為RK的白噪聲序列，RK值根據(jù)實際應(yīng)用環(huán)境人為設(shè)定，RK為正實數(shù)；根據(jù)公式7和公式9，采用從卡爾曼濾波器即可估計出k時刻的觀測噪聲nk的均值Mf和方差C；從卡爾曼濾波器為經(jīng)典卡爾曼濾波器；步驟五、對慣性導(dǎo)航系統(tǒng)的狀態(tài)xk進(jìn)行估計；根據(jù)步驟一建立的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型及步驟二中的觀測方程，結(jié)合步驟四給出的觀測噪聲nk的均值Mf和方差cf的估計值，采用主卡爾曼濾波器對慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量Xk進(jìn)行估計；具體為主卡爾曼濾波器為經(jīng)典卡爾曼濾波器的改進(jìn)，計算過程如公式1014所示其中，如w表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量xk的一步預(yù)測；Pk^表示一步預(yù)測方差；么表示慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型的狀態(tài)向量xk的估計值；Pk表示估計方差；Kk表示濾波增益；if⑴表示步驟四中M，的估計值；#(2)表示Cf的估計值，其主對角線元素等于步驟四中diag(Cf)的估計值，非主對角線的元素為0；經(jīng)過上述步驟即可得到k時刻慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型狀態(tài)向量xk的估計值jb，包含位置誤差(SP)k，速度誤差(SVn)k和失準(zhǔn)角辦；利用這些誤差估計結(jié)果對慣性導(dǎo)航系統(tǒng)的位置輸出、速度輸出F/MS'和姿態(tài)矩陣(CO輸出進(jìn)行校正，通過公式1517即可得到修正后的載車的位置月、速度巧、姿態(tài)矩陣(Q>；其中，Pk'NS、「廣和(C》分別表示k時刻慣性導(dǎo)航系統(tǒng)輸出的位置、速度和姿態(tài)矩陣，是已知量；I3X3表示3階單位陣；辦x表示由辦構(gòu)成的斜負(fù)對稱陣；步驟六、對步驟一中的慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型一步轉(zhuǎn)移矩陣Fh進(jìn)行更新，同時將k+1的值賦給k，然后返回到步驟二。2.如權(quán)利要求1所述的一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，其特征在于步驟一中所述的動態(tài)誤差模型為角誤差方程或V角誤差方程。全文摘要本發(fā)明涉及一種實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)運動對準(zhǔn)的方法，屬于慣性導(dǎo)航系統(tǒng)初始對準(zhǔn)
技術(shù)領(lǐng)域：
。本發(fā)明在慣性導(dǎo)航系統(tǒng)動態(tài)誤差模型和觀測方程的基礎(chǔ)上，以GPS輸出速度作為觀測量，采用卡爾曼濾波對載車的加加速度進(jìn)行估計，并判別GPS速度輸出是否為野值。若GPS速度輸出為野值，采用卡爾曼濾波平滑方法對當(dāng)前狀態(tài)進(jìn)行遞推；否則，采用主、從卡爾曼濾波器來實現(xiàn)慣性導(dǎo)航系統(tǒng)狀態(tài)和噪聲統(tǒng)計特性的同步估計，主濾波器對慣性導(dǎo)航系統(tǒng)的狀態(tài)進(jìn)行估計，其新息和方差作為從濾波器的觀測量；從濾波器對主濾波器觀測噪聲的均值和方差進(jìn)行估計，估計結(jié)果為下一次主濾波器的濾波提供噪聲特性輸入；從而實現(xiàn)陸用慣性導(dǎo)航系統(tǒng)的運動對準(zhǔn)。文檔編號G01C21/16GK101900573SQ201010227110公開日2010年12月1日申請日期2010年7月15日優(yōu)先權(quán)日2010年7月15日發(fā)明者付夢印,林杰,王清哲,肖烜,鄧志紅申請人:北京理工大學(xué)