[發明專利]一種改進的魯棒無跡卡爾曼濾波組合導航方法

在審

• 申請號: 202010843690.2
• 申請日: 2020-08-20
• 公開（公告）號: CN111896008A
• 公開（公告）日: 2020-11-06
• 發明（設計）人: 王偉;龔佳承;李欣;薛冰;黃平
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G01C21/16
• 代理公司: 哈爾濱市松花江專利商標事務所 23109
• 代理人: 劉強

1.一種改進的魯棒無跡卡爾曼濾波組合導航方法，其特征在于包括以下幾個步驟：

步驟一：收集UWB系統和MEMS系統輸出的數據；

步驟二：選擇系統的狀態量和觀測量，建立UWB/MEMS組合導航系統狀態空間模型；

步驟三：對UWB/MEMS組合導航系統進行濾波初始化；

步驟四：在k時刻對UWB/MEMS組合導航系統進行時間更新和量測更新，得到系統狀態一步預測的誤差協方差和一步預測的互協方差；

步驟五：引入量測誤差判據，若量測存在誤差，則計算多重因子的魯棒矩陣，并對狀態一步預測的誤差協方差進行校正，然后進入步驟六，若量測不存在誤差，則直接進入步驟六；

步驟六：若量測存在誤差，則利用校正后的狀態一步預測誤差協方差，計算k時刻系統的狀態估計值和狀態誤差協方差，若量測不存在誤差，則利用步驟四中的狀態一步預測誤差協方差，計算k時刻系統的狀態估計值和狀態誤差協方差；

步驟七：根據步驟六中得到的狀態估計值獲取MEMS慣導的陀螺儀、加速度計的常值漂移，并將其反饋到組合系統中，輸出姿態、速度和位置信息。

2.根據權利要求1所述的一種改進的魯棒無跡卡爾曼濾波組合導航方法，其特征在于所述UWB/MEMS組合導航系統狀態空間模型的系統狀態方程表示為：

X_k+1＝F_kX_k+w_k

其中，X_k為15維的慣導解算信息誤差，表示為其中，φ_k＝[φ_k,x φ_k,y φ_k,z]^T為k時刻慣導解算的姿態誤差，ΔV_k＝[ΔV_k,x ΔV_k,y ΔV_k,z]^T為k時刻慣導解算的地理坐標系下的速度誤差，ΔP_k＝[ΔL_k Δλ_k Δh_k]^T為k時刻慣導解算的經緯高誤差，ε_k＝[ε_k,x ε_k,y ε_k,z]^T為k時刻陀螺常值漂移，為k時刻加速度計零偏，w_k為k時刻組合系統過程噪聲，w_k噪聲協方差矩陣為Q_k，F_k為k時刻慣導系統狀態轉移矩陣。