一種基于平方根容積卡爾曼濾波器的飛行器姿態(tài)估計方法
【專利摘要】本發(fā)明涉及一種基于平方根容積卡爾曼濾波器的飛行器姿態(tài)估計方法。本發(fā)明包括在計算時間更新時，設(shè)置初始值及羅德里格斯參數(shù)；計算容積點值；計算四元數(shù)誤差點；計算四元數(shù)容積點；計算迭代四元數(shù)及迭代四元數(shù)誤差；計算迭代容積點；估計一步預(yù)測狀態(tài)；估計平方根因子預(yù)測誤差協(xié)方差陣；計算測量更新中的容積點；計算迭代容積點值；計算預(yù)測測量估計；計算平方根因子新息協(xié)方差矩陣估計；計算互協(xié)方差陣；卡爾曼增益；狀態(tài)更新估計；計算估計誤差協(xié)方差陣及相應(yīng)的平方根因子；四元數(shù)更新；計算相應(yīng)的估計誤差歐拉角。本發(fā)明能夠提高對飛行器姿態(tài)的精確性與穩(wěn)定性。
【專利說明】一種基于平方根容積卡爾曼濾波器的飛行器姿態(tài)估計方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于非線性系統(tǒng)的飛行器姿態(tài)估計問題，特別涉及一種基于平方根容積卡 爾曼濾波器的飛行器姿態(tài)估計方法。

【背景技術(shù)】
[0002] 非線性濾波是信號處理、目標(biāo)跟蹤和控制領(lǐng)域方面的熱門話題之一，特別是，在卡 爾曼濾波框架下的非線性濾波研究仍是一個很火的問題，在近年來受到越來越多的關(guān)注和 研究。
[0003] 眾所周知，姿態(tài)估計的本質(zhì)是典型的非線性濾波問題。傳統(tǒng)的非線性濾波器不能 直接用來處理大多情況下的飛行器姿態(tài)確定。這是因為可用的姿態(tài)估計器需要非線性濾波 器能夠很好地結(jié)合實際的姿態(tài)估計背景。那就是，姿態(tài)估計原則必須要整合到非線性濾波 器的設(shè)計過程中。傳統(tǒng)的非線性濾波方法包括擴(kuò)展卡爾曼濾波器（EKF)和無跡卡爾曼濾波 器（UKF)。EKF是現(xiàn)在在非線性系統(tǒng)中應(yīng)用最為廣泛的方法。通過近似狀態(tài)分布為高斯隨 機(jī)變量，然后對其進(jìn)行一階泰勒展開來近似非線性函數(shù)。但不幸的是，這種線性化方法只能 得到低的估計精度，甚至?xí)斐晒烙嬈鞯牟环€(wěn)定性。同時，EKF需要計算雅克比矩陣，且計算 量會很大。為了克服EKF的線性化，無跡卡爾曼濾波器（UKF)是一個處理非線性濾波時更 為實用的方法。因為UKF能夠直接使用非線性系統(tǒng)的模型從而取代使用EKF時的線性化， 同時也避免了對雅克比矩陣的計算。UKF是通過非線性無跡變換（UT)和最小采樣即sigma 點。這種策略使得UKF能夠明顯改善EKF的性能，盡管UKF已經(jīng)有很多的應(yīng)用，但它還是有 很多缺點，尤其是在高維情況下。同時UKF計算過程中對于協(xié)方差矩陣并不能總是保證是 正定的。因此，UKF的使用仍然受到限制。


【發(fā)明內(nèi)容】

[0004] 本發(fā)明針對現(xiàn)有技術(shù)的不足，提供了一種基于平方根容積卡爾曼濾波器的飛行器 姿態(tài)估計方法。
[0005] 本發(fā)明的方法具體是：
[0006] 1).設(shè)置初始值參數(shù)，假設(shè)飛行器姿態(tài)帶有平方根因子估計誤差協(xié)方差S+和飛 行器姿態(tài)狀態(tài)估計夫。
[0007] 2).設(shè)置姿態(tài)迭代

【權(quán)利要求】
1. 一種基于平方根容積卡爾曼濾波器的飛行器姿態(tài)估計方法，其特征在于該方法包括 以下步驟： 1) .設(shè)置初始值參數(shù)，假設(shè)飛行器姿態(tài)帶有平方根因子估計誤差協(xié)方差S+和飛行器 姿態(tài)狀態(tài)估計; 2) .設(shè)置姿態(tài)迭代
其中k表示時間； 3) .計算第i次容積點參量
，其中表示i個數(shù)，i = 1，2，...， m，表示k-Ι時刻飛行器姿態(tài)帶有平方根因子估計誤差協(xié)方差陣，ξ ,表示列向量序 列，表示k-Ι時刻飛行器姿態(tài)狀態(tài)估計； 4) .計算飛行器姿態(tài)參數(shù)誤差點
三個歐拉角，即飛行器姿態(tài)方位角，表示角度的余弦值； 5) .將計算得到的飛行器姿態(tài)參數(shù)誤差點帶入第i次四元數(shù)容積點
為飛行器姿態(tài)參數(shù)估計； 6) .迭代四元數(shù)預(yù)測估計
及迭代四元數(shù)預(yù)測誤差
為飛行器角度函數(shù)； 7) .迭代容積點.
其中，f為標(biāo)量因子，a e [〇，1]; 8) .計算一步狀態(tài)預(yù)測估計、帶有平方根因子的預(yù)測誤差協(xié)方差
為狀態(tài)估計的均方差，
的平方根因子； 9) .將帶有平方根因子的預(yù)測誤差協(xié)方差及飛行器姿態(tài)k-Ι時刻的狀態(tài)預(yù)測估計值 毛η帶入測量更新容積點方程
10) .測量更新中的迭代容積點
11) .計算測量預(yù)測估計^_、平方根因子新息協(xié)方差陣SzUHiTriaaZkh SK, k])、互協(xié)方差陣
、卡爾曼增益，其中，SK,k表 示為測量噪聲方差Rk的平方根因子；
12) .從上面計算中得到狀態(tài)更新估計
、平方根因子Sk|k、估 計誤差協(xié)方差陣
13) .計算更新四元數(shù)估計4_、四元數(shù)估計誤差的 14) .判斷是k否小于迭代次數(shù)N，如果k小于N，則跳轉(zhuǎn)到步驟2, k : = k+Ι，否則，執(zhí)行 步驟15 ; 15) .計算中得到三個歐拉角度（飛行器的滾動角，俯仰角和偏航角）的一致估計誤差 值
16).輸出結(jié)果飛行器姿態(tài)方位角δ?Β#，實現(xiàn)對飛行器姿態(tài)的估計。
【文檔編號】G01C21/20GK104121907SQ201410369663
【公開日】2014年10月29日 申請日期:2014年7月30日 優(yōu)先權(quán)日:2014年7月30日
【發(fā)明者】葛泉波, 邵騰, 程天發(fā), 文成林 申請人:杭州電子科技大學(xué)