[發明專利]一種基于誤差四元數三維矢量分布的高斯粒子濾波數據處理方法有效
| 申請號： | 202010950221.0 | 申請日： | 2020-09-10 |
| 公開（公告）號： | CN112066993B | 公開（公告）日： | 2022-12-13 |
| 發明（設計）人： | 周翟和;曾傳偉;鄒克臣;賴際舟;姚睿;曾慶喜 | 申請（專利權）人： | 南京航空航天大學 |
| 主分類號： | G01C21/20 | 分類號： | G01C21/20;G01C21/10;G01C21/00;G06F17/16 |
| 代理公司： | 暫無信息 | 代理人： | 暫無信息 |
| 地址： | 211106 江*** | 國省代碼： | 江蘇;32 |
| 權利要求書： | 查看更多 | 說明書： | 查看更多 |
| 摘要： | |||
| 搜索關鍵詞： | 一種 基于 誤差 四元數 三維 矢量 分布 粒子 濾波 數據處理 方法 | ||
1.一種基于誤差四元數三維矢量分布的高斯粒子濾波數據處理方法，其特征在于，具體包括以下步驟：
步驟1：將誤差四元數δq的三維矢量δρ定義為狀態量并利用陀螺儀輸出建立系統狀態方程；將加速度計及磁力計的輸出作為觀測量建立系統觀測方程；
步驟2：通過加速度計和磁力計的初始觀測量獲得初始四元數粒子集對系統進行初始化；其中，i表示粒子個數索引，M表示粒子數，上標+表示后驗，下標0表示初始時刻；
步驟3：已知k時刻狀態量的后驗分布，從中采樣獲得后驗誤差四元數三維矢量粒子集通過k時刻最優四元數估計值叉乘誤差四元數粒子集來生成后驗四元數粒子集其中，下標k表示k時刻；生成后驗四元數粒子集的方法如下：
Step1：從k時刻狀態量的后驗分布中采樣獲得后驗誤差四元數三維矢量粒子集
其中，符號*表示乘法，表示高斯分布，chol(·)為喬里斯基分解函數，分別為后驗分布均值、協方差，randn(3，1)為生成三維標準正態分布隨機樣本的函數；
Step2：通過k時刻最優四元數估計值叉乘誤差四元數粒子集生成后驗四元數粒子集
其中，符號T表示矩陣轉置，表示四元數乘法算子，從步驟6的上一時刻獲得；
步驟4：根據步驟3得到的后驗四元數粒子集利用步驟1中系統狀態方程得到更新后的四元數粒子集其中，上標-表示先驗含義，下標k+1表示k+1時刻；
步驟5：根據步驟1中系統觀測方程對步驟4得到的四元數粒子集進行量測更新得到k+1時刻權值
步驟6：計算k+1時刻最優四元數估計值并根據最優四元數估計值計算姿態角；其中，姿態角包括俯仰角θ、橫滾角γ、航向角ψ；
步驟7：根據步驟5中四元數粒子集的相應權值以及步驟6中k+1時刻的最優四元數估計值由四元數粒子集叉乘最優四元數估計值的逆估計k+1時刻狀態量的后驗分布，估計方法如下：
Step1：根據步驟5中四元數粒子集的相應權值以及步驟6中k+1時刻的最優四元數估計值由四元數粒子集叉乘最優四元數估計值的逆生成k+1時刻誤差四元數粒子集
其中，(·)-1表示矩陣求逆；
Step2：k+1時刻狀態量的后驗分布均值求解：
Step3：k+1時刻狀態量的后驗分布協方差求解：
其中，E{·}表示期望，Pqqk+1為四元數二階矩，q1為四元數標量部分，ρ為四元數矢量部分，I3×3為3階單位矩陣，[ρ×]為ρ的反對稱矩陣，為k+1時刻狀態量的后驗分布，逼近姿態估計誤差的真實分布；
步驟8：根據步驟6、步驟7得到k+1時刻濾波估計值，循環執行步驟3、4、5、6、7，得到下一時刻濾波估計值。
該專利技術資料僅供研究查看技術是否侵權等信息，商用須獲得專利權人授權。該專利全部權利屬于南京航空航天大學，未經南京航空航天大學許可，擅自商用是侵權行為。如果您想購買此專利、獲得商業授權和技術合作，請聯系【客服】
本文鏈接：http://www.szxzyx.cn/pat/books/202010950221.0/1.html，轉載請聲明來源鉆瓜專利網。
