本發(fā)明屬于測繪領(lǐng)域，特別涉及一種基于慣性導(dǎo)航系統(tǒng)(INS)和全球定位系統(tǒng)(GPS)組合的垂線偏差動(dòng)態(tài)測量方法。

背景技術(shù)：
基于慣性導(dǎo)航系統(tǒng)/全球定位系統(tǒng)組合的矢量重力測量方法是獲取重力場信息的有效手段，它具有高精度、高分辨率、測量效率高等特點(diǎn)，廣泛應(yīng)用于海洋、航空重力測量中。重力矢量垂直分量的測量目前已較為成熟，并廣泛應(yīng)用于商業(yè)測量中，然而對重力矢量水平分量(即垂線偏差)的測量仍然是一個(gè)難以解決的技術(shù)問題。國防科學(xué)技術(shù)大學(xué)的戰(zhàn)德軍等在申請?zhí)枮椤癈N201310730904.5”的發(fā)明專利申請中(公開號(hào)：CN103674030A)提出了一種“基于天文姿態(tài)基準(zhǔn)保持的垂線偏差測量裝置與方法”，采用的技術(shù)方案如圖1所示，它利用INS/GPS姿態(tài)測量系統(tǒng)中的激光陀螺組合體(LGU)與GPS組合，構(gòu)建了LGU/GPS姿態(tài)測量子系統(tǒng)；首先啟動(dòng)INS/GPS姿態(tài)測量系統(tǒng)，進(jìn)行初始對準(zhǔn)并輸出系統(tǒng)姿態(tài)；然后啟動(dòng)星敏感器并利用其姿態(tài)輸出對LGU/GPS姿態(tài)測量子系統(tǒng)進(jìn)行初始化，此后LGU/GPS姿態(tài)測量子系統(tǒng)自主實(shí)現(xiàn)姿態(tài)測量；利用LGU/GPS姿態(tài)測量子系統(tǒng)輸出的姿態(tài)和INS/GPS姿態(tài)測量系統(tǒng)輸出的姿態(tài)求差，進(jìn)而計(jì)算垂線偏差；最后修正垂線偏差測量值中的跳變誤差，并利用全球重力模型數(shù)據(jù)修正垂線偏差測量值中的低頻誤差。該方法能夠?qū)崿F(xiàn)垂線偏差的動(dòng)態(tài)測量，并具有魯棒性強(qiáng)，能夠有效抑制慣性器件誤差，不依賴于差分GPS，應(yīng)用范圍廣等優(yōu)點(diǎn)。然而，上述測量方法存在一定的局限性，主要表現(xiàn)在如下三個(gè)方面：一是該測量方法需要由星敏感器提供高精度天文姿態(tài)基準(zhǔn)，大大增加了測量設(shè)備的成本和結(jié)構(gòu)的復(fù)雜度；二是星敏感器的使用依賴于天氣條件，且只能在夜晚使用，因而嚴(yán)重地限制了測量實(shí)施的靈活性；二是該發(fā)明所提出的垂線偏差低頻誤差補(bǔ)償算法只是一種工程上的經(jīng)驗(yàn)處理方法，其算法不是最優(yōu)的，因此補(bǔ)償精度不高，可靠性也較差。如何在動(dòng)態(tài)條件下實(shí)現(xiàn)垂線偏差測量，抑制測量誤差，提高測量的精度和可靠性，同時(shí)減小甚至消除測量設(shè)備對環(huán)境的依賴，是本領(lǐng)域技術(shù)人員極為關(guān)注的技術(shù)問題。

技術(shù)實(shí)現(xiàn)要素：
本發(fā)明是對發(fā)明“CN201310730904.5”的一種改進(jìn)，要解決的技術(shù)問題是提出一種在不依賴于天文姿態(tài)基準(zhǔn)的條件下，利用INS和GPS組合實(shí)現(xiàn)垂線偏差動(dòng)態(tài)測量的裝置及方法，提高測量的精度和可靠性，簡化測量系統(tǒng)的復(fù)雜度，且不依賴于測量環(huán)境，便于實(shí)施。為解決上述技術(shù)問題，本發(fā)明所采用的技術(shù)方案是：一種基于INS和GPS組合的垂線偏差動(dòng)態(tài)測量裝置，由慣性導(dǎo)航系統(tǒng)(INS)、GPS天線、GPS接收機(jī)和數(shù)據(jù)處理計(jì)算機(jī)組成，所述INS、GPS天線以及GPS接收機(jī)組成INS/GPS姿態(tài)測量子系統(tǒng)(以下簡稱INS/GPS)，所述INS包含了三個(gè)正交安裝的激光陀螺，稱為激光陀螺組合體(LGU)，所述LGU、GPS天線以及GPS接收機(jī)組成LGU/GPS姿態(tài)測量子系統(tǒng)(以下簡稱LGU/GPS，關(guān)于LGU/GPS參見發(fā)明CN201310730904.5中的描述)，三個(gè)激光陀螺均與GPS接收機(jī)通信；所述INS、GPS天線、GPS接收機(jī)固聯(lián)安裝于測量載體上，測量載體可以是測量船、測量車等運(yùn)載工具；所述GPS天線與所述GPS接收機(jī)通信，所述INS、GPS接收機(jī)通過數(shù)據(jù)線與數(shù)據(jù)處理計(jì)算機(jī)連接，INS和GPS接收機(jī)的測量數(shù)據(jù)通過數(shù)據(jù)線傳輸?shù)綌?shù)據(jù)處理計(jì)算機(jī)中，在數(shù)據(jù)處理計(jì)算機(jī)中完成垂線偏差的解算。INS采用龍興武等在2010年《中國慣性技術(shù)學(xué)報(bào)》第2期“激光陀螺單軸旋轉(zhuǎn)慣性導(dǎo)航系統(tǒng)”論文中公開的單軸旋轉(zhuǎn)式結(jié)構(gòu)。本發(fā)明還提供了一種利用上述裝置動(dòng)態(tài)測量垂線偏差的方法，具體包括以下步驟：1、構(gòu)建INS/GPS姿態(tài)測量子系統(tǒng)，并進(jìn)行8小時(shí)以上對準(zhǔn)。由INS、GPS天線和GPS接收機(jī)構(gòu)建INS/GPS姿態(tài)測量子系統(tǒng)，啟動(dòng)INS/GPS姿態(tài)測量子系統(tǒng)，進(jìn)行8小時(shí)以上對準(zhǔn)，在對準(zhǔn)過程中INS/GPS姿態(tài)測量子系統(tǒng)采用專利“CN201310730904.5”實(shí)施步驟1中所述的算法實(shí)現(xiàn)組合姿態(tài)測量，在整個(gè)測量過程中INS/GPS姿態(tài)測量子系統(tǒng)連續(xù)輸出INS坐標(biāo)系(b系)相對于計(jì)算導(dǎo)航坐標(biāo)系(n'系)的姿態(tài)矩陣對準(zhǔn)過程中無垂線偏差數(shù)據(jù)輸出。2、INS/GPS對準(zhǔn)結(jié)束后，啟動(dòng)LGU/GPS姿態(tài)測量子系統(tǒng)，利用第一步中INS/GPS對準(zhǔn)后輸出的姿態(tài)矩陣對LGU/GPS進(jìn)行姿態(tài)矩陣的初始化。記ti為第i個(gè)測量采樣點(diǎn)對應(yīng)的時(shí)刻，1≤i≤N，N為整個(gè)測量過程的采樣點(diǎn)總數(shù)，ti時(shí)刻INS/GPS輸出的姿態(tài)矩陣為LGU/GPS輸出的姿態(tài)矩陣為上標(biāo)n表示真實(shí)導(dǎo)航坐標(biāo)系(n系)。啟動(dòng)LGU/GPS姿態(tài)測量子系統(tǒng)的時(shí)刻記為t0，即i＝0，則LGU/GPS姿態(tài)矩陣的初始化方法為：令3、LGU/GPS姿態(tài)測量子系統(tǒng)進(jìn)行姿態(tài)更新。LGU/GPS姿態(tài)測量子系統(tǒng)采用專利“CN201310730904.5”中步驟5.3所述的方法進(jìn)行姿態(tài)更新，同時(shí)數(shù)據(jù)處理計(jì)算機(jī)存儲(chǔ)全部測量采樣時(shí)刻INS/GPS輸出的姿態(tài)矩陣和LGU/GPS輸出的姿態(tài)矩陣4、利用全部測量過程中ti時(shí)刻INS/GPS輸出的姿態(tài)矩陣和LGU/GPS輸出的姿態(tài)矩陣計(jì)算姿態(tài)矩陣并計(jì)算其輸出的相應(yīng)的姿態(tài)角之差。的計(jì)算方法為：其中[]T表示矩陣的轉(zhuǎn)置，符號(hào)·表示矩陣乘法。為了方便描述，將任意t時(shí)刻計(jì)算得到的n′系到n系的姿態(tài)矩陣簡記為任意t時(shí)刻INS/GPS與LGU/GPS輸出的三個(gè)姿態(tài)角之差為ΔΦE，ΔΦN，ΔΦU，其中，下標(biāo)E、N、U分別表示東向、北向、天向分量。ΔΦE、ΔΦN和ΔΦU由式(2)計(jì)算得到：其中表示矩陣的第2行，第3列的元素，表示矩陣的第3行，第1列的元素，表示矩陣的第1行，第2列的元素。5、以INS/GPS與LGU/GPS輸出的三個(gè)姿態(tài)角之差ΔΦE，ΔΦN，ΔΦU為觀測量，通過建立垂線偏差測量的觀測方程和狀態(tài)方程，在全球重力模型的輔助下，利用Kalman濾波的方法提取垂線偏差。具體的實(shí)施方法如下：記t時(shí)刻測量載體所在位置真實(shí)的東向垂線偏差為η，北向垂線偏差為ξ，如圖3所示，由全球重力模型計(jì)算得到的東向和北向垂線偏差值分別記為選用的全球重力模型為EGM2008全球重力模型，該全球重力模型的計(jì)算程序和使用方法可以從“http://earth-info.nga.mil/GandG/wgs84/gravitymod/index.html”網(wǎng)站獲取。由EGM2008全球重力模型計(jì)算得到的東向和北向垂線偏差誤差分別記為δη和δξ，并存在式(3)的關(guān)系：記INS/GPS輸出的相對于n′系的姿態(tài)誤差角為vE，vN，vU，LGU/GPS輸出的相對于n系的姿態(tài)誤差角為φE、φN、φU，下標(biāo)E、N、U分別表示東向、北向、天向分量。5.1建立垂線偏差測量的狀態(tài)方程。選取垂線偏差測量系統(tǒng)的狀態(tài)變量為φE、φN、φU、δη、δξ、εU，其中εU為激光陀螺的等效天向零偏，分別對上述狀態(tài)變量進(jìn)行動(dòng)態(tài)建模，φE、φN、φU滿足如下微分方程：其中，ωie為地球自轉(zhuǎn)角速度，L為測量點(diǎn)的地理緯度。εU建模為隨機(jī)常值模型，則有：EGM2008的東向和北向垂線偏差誤差δη、δξ的統(tǒng)計(jì)模型分別由(6)式、(7)式給出：其中xE、xN為中間狀態(tài)變量；ω0為該統(tǒng)計(jì)模型的固有頻率，ω0與測量載體的運(yùn)動(dòng)速度V之間存在固定關(guān)系ω0＝2π×V/10000；ζ為該統(tǒng)計(jì)模型的阻尼系數(shù)，wE和wN分別為東向和北向垂線偏差統(tǒng)計(jì)模型的過程噪聲，wE～(0,qE)，wN～(0,qN)，即wE和wN分別服從均值為0的高斯分布，其方差分別為qE和qN。選擇垂線偏差測量系統(tǒng)的狀態(tài)空間矢量為：x＝[φE,φN,φU,εU,xE,xN,δη,δξ]T(8)將式(4)～(7)寫為統(tǒng)一的狀態(tài)方程的形式為：其中，矩陣w＝[0,0,0,0,0,0,wE,wN]T。式(9)即為垂線偏差測量系統(tǒng)的狀態(tài)方程。過程噪聲w～(0,Q)，即w服從均值為0，方差為Q的高斯分布。其中O6×6、O2×6、O6×2分別表示6行6列、2行6列、6行2列的零矩陣，diag[qE,qN]表示以qE和qN為對角元素的對角矩陣。將狀態(tài)方程(9)離散化得到：xi＝Mi/i-1·xi-1+wi(10)xi表示x在ti時(shí)刻的采樣值,Mi/i-1為ti-1到ti時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，Mi/i-1≈I8×8+Δt·Fi，其中I8×8為8維的單位矩陣，F(xiàn)i為矩陣F在ti時(shí)刻的采樣值，Δt為采樣間隔。wi服從均值為0，方差為Qi的高斯分布，Qi＝Δt2·Q。5.2建立垂線偏差測量的觀測方程。以INS/GPS和LGU/GPS輸出的姿態(tài)角之差ΔΦE，ΔΦN為觀測量，建立如下觀測方程：將式(3)代入式(11)，整理后得到：選取新的觀測矢量為：將式(12)重新寫為：y＝H·x+v(14)其中矩陣v＝[vE,vN]T。式(14)即為垂線偏差測量系統(tǒng)的觀測方程。INS/GPS的姿態(tài)誤差角vE、vN分別為東向和北向觀測分量的觀測噪聲，vE～(0,rE)，vN～(0,rN)，即vE和vN分別服從均值為0的高斯分布，方差分別為rE和rN。觀測噪聲v～(0,R)，即v服從均值為0，方差為R的高斯分布，觀測噪聲方差矩陣將觀測方程離散化得到：yi＝Hi·xi+vi(15)yi、vi、Hi分別為y、v、H在ti時(shí)刻的采樣值，vi服從均值為0，方差為Ri的高斯分布，Ri＝Δt2·R，Hi＝H。5.3根據(jù)式(10)和(15)所描述的垂線偏差測量系統(tǒng)離散形式的狀態(tài)方程和觀測方程，利用Kalman濾波算法對狀態(tài)矢量進(jìn)行估計(jì)。Kalman濾波算法的迭代算法如下：其中為xi的估計(jì)值，為xi的一步預(yù)測值，Pi為xi的估計(jì)協(xié)方差矩陣，Pi/i-1為Pi的一步預(yù)測值，Ki為濾波增益，[]-1符號(hào)表示矩陣的求逆運(yùn)算。ti時(shí)刻?hào)|向和北向垂線偏差誤差δη、δξ的估計(jì)值分別為由ti時(shí)刻的狀態(tài)矢量估計(jì)值給出：其中，分別表示狀態(tài)矢量估計(jì)值的第7和第8個(gè)元素。5.4計(jì)算垂線偏差測量值。ti時(shí)刻?hào)|向和北向垂線偏差測量值ηi、ξi分別由式(18)、(19)計(jì)算得到：其中分別為全球重力模型計(jì)算得到的ti時(shí)刻測量點(diǎn)上東向和北向垂線偏差的值。本發(fā)明具有以下技術(shù)效果：1.利用INS/GPS輸出的姿態(tài)矩陣對LGU/GPS進(jìn)行姿態(tài)初始化，從而消除了對星敏感器的依賴，有利于降低成本，減小系統(tǒng)復(fù)雜度，同時(shí)也消除了測量環(huán)境和測量時(shí)間的限制。2.建立了垂線偏差測量系統(tǒng)的狀態(tài)方程和觀測方程，在全球重力模型的輔助下，通過Kalman濾波的方法實(shí)現(xiàn)垂線偏差的最優(yōu)估計(jì)，比發(fā)明“CN201310730904.5”所述的方法更有效地分離了測量結(jié)果中存在的低頻誤差，提高了測量方法的精度和可靠性。3.本發(fā)明相對于發(fā)明“CN201310730904.5”，只需要對LGU/GPS進(jìn)行一次姿態(tài)初始化，簡化了操作，提高了測量效率。附圖說明圖1是中國專利申請CN201310730904.5公布的垂線偏差動(dòng)態(tài)測量裝置結(jié)構(gòu)示意圖；圖中：1.INS，2.GPS天線，3.GPS接收機(jī)，4.星敏感器，5.測量載體，6.數(shù)據(jù)處理計(jì)算機(jī)圖2本發(fā)明垂線偏差動(dòng)態(tài)測量裝置結(jié)構(gòu)示意圖；圖中：1.INS，2.GPS天線，3.GPS接收機(jī)，4.數(shù)據(jù)處理計(jì)算機(jī)，5.測量載體圖3是導(dǎo)航坐標(biāo)系與垂線偏差的關(guān)系；圖4是本發(fā)明的垂線偏差動(dòng)態(tài)測量方法流程圖。具體實(shí)施方式下面將結(jié)合附圖和實(shí)施例對本發(fā)明作進(jìn)一步的詳細(xì)說明。本發(fā)明采用的垂線偏差動(dòng)態(tài)測量裝置如圖2所示，由INS1、GPS天線2、GPS接收機(jī)3、數(shù)據(jù)處理計(jì)算機(jī)4和測量載體5構(gòu)成。其中INS1與GPS天線2分別固聯(lián)安裝于測量載體5上，測量載體可以是測量船、測量車等運(yùn)載工具。INS1、GPS接收機(jī)3的測量數(shù)據(jù)通過數(shù)據(jù)線傳輸?shù)綌?shù)據(jù)處理計(jì)算機(jī)4中，在數(shù)據(jù)處理計(jì)算機(jī)4中完成垂線偏差的解算。INS采用龍興武等在2010年《中國慣性技術(shù)學(xué)報(bào)》第2期“激光陀螺單軸旋轉(zhuǎn)慣性導(dǎo)航系統(tǒng)”論文中公開的單軸旋轉(zhuǎn)式結(jié)構(gòu)。如圖3所示，定義INS的坐標(biāo)系為b系Ob-xbybzb，定義東-北-天坐標(biāo)系O-xyz為真實(shí)導(dǎo)航坐標(biāo)系，即n系，其中O-xy平面與地球的參考橢球面平行。定義計(jì)算導(dǎo)航坐標(biāo)系為O'-x'y'z'，即n'系，計(jì)算導(dǎo)航坐標(biāo)系的定義可參考“高鐘毓著、清華大學(xué)出版社2012年出版的《慣性導(dǎo)航系統(tǒng)技術(shù)》”第225～226頁的描述。由于垂線偏差的存在，通過慣性導(dǎo)航解算得到的計(jì)算導(dǎo)航坐標(biāo)系n'系與真實(shí)的導(dǎo)航坐標(biāo)系n系存在偏差。圖4為本發(fā)明測量方法總體流程圖，本發(fā)明測量方法具體實(shí)施步驟如下：步驟1：構(gòu)建INS/GPS組合姿態(tài)測量系統(tǒng)，并進(jìn)行8小時(shí)以上對準(zhǔn)。由圖2裝置中的INS1、GPS天線2和GPS接收機(jī)3構(gòu)建INS/GPS姿態(tài)測量系統(tǒng)，首先啟動(dòng)INS/GPS姿態(tài)測量子系統(tǒng)，進(jìn)行8小時(shí)以上對準(zhǔn)。INS/GPS組合采用專利“CN201310730904.5”實(shí)施步驟1中所述的算法實(shí)現(xiàn)姿態(tài)測量。在整個(gè)測量過程中INS/GPS姿態(tài)測量子系統(tǒng)連續(xù)輸出INS坐標(biāo)系(b系)相對于計(jì)算導(dǎo)航坐標(biāo)系(n'系)的姿態(tài)矩陣對準(zhǔn)過程中無垂線偏差數(shù)據(jù)輸出。步驟2：INS/GPS對準(zhǔn)結(jié)束后，啟動(dòng)LGU/GPS姿態(tài)測量子系統(tǒng)，利用第一步中INS/GPS輸出的姿態(tài)矩陣對LGU/GPS進(jìn)行姿態(tài)矩陣的初始化。記ti為第i個(gè)測量采樣點(diǎn)對應(yīng)的時(shí)刻，1≤i≤N，N為整個(gè)測量過程的采樣點(diǎn)總數(shù)，ti時(shí)刻INS/GPS輸出的姿態(tài)矩陣為LGU/GPS輸出的姿態(tài)矩陣為上標(biāo)n表示真實(shí)導(dǎo)航坐標(biāo)系(n系)。啟動(dòng)LGU/GPS姿態(tài)測量子系統(tǒng)的時(shí)刻記為t0，則LGU/GPS姿態(tài)矩陣的初始化方法為：令步驟3：LGU/GPS姿態(tài)測量子系統(tǒng)進(jìn)行姿態(tài)更新。LGU/GPS姿態(tài)測量子系統(tǒng)采用專利“CN201310730904.5”中步驟5.3所述的方法進(jìn)行姿態(tài)更新，同時(shí)數(shù)據(jù)處理計(jì)算機(jī)存儲(chǔ)全部測量采樣時(shí)刻INS/GPS輸出的姿態(tài)矩陣和LGU/GPS輸出的姿態(tài)矩陣步驟4：利用全部測量過程中ti時(shí)刻INS/GPS輸出的姿態(tài)矩陣和LGU/GPS輸出的姿態(tài)矩陣計(jì)算姿態(tài)矩陣并計(jì)算其輸出的相應(yīng)的姿態(tài)角之差。的計(jì)算方法為：其中[]T表示矩陣的轉(zhuǎn)置，符號(hào)·表示矩陣乘法。將任意t時(shí)刻計(jì)算得到的n′系到n系的姿態(tài)矩陣簡記為任意t時(shí)刻INS/GPS與LGU/GPS輸出的三個(gè)姿態(tài)角之差為：ΔΦE，ΔΦN，ΔΦU，其中，下標(biāo)E、N、U分別表示東向、北向、天向分量。ΔΦE、ΔΦN和ΔΦU由式(2)計(jì)算得到：其中表示矩陣的第2行，第3列的元素，表示矩陣的第3行，第1列的元素，表示矩陣的第1行，第2列的元素。步驟5：建立垂線偏差測量的觀測方程和狀態(tài)方程，在全球重力模型的輔助下，利用Kalman濾波的方法提取垂線偏差。具體的實(shí)施方法如下：記t測量時(shí)刻載體所在位置真實(shí)的東向垂線偏差為η，北向垂線偏差為ξ，由全球重力模型計(jì)算得到的東向和北向的垂線偏差值分別記為全球重力模型選用EGM2008全球重力模型，該全球重力模型的計(jì)算程序和使用方法可以從“http://earth-info.nga.mil/GandG/wgs84/gravitymod/index.html”網(wǎng)站獲取。由EGM2008全球重力模型計(jì)算得到的東向和北向垂線偏差誤差分別記為δη和δξ，并滿足式(3)的關(guān)系：記INS/GPS輸出的相對于n′系的姿態(tài)誤差角為vE，vN，vU，LGU/GPS輸出的相對于n系的姿態(tài)誤差角為φE、φN、φU，下標(biāo)E、N、U分別表示東向、北向、天向分量。步驟5.1：建立垂線偏差測量的狀態(tài)方程。選取垂線偏差測量系統(tǒng)的狀態(tài)變量為φE、φN、φU、δη、δξ、εU，其中εU為激光陀螺的等效天向零偏，分別對狀態(tài)量進(jìn)行動(dòng)態(tài)建模，φE、φN、φU滿足如下微分方程：其中，ωie為地球自轉(zhuǎn)角速度，L為測量點(diǎn)的地理緯度。εU建模為隨機(jī)常值模型，則有：EGM2008的東向和北向垂線偏差誤差δη、δξ的統(tǒng)計(jì)模型分別滿足如下微分方程：其中xE、xN為中間狀態(tài)量；ω0為該統(tǒng)計(jì)模型的固有頻率，ω0與載體的運(yùn)動(dòng)速度V之間存在固定關(guān)系ω0＝2π×V/10000；ζ為該統(tǒng)計(jì)模型的阻尼系數(shù)，可設(shè)置為ζ＝0.05，wE和wN分別為東向和北向垂線偏差統(tǒng)計(jì)模型的過程噪聲，wE～(0,qE)，wN～(0,qN)，即wE和wN分別服從均值為0的高斯分布，方差分別為qE和qN，其典型值可設(shè)置為qE＝(3″)2×(4ζω0)，qN＝(3″)2×(4ζω0)。選擇狀態(tài)空間矢量為：x＝[φE,φN,φU,εU,xE,xN,δη,δξ]T(8)將式(4)-(7)寫為統(tǒng)一的狀態(tài)方程的形式為：其中，矩陣w＝[0,0,0,0,0,0,wE,wN]T。式(9)即為垂線偏差測量系統(tǒng)的狀態(tài)方程。過程噪聲w～(0,Q)，即w服從均值為0，方差為Q的高斯分布。其中O6×6、O2×6、O6×2分別表示6行6列、2行6列、6行2列的零矩陣，diag(qE,qN)表示以qE和qN為對角元素的對角矩陣。將狀態(tài)方程離散化得到：xi＝Mi/i-1·xi-1+wi(10)xi表示x在ti時(shí)刻的采樣值,Mi/i-1為ti-1到ti時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，且Mi/i-1≈I8×8+Δt·Fi，其中I8×8為8維的單位矩陣，F(xiàn)i為矩陣F在ti時(shí)刻的采樣值，Δt為采樣間隔。wi服從均值為0，方差為Qi的高斯分布，Qi＝Δt2·Q。步驟5.2：建立垂線偏差測量的觀測方程。以INS/GPS和LGU/GPS輸出的姿態(tài)角之差為觀測量，建立如下觀測方程：將式(3)代如式(11)，整理后得到：選取新的觀測矢量為：將式(12)重新寫為：y＝H·x+v(14)其中矩陣v＝[vE,vN]T。式(14)即為垂線偏差測量系統(tǒng)的觀測方程。INS/GPS的姿態(tài)誤差角vE、vN分別為東向和北向觀測分量的觀測噪聲，vE～(0,rE)，vN～(0,rN)，即vE和vN分別服從均值為0的高斯分布，方差分別為rE和rN。觀測噪聲矢量v～(0,R)，即v服從均值為0，方差為R的高斯分布，觀測噪聲方差矩陣rE和rN的典型值可設(shè)置為0.5″。將觀測方程離散化得到：yi＝Hi·xi+vi(15)yi、vi、Hi分別為y、v、H在ti時(shí)刻的采樣值，vi服從均值為0，方差為Ri的高斯分布，Ri＝Δt2·R，Hi＝H。步驟5.3：根據(jù)式(10)、(15)所描述的垂線偏差測量系統(tǒng)離散形式的狀態(tài)方程和觀測方程，利用Kalman濾波算法對狀態(tài)矢量進(jìn)行估計(jì)。首先設(shè)置狀態(tài)矢量的初值和估計(jì)噪聲協(xié)方差矩陣P0的初值?？梢栽O(shè)置為8維零矢量；P0設(shè)置為對角矩陣，可設(shè)置為典型值：P0＝diag((2″)2，(2″)2，(5″)2，(0.003″)2，(1″)2，(1″)2，(1″)2，(1″)2)。再由式(16)中的Kalman濾波迭代算法進(jìn)行狀態(tài)矢量和協(xié)方差矩陣估計(jì)值的更新。其中為xi的估計(jì)值，為xi的一步預(yù)測值，Pi為xi的估計(jì)協(xié)方差矩陣，Pi/i-1為Pi的一步預(yù)測值，Ki為濾波增益，[]-1符號(hào)表示矩陣的求逆運(yùn)算。ti時(shí)刻?hào)|向和北向垂線偏差誤差δη、δξ的估計(jì)值分別為由ti時(shí)刻的狀態(tài)矢量估計(jì)值給出：步驟5.4：計(jì)算垂線偏差測量值：ti時(shí)刻?hào)|向和北向垂線偏差測量值ηi、ξi分別由式(18)、(19)計(jì)算得到：其中分別為全球重力模型計(jì)算得到的東向和北向垂線偏差的值。