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

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

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


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

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

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