[發(fā)明專利]一種MEMS/GPS組合導航系統(tǒng)中量測滯后的濾波方法

有效

• 申請?zhí)枺?/td>: 201410083232.8
• 申請日: 2014-03-08
• 公開（公告）號: CN103901459A
• 公開（公告）日: 2014-07-02
• 發(fā)明（設計）人: 周廣濤;姜鑫;孫艷濤;趙博;郝勤順;夏秀瑋;于春陽;趙維珩;李佳璇;林萌萌
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01S19/49
• 分類號: G01S19/49;G01C21/20
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種MEMS/GPS組合導航系統(tǒng)中量測滯后的濾波方法,其特征在于：

步驟一、采集MEMS三軸陀螺信號和加速度計信號；

步驟二、采集GPS輸出的速度V、經(jīng)度λ和緯度用GPS接收的導航信息來校正MEMS的陀螺和加速度計的漂移；

步驟三、選取MEMS/GPS組合導航系統(tǒng)Kalman濾波器的狀態(tài)變量，選取的Kalman濾波器的狀態(tài)變量為

其中，δλ是經(jīng)度誤差，是緯度誤差；δV_E是東向速度誤差，δV_N北向速度誤差；α,β,γ是初始的姿態(tài)誤差角；V_E為載體的東向速度，V_N為載體的北向速度；是加速度計零偏；ε_x,ε_y,ε_z是三個軸的陀螺常值漂移；

步驟四、根據(jù)MEMS的慣性系統(tǒng)的誤差方程，計算姿態(tài)誤差角、速度誤差和位置誤差；

步驟五、在接收到量測數(shù)據(jù)時，根據(jù)Bernoulli序列γ_k來確定測量數(shù)據(jù)是否發(fā)生延遲，并將γ_k加入到Kalman濾波器中；

步驟六、根據(jù)步驟五的判斷結果，選擇量測方程對系統(tǒng)進行量測更新；當系統(tǒng)只有一個測量數(shù)據(jù)發(fā)生了滯后，滯后時間為d，且在此時段之內(nèi)沒有再次發(fā)生測量數(shù)據(jù)滯后，則系統(tǒng)的量測方程為

Z_k=H_kX+V_k

Z′_k=H′_sX_s+V′_s

其中，k≥1；Z_k是k時刻的觀測向量；V_k是觀測噪聲，第二個式子是針對滯后的量測信息建立的量測方程，這個方程的Z′_k表示的s時刻的觀測向量，這里s=k-d，Z′_k在k時刻到達系統(tǒng)，但是反映的卻是s時刻的量測信息，V′_s是s時刻的觀測噪聲；

步驟七、計算量測數(shù)據(jù)延遲時間段內(nèi)濾波的修正量，在s時刻，量測數(shù)據(jù)發(fā)生了滯后，此時系統(tǒng)自動記錄s時刻的觀測矩陣H′_s和觀測噪聲方差R′_s，則此系統(tǒng)在k時刻的濾波修正量為

δX^k|k=M′Ks′(Zk′-Hs′X^s|s-1)]]>

其中，當延遲時間d=0的時候，M′=I；

步驟八、在量測數(shù)據(jù)正常到達時，利用真實的濾波增益K_s進行濾波；在量測數(shù)據(jù)發(fā)生延遲時，對當前通道下的濾波不做調(diào)整，另外開辟一個濾波通道，這個通道用標準Kalman濾波增益K′_s來代替另外一個通道真實濾波增益K_s并將這些增益保存在緩存中，然后利用步驟七中修正公式計算濾波修正量待滯后的量測數(shù)據(jù)到達時，對該計算通道的計算的濾波信息進行修正，得到k時刻的Kalman濾波值；

步驟九、對MEMS/GPS組合導航系統(tǒng)中量測滯后進行補償，利用步驟六中得出的Kalman濾波方程對組合導航系統(tǒng)進行濾波，當數(shù)據(jù)發(fā)生延遲時，根據(jù)步驟七中的公式計算濾波信息補償值，并切換到步驟八中的雙通道濾波方案進行濾波，當延遲的測量數(shù)據(jù)在k時刻到達時，將得到的與相加就得到了滯后補償后的Kalman濾波值。