[發明專利]一種基于變分迭代卡爾曼濾波的李群重尾干擾噪聲動態飛行器姿態估計方法

在審

• 申請號: 202110982644.5
• 申請日: 2021-08-25
• 公開（公告）號: CN113670315A
• 公開（公告）日: 2021-11-19
• 發明（設計）人: 王蛟龍;段一銘;韓楚楚;曲金慧
• 申請（專利權）人: 江南大學
• 主分類號: G01C21/20
• 分類號: G01C21/20;G06F17/15;G06F17/16
• 代理公司: 哈爾濱市陽光惠遠知識產權代理有限公司 23211
• 代理人: 張勇

1.一種基于變分迭代卡爾曼濾波的李群重尾干擾噪聲動態飛行器姿態估計方法，其特征在于，包括以下步驟：

步驟一：建立矩陣李群SO(3)上姿態估計的離散姿態動力學模型；

步驟二：將系統的李群動態模型映射到歐氏幾何空間；

步驟三：基于觀測數據定義重尾噪聲中各參數的先驗分布函數；

步驟四：對重尾噪聲的概率密度函數進行變分貝葉斯近似；

步驟五：定點迭代求解系統狀態與概率密度函數的分布參數；

步驟六：將步驟五中求解出的各個參數帶入步驟四中進行計算，得到噪聲的后驗概率密度函數近似值。

2.根據權利要求1所述的一種基于變分迭代卡爾曼濾波的李群重尾干擾噪聲動態飛行器姿態估計方法，其特征在于，所述的步驟一中，SO(3)上姿態估計的離散姿態動力學模型如下所示：

R_k＝exp_G(w_k-1)R_k-1Ω_k-1 k＝1,2,... (1)

其中，R_k∈SO(3)，表示在第k時刻將本體坐標系映射到地球地固坐標系的旋轉矩陣；Ω_k∈G為根據姿態演變模型編碼對瞬時旋轉的控制輸入；高斯白過程噪聲表示通過李群指數項exp_G(w_k)注入的小過程噪聲，Q_w表示協方差；Y_k是具有已知向量b′和b″的離散噪聲觀測y_k′和y_k″的合成；v_k′,v_k″為獨立的各向同性白噪聲，其協方差為對角矩陣，分別用Q_v′和Q_v″表示。

3.根據權利要求1所述的一種基于變分迭代卡爾曼濾波的李群重尾干擾噪聲動態飛行器姿態估計方法，其特征在于，所述的步驟二中，設是R_k-1在k-1時刻的估計值，是R_k的估計值；定義不變誤差ξ_k與模型參數Ω_k-1無關，其演化模型服從：

ξ_k＝ξ_k-1-w_k+O(|ξ_k-1|²,|w_k|²,|ξ_k-1||w_k|) (3)

其中，O(·)為關于ξ_k-1和w_k的可忽略的二次以上小項；進一步忽略該二次項，則在李群動態模型下旋轉矩陣R_k的求解就可以轉化為在歐式空間中線性狀態空間下對不變誤差ξ_k的求解：

那么，觀測序列Y_k則被重寫為一個關于不變誤差ξ_k的函數，新的序列表示為z_k，具有以下形式：

其中，和分別是映射后觀測矩陣與噪聲。