步驟5 :聯立求解（3)式中的S個微分方程，得到罐籠速度信息V，利用數值積分法 直接對速度積分計算罐籠位置： 陽 106]
(.4》
[0107] 式中：L、E、h分別表示罐籠東、北、天方向位移，L。、E。、h。為相應初始值，T為慣性 單元采樣周期。
[0108] 本發明所述系統采用聯合卡爾曼濾波算法進行多傳感器信息融合校正，信息融合 濾波器結構如圖3所示，由一個全局濾波器和S個子濾波器組成。SINS與旋轉編碼器構成 速度子濾波器1，進行垂直速度融合校正；SINS與接近開關構成位置子濾波器2,進行位置 融合校正；罐籠垂直運動，由柔性鋼絲繩約束其水平運動，可認為水平面兩個方向位置和速 度為零，SINS與約束修正構成約束子濾波器3,進行動態零速、零位修正；約束子濾波器3的 速度和位置觀測量與其他濾波器觀測量更新頻率不同，因此不與其他濾波器合并。 陽109] 所述信息融合的步驟如下：
[0110] 步驟1 :建立捷聯慣性導航系統誤差模型，包括=軸巧螺儀誤差模型、加速度測量 誤差模型、位置誤差模型及姿態角誤差模型 陽111] 步驟2 步驟1中的捷聯慣性導航系統誤差模型建立離散化的組合導航式位姿 測量系統的狀態方程
[0112] 所述組合導航式位姿測量系統是指由捷聯慣性導航系統、接近開關和旋轉編碼器 組成；
[0113] 步驟3:構建信息融合卡爾曼濾波器，并依據組合導航式位姿測量系統的狀態向 量建立信息融合卡爾曼濾波器中各子濾波器的觀測方程，求得各子濾波器的狀態向量估計 值；
[0114] 所述信息融合卡爾曼濾波器包括第一子濾波器、第二子濾波器、第=子濾波器W 及主濾波器，所述第一子濾波器、第二子濾波器及第=子濾波器均與主濾波器相連；
[0115] 所述第一子濾波器、第二子濾波器、第=子濾波器W及主濾波器均為卡爾曼濾波 器；
[0116] 所述第一子濾波器即為速度子濾波器1、第二子濾波器即為位置子濾波器2、第= 子濾波器即為約束子濾波器3 ;
[0117] 【各濾波器狀態方程與組合導航式位姿測量系統狀態方程結構相同；】
[0118] 將捷聯慣性導航系統的速度與編碼器速度之差作為第一子濾波器的輸入信號；
[0119] 將捷聯慣性導航系統的位置信號與接近開關位置信號之差作為第二子濾波器的 輸入信號；
[0120] 將捷聯慣性導航系統的東、北方向的零速度和零位置信號作為約束條件，構造成 虛擬信號作為第=子濾波器的輸入信號； 陽121] 第一子濾波器的觀測方程為：Zi化）=Hi化)Xi化)+Vi化）
[0122] 第一子濾波器觀測向量Zi化）=[Vu-vJ，Vb為地面處理裝置基于旋轉編碼器計 算得到的罐籠速度，VI化）為第一子濾波器觀測噪聲，是均值為零的高斯白噪聲序列，由高 斯函數可得，Xi(k)為第一子濾波器的狀態向量，第一子濾波器觀測矩陣Hi化）=[0ix8 1 0lX6] 1X15; 陽123] 第二子濾波器的觀測方程為：Z2(k) =H2GOX2化)+V2化）；
[0124] 第二子濾波器觀測向量Z2(k)=出-H,]，H為捷聯慣性導航系統解算出的罐籠垂直 位置，H，為接近開關位置信號，V2(k)為第二子濾波器位置觀測噪聲，是均值為零的高斯白 噪聲序列；X2(k)為第二子濾波器的狀態向量，&化）=[0ix5 1OlxJlXlS為第二子濾波器位 置觀測矩陣； 陽1巧]第S子濾波器觀測方程為：Z3化）=&化)X3化)+V3化） 陽126] 第S子濾波器觀測向量Z3(k) =[LEV。vjT，L和E為捷聯慣性導航系統東向和 西向位置，V。和V。為捷聯慣性導航系統東向和西向速度，V3(k)為第S子濾波器觀測噪聲， 是均值為零的高斯白噪聲序列；X3(k)為第=子濾波器的狀態向量； 陽127] 第S子濾波器觀測矩陣&化）為：
[0128] k表示離散時刻； 陽129] 步驟4 :按照如下規則將組合導航式位姿測量系統整體的過程信息包括Qi似、 夫化)及Pi(k)平均分配到各子濾波器：
陽m] 其中，Qi化)、義腳及Pi似分別為各濾波器的噪聲方差陣、狀態估計向量、估計誤 差協方差，i=l，2,3，m; 陽133] Qg化)、ig(恃、Pg(k)分別為組合導航式位姿測量系統的噪聲方差陣、狀態估計向 量、估計誤差協方差；為分配參數P1化）的倒數；
[0134] =巧作,-1,)化-急丹，E□是期望函數，矣的初始值為[0]ixi5; 陽135]名由步驟3的觀測方程求得，Qi=E[Wi(k)Wi化)t]/T，Wi為各濾波器的噪聲向量； 陽136] 在本實例中，組合導航式位姿測量系統的噪聲方差陣初始值取值如下： 陽137]Qg=diag[(0. 05。A)2,（0. 05°A)2,（0. 05°A)2,（0.orA)2,（0.or/h)2, (0. 01°A) 2]
[0138] 組合導航式位姿測量系統的估計誤差協方差初始值： 陽139]Pg = diag[(0. lm/s)2, (0. lm/s)2, (0. lm/s)2,化002° )2,化004° )2,（1° )2,（1 ° )2,（1° )2,（1° )2,（1° )2,（1° )2,（1° )2,（1° )2,（l0-4g)2,（l0-4g)2]
[0140]速度子濾波器1、位置子濾波器2、約束子濾波器3的觀測噪聲方差陣初始值： 陽1川Ri= diag[(0. 002。）2,（0. 004。）2] 陽1創尺2= diag[ (0. 5m/s) 2, (0. 5m/s)2] 陽14引尺3= diag[ (0. 002。）2, (0. 004。）2, (0. 5m/s) 2, (0. 5m/s) 2]
[0144] 步驟5 :基于步驟4所得各濾波器的噪聲方差強度矩陣、狀態估計向量、估計誤差 協方差即Qi化)、i^i(k)及Pi化)，將各濾波器狀態估計與估計誤差協方差按組合導航式位 姿測量系統的轉移矩陣進行轉移，得到各濾波器轉移后的狀態估計向量和估計誤差協方差
[3] ，
[0145] 步驟6:利用步驟5所得各濾波器轉移后的狀態估計向量與估計誤差協方差，采用 如下測量方程，對.Ki化)、Pi似按W下公式進行更新：
[0146] X：{ky=Xi{k,k-\) +K,{k)(Z；(k}-H,(k)Xi(A,Kl)) 陽 147] 人（k)=R.(A'，/l'-l)H;'(k)(H/kWC/c，/t-l) +A〇t))-i陽 148]Pi似=(I-Ki似Hi(k))Pi(k，k-1) 陽149] 其中，i= 1，2, 3，m，當i=m時，Pm似=Pm(k，k-1)屯為各濾波器的增益矩陣；Ri=E[V 1 (k)Vi(k)T] /T為各濾波器觀測噪聲的協方差矩陣，Vi為各濾波器觀測白噪聲序列， I為15X15的單位矩陣；T為慣性單元采樣周期，Pi化，k-1)為轉移估計誤差協方差陣；
[0150] 步驟7 :將各子濾波器的估計信息按照下式進行融合，得到組合導航式位姿測量 系統的狀態估計與估計誤差協方差全局最優估計Pg，并返回步驟4,依據！g、Pg將組合 導航式位姿測量系統的過程信息對各子濾波器進行重新分配，對各濾波器進行反饋修正：
[0151]
[0152] 在各子濾波器計算得到各自局部估計值后，經過全局濾波器融合，得到位置、速度 全局最優估計，利用全局濾波結果，按照信息守恒原理在各濾波器進行分配，對局部濾波器 進行反饋修正，提高測量精度。
[0153] 組合導航式位姿測量系統硬件結構如圖4所示，采用雙DSP處理器，DSP1進行數 據采集及處理；DSP2進行導航解算和信息融合運算；慣性測量單元（IMU)W500化頻率實 時測量加速度、巧螺儀、溫度信號，通過SPI接口輸入DSP1 ;編碼器和接近開關信號