基于自適應(yīng)UKF微型航姿系統(tǒng)噪聲的在線估計

1、基于自適應(yīng)ukf微型航姿系統(tǒng)噪聲在線估計劉宇劉瓊周帆李云梅向磊（垂慶郵電人學(xué)光電信息感測與傳輸技術(shù)巫慶市重點(diǎn)實(shí)驗(yàn)空，巫慶400065）摘 要：針對先驗(yàn)噪聲與系統(tǒng)克實(shí)噪聲不符引起標(biāo)準(zhǔn)無跡卡爾曼（ukf）性能退化的情況，提出了一種應(yīng)用于非線性吋變狀 態(tài)和參數(shù)聯(lián)合估計的自適應(yīng)ukf（aukf）算法。該算法首先基于新的協(xié)方差矩陣與相應(yīng)估計值z間存在的誤差，構(gòu)建成本 函數(shù)。然后為了最小化成木函數(shù)，采用梯度下降法進(jìn)行在線預(yù)估。最后對噪聲的協(xié)方差進(jìn)行在線更新，將更新后的協(xié)方差反 饋給標(biāo)準(zhǔn)的ukf,使更新后的理論協(xié)方差接近真實(shí)噪聲。通過實(shí)驗(yàn)仿真分析，口適應(yīng)ukf相比標(biāo)準(zhǔn)ukf,靜態(tài)精度提高了 40%,動態(tài)精度

2、提高了 30%,驗(yàn)證了自適應(yīng)ukf噪聲在線估計模型的準(zhǔn)確性和可行性。在實(shí)際測試系統(tǒng)中，對于時變噪聲 協(xié)方差不確定吋，自適應(yīng)ukf噪聲在線估計的魯棒性得到明顯改善。關(guān)鍵詞：ukf ； aukf；聯(lián)合估計；成本函數(shù)；梯度下降算法；魯棒性中圖分類號：tn212文獻(xiàn)標(biāo)識碼：anoise on-line estimation of mini-ahrs based on adaptive ukf algorithmyu liu, qiong liu,(chongqing municipal level key laboratory of pholoclcctronic information sensin

3、g & transmitting technology,chongqing university of posts and telecommunications. chongqing, 400065, china)abstract: considering that the prior noise of a normal unscented kalman filter does not agree with its real behavior, an adaptive unscented kalman filter algorithm applied to nonlinear join

4、t estimation of both time-varying states and parameters is proposed. firstly, a cost function is built based on the error between the covariance matrices of innovation and their corresponding estimations. then in order to minimize the cost function, the gradient descent method for on-line forecast i

5、s used. finally, the noise covariance is online updated, the updated covariance feedback to the standard ukf, which makes the updated theoretical covariance closer to the true noise. through experimental simulation analysis, compared adaptive ukf with standard ukf, static precision is improved by 40

6、%, dynamic precision is improved by 30%, the accuracy and feasibility of online adaptive ukf noise estimation model is verified. in the actual test system, for time-varying noise covariance is uncertain, adaptive ukf online noise estimation robustness are improved significantlykey words: ukf;aukf; j

7、oint cstimation;cost function; gradient dcsccnt;robustncss引言由于微型航姿系統(tǒng)的復(fù)雜性和時變性，為了讓 該系統(tǒng)得到精確的姿態(tài)角新息，準(zhǔn)確的估計狀態(tài)和 參數(shù)是很關(guān)鍵的。但傳統(tǒng)的估計方法具有明顯的缺 陷，如僅適用于線性時不變系統(tǒng)、對目標(biāo)模型的強(qiáng) 依賴性、對突變狀態(tài)跟蹤的滯后性等，這些都大大 限制英工程應(yīng)用。所以對航姿系統(tǒng)的噪聲進(jìn)行實(shí)時 的估計。將成為提高微型姿態(tài)儀精度的關(guān)鍵技術(shù)。 近些年來，人們在在線建模領(lǐng)域取得了驕人的成 績，參考模型控制領(lǐng)域正在成為一個重要的研究方 向。對于非線性系統(tǒng)來說，普遍運(yùn)用的估計器就 是擴(kuò)展卡爾曼濾波（ekf）。

8、盡管它被廣泛應(yīng)用，但 它在估計過程中仍然冇一些缺陷，比如動態(tài)狀態(tài)的 可導(dǎo)性，對偏差和散度敏感。無跡r爾曼濾波器 （ukf）,使用非線性動態(tài)方程直接代替了線性方程。 ukf與ekf相比,具有相同的復(fù)雜度,都是o（l3） 階。因?yàn)閡kf使用的非線性方程并沒有線性化， 因此不需要估計雅可比行列式或者h(yuǎn)essians矩陣就 能實(shí)現(xiàn)二階精度（ekf的精度是一階）。然而ukf 也是卡爾曼濾波器的擴(kuò)充，它和卡爾曼濾波一樣， 它只有在知道一些假定信息作為先驗(yàn)信息的情況 下沖叫才能實(shí)現(xiàn)優(yōu)異的性能。這些先驗(yàn)信息包括： 1）精確參考模型；2）噪聲分布的完整信息；3）正確 的初始條件。但是，因?yàn)橄闰?yàn)信息會被時變和不確

9、 定的環(huán)境所影響，所以不能保證很強(qiáng)的穩(wěn)定性?？?服上述提缺陷的一種有效方式是使用自適應(yīng)算法。 國內(nèi)外在自適應(yīng)濾波領(lǐng)域已有許多研究。huetal 提出了限制kf記憶的方法，這種方式能根據(jù)最優(yōu)化條件來自適應(yīng)調(diào)整遺忘因子。may beck罔設(shè)計一 個自適應(yīng)kf,通過最大似然估計的方法估計錯誤 統(tǒng)計的協(xié)方差矩陣。loebis et al叫吏用了模糊邏輯 技術(shù)來更新傳感器噪聲方差。但是在實(shí)際屮，每次 要立即確定協(xié)方差的增量值是困難的。在本論文 中，提出了自適應(yīng)無跡卡爾曼濾波，它通過新的協(xié) 方差矩陣和它相應(yīng)的估計之間的誤差建立了成本 函數(shù)，然后將更新好的協(xié)方差反饋到該系統(tǒng)，實(shí)現(xiàn) 系統(tǒng)噪聲的在線估計。1 u

10、kf濾波算法11 ukf濾波的算法原理假設(shè)微型航姿系統(tǒng)的離散方程和觀測方程分 別為：xhk、-1-1(i)zk=h(xk) + ek(2)對以知道，ukf算法中的觀測方程也是非線性 函數(shù)。狀態(tài)向量笛和觀測向量乙所包含坐標(biāo)向量 數(shù)量，根據(jù)實(shí)際的微型航姿系統(tǒng)三維姿態(tài)數(shù)據(jù)確 定。1.2標(biāo)準(zhǔn)ukf的計算步驟(1) 利用初始狀態(tài)估計，設(shè)計最初的2n+l個 sig伽點(diǎn)：ukf會將噪聲項添加到狀態(tài)項屮，即狀 態(tài)擴(kuò)維，并將驅(qū)動噪聲陣q擴(kuò)到p陣中。進(jìn)行狀態(tài)擴(kuò) 充的原因是考慮噪聲對系統(tǒng)的影響，進(jìn)行擴(kuò)維以后 的狀態(tài)變量和其他協(xié)方差矩陣：pk 0 0 p; = o a o_0 0 rk設(shè)賈為l維列向量，顯然l二n+p

11、+q。(2) 利用過程模型變換這些sgam點(diǎn)，即對狀態(tài) 變量進(jìn)行u變換：xf(k + 1/ k) = fxt (kl k)yu(k)yk(5)式中/卜為u變換的具體方程，步驟如下：(3)(4)(6)=匹十(電亙埜)-.(心1,.,勵(?)xk - (j(2 + yl)yx(,=加 +1,2m)(11)(12)(13)(14)昭伽+ 2)(8)= a /(m + 2) + (1 -+ /?)(9)叫”二必=0.5 皈+2 )j=( 皿 (10)式中加為狀態(tài)參數(shù)個數(shù),兄二小加+歸-加為尺 度因子，其中的a表示sig伽點(diǎn)到乞的距離，一般 將這個數(shù)值設(shè)置為104<a<l, k為常數(shù)，一般設(shè)

12、 為0或者為3-7, “用于融入預(yù)報向量笛的驗(yàn)前信 息，對于高斯分布，0=2最優(yōu)。(j(m+久)工元)是 矩陣平方根的第i列，可以通過cholesky分解獲得。(3) 計算預(yù)測估計值f伙+ 1/燈=£比川2“ + 1/幻/h)(4) 計算預(yù)測協(xié)方差p(k+l/k) = £比叫 (k + l/k)-x 伙 + 1/幻丁/=0(5) 通過測量方程計算測量值乙伙 + /k) = hx 伙+ 1/幻(6) 計算預(yù)測測量值2nz(k + / k) = wi(c)zi(k + / k)-.r=0八z伙+1/幻乙伙+1/切一z伙+1/燈r+&式屮，假設(shè)預(yù)報殘差向量為k =z

13、63;k + /k)-zk + /k)可以知道向量確定等價 權(quán)戸，進(jìn)而求出。(7) 計算去仏+1/幻和2伙+ 1/燈的協(xié)方差化伙+ 1/約二左比尢伙+ 1/幻八1=0、x(k +1 / r)z“ +1 / 約 一 z 伙 +1 / 加(8) 計算卡爾曼增益k( £+ 1 )=匕 i(9) 更新誤差協(xié)方差p伙+ 1/比+ 1)=卩伙+ 1/燈伙+ 1)冒7伙+ 1)(10) 更新狀態(tài)x( ah- 1 /總 1#x *+1 £k(心1) £ 匚)z幷(15)(16)(17)(18)ukf其實(shí)就是狀態(tài)估計方法的一種，同過聯(lián)合 估計系統(tǒng)參數(shù)，將模型參數(shù)作為動態(tài)變量，將其添

14、加到真實(shí)的狀態(tài)矢量，合并成增廣狀態(tài)矢量，利用 自適應(yīng)ukf對狀態(tài)和模型參數(shù)進(jìn)行預(yù)測估計。ukf 方法其關(guān)鍵技術(shù)就是對sigma點(diǎn)采樣策略，包括 sigma點(diǎn)個數(shù)的確定，位置的確定，和相應(yīng)權(quán)值的確 定，目前大多數(shù)研究表明，采用2n+l個sigma點(diǎn)， 它們服從x分布，并且當(dāng)前的狀態(tài)值與方差和樣本 均值相同。2自適應(yīng)ukf濾波算法2.1成本指標(biāo)函數(shù)本文主要是將最小化真實(shí)值和新息方差的差 作為成本指標(biāo)函數(shù)，將濾波更新中產(chǎn)生的真實(shí)值和 估計狀態(tài)值差值來更新濾波器的參數(shù)。yr （式中n是估計窗口的尺度因子，心是新息，vk 可以這樣描述：u =兒-加t（2°）式子中，兒和”-是濾波和預(yù)測的真實(shí)值

15、。相 刈來說，我們可以通過標(biāo)準(zhǔn)ukf知道：$=：£町億“匸孤門兔- w） /=0然后提出自適應(yīng)的成本指標(biāo)函數(shù)，以盡量減少 成本函數(shù)。k = "3（） = "（/ 一 £ 尸（22）式中，t是取樣時間或者恒定的時間間隔。式 （25）需要結(jié)合常規(guī)的ukf來實(shí)現(xiàn)自適應(yīng)ukf。rfl 式（22）可得：(26)以根據(jù)式（19）, （20）可以推導(dǎo)出:(27)根據(jù)式（21）,可以推導(dǎo)出：務(wù)=z吋一愛p（兀詢1 一）t販 r=0 兩(28)為了計算兩爲(wèi)辿，必須先求先ukf的濾 波方程的導(dǎo)數(shù)。參照標(biāo)準(zhǔn)ukf算法，對應(yīng)的可以 得到嘰悶的迭代計算公式如下所述：2.2自適應(yīng)u

16、kf實(shí)際上，方差矩陣q和r是決定精度和穩(wěn)定性的 因素，因此，選擇協(xié)方差的對角元素去自適應(yīng)更新 過程噪聲q。不失一般性,r也可以通過該方法更新。 whittake準(zhǔn)則是一種比較常規(guī)的口適應(yīng)參數(shù)更新算 法，方法簡單，計算量少。此方法就是基于梯度下 降算法，將負(fù)梯度方向作為參數(shù)更新的方向。即：sigma點(diǎn)的導(dǎo)數(shù)wa-i(29)(23)式中，q：為時刻噪聲方差陣第m行的對角線元 素；qk是控制收斂速度的自適應(yīng)調(diào)節(jié)率，其符合傳 統(tǒng)的隨機(jī)佔(zhàn)計條件久o' »人=出（24）kk根據(jù)式（23）,我們可以得到下面遞歸機(jī)制：(25)p z =)pz = n + l,.2n2 j加 wkj/? +

17、兄(（1）初始化測量更新:),/ = 1,川dq； <72如.=血二l 一 厶 + 2.().,/ = « + 1,.2川dq；1 dx x=ximl 兩"梯度預(yù)測:；=0'勿;”勿;"八汗如 +(z)(空吐至zur_， '阻民=_p:刃d p-< 砸砸&kk _ p-i _ p- p- °仏<砸禍飾dq；opk _ d&w _ 6kk p _ 尺績齊 y 議一市一議砸一八市 函 _ cxkk- gcyk- 一+議6皿)一瓦研)(33)3系統(tǒng)試驗(yàn)及分析3.1仿真平臺在本論文中，提出的自適應(yīng)ukf運(yùn)用在自主

18、 研制的微型航姿系統(tǒng)(參見圖1),微型航姿系統(tǒng) 主要是由三軸陀螺儀、三軸加速度和三軸磁強(qiáng)計組 成，根據(jù)慣性導(dǎo)航的相關(guān)理論，采用東北天導(dǎo)航坐 標(biāo)描述載體姿態(tài)新息重要模型。圖1門主研制的微型姿態(tài)儀3.2微型航姿系統(tǒng)的測量的系統(tǒng)結(jié)構(gòu)本文提出的是基于自適應(yīng)ukf噪聲的在線估 計，系統(tǒng)利用加速度和磁力計計算出姿態(tài)，將陀螺 的輸出反饋到動態(tài)解算過程中，利用自適應(yīng)ukf 實(shí)現(xiàn)對磁力計和加速度的噪聲估計，1司時也用磁力 計和加速度計対陀螺的累積誤差進(jìn)行在線預(yù)估。微 型航姿系統(tǒng)的數(shù)學(xué)模型如圖2所示。圖2微型航姿系統(tǒng)測戢的系統(tǒng)結(jié)構(gòu)3.3微型姿態(tài)儀的狀態(tài)模型在慣性坐標(biāo)系屮，定義姿態(tài)角為(0 &, 0, 通過

19、姿態(tài)角的導(dǎo)數(shù)，對磁力計和加速度動態(tài)測量的 誤差進(jìn)行補(bǔ)償，此時，在對自適應(yīng)ukf的狀態(tài)模 型進(jìn)行設(shè)計時，采用陀螺儀的數(shù)據(jù)，但是在k+1時 刻姿態(tài)角通過該時刻角速度在釆樣時間內(nèi)的積分， 如式(34):(32)/ 7、= rr(x,(fi)s、+疋(兀,0)疋(幵0)"0)0(34)丿allju丿+ 卩(兀,0)疋(”&) 0sinta n&cos tan 0、/ 、%=0cos0sin0/<0sin 0/cos。cos 0 / cos 0 jm丿(35) 式(35),等式左邊是姿態(tài)角，它是通過磁力計和 加速度進(jìn)行解算的，把這個解算出來的姿態(tài)角作為 自適應(yīng)ukf算法的

20、觀測值，式(36)觀測模型。丿 、 丿 oo o oo o o o o o o 1 o o 1 1 o o=、 丿(36由于時間的積累，根據(jù)ukf的狀態(tài)方程，將 前一個時刻計算的結(jié)果作為下一個時刻角速度，對 其做一個簡單的預(yù)估計，但是估計中會帶來積累誤 差，如式(37) o(37)本論文將磁力計和加速度訃解算后的姿態(tài)角 作為觀測值，用估算值通過加權(quán)計算去求得z,+1, 然后經(jīng)過式(38)去補(bǔ)償陀螺長時間帶來的積累誤 差,m是卡爾曼的增益。兀；+1-母+i = m(z&+ -z+j(38)式(37)是陀螺儀的輸出，通過狀態(tài)轉(zhuǎn)移可以描 述其中的動態(tài)過程，能夠補(bǔ)償通過磁力計和加速度 動態(tài)測量

21、的誤差，通過反饋，能夠克服長時間陀螺 的積累誤差。4仿真驗(yàn)證為了驗(yàn)證自適應(yīng)ukf算法的效果，實(shí)驗(yàn)對自 主研制的微型航姿系統(tǒng)進(jìn)行數(shù)據(jù)釆集，將算法運(yùn)用 到該系統(tǒng)中。采樣時間選擇30s,采樣頻率為50h乙 為了驗(yàn)證該算法的優(yōu)越性，將自適應(yīng)ukf算法和 普通的ukf算法進(jìn)行對比，分別做了靜態(tài)和動態(tài) 實(shí)驗(yàn)。由于橫滾角和俯仰角在算法的原理上一樣， 為避免贅述，所以本實(shí)驗(yàn)主要對航向和俯仰進(jìn)行驗(yàn) 證。圖3分別是靜態(tài)和動態(tài)俯仰角和航向角偏差。 圖中的藍(lán)線為標(biāo)準(zhǔn)ukf算法獲得的誤差曲線，圖 中的紅線是自適應(yīng)ukf算法獲得的誤差曲線，黑 色的線是期望得到的值。圖中曲線表明，靜態(tài)實(shí)驗(yàn), 采用標(biāo)準(zhǔn)ukf航向角偏差在&#

22、177;0.5,靜態(tài)俯仰角偏 差在土 1°,然而自適應(yīng)ukf算法靜態(tài)航向角偏差在 0.3°,俯仰角偏差在o.zo動態(tài)實(shí)驗(yàn)，標(biāo)準(zhǔn)ukf 航向角偏差在±2°,俯仰角偏差在0.75 o自適應(yīng) ukf航向角偏差在1°,俯仰角偏差在0.4實(shí)驗(yàn)驗(yàn) 證表明，自適應(yīng)ukf算法在動靜態(tài)實(shí)驗(yàn)中都比標(biāo) 準(zhǔn)的ukf算法對誤差估計的精度要高。ukf不能 得到較好的估計性能；而自適應(yīng)ukf算法可以很 快地克服噪聲特性的改變。1r曲mm 自般urng谿割200 ioo 6938031»312001400m.（+）（a）靜態(tài)俯仰角偏差葉礎(chǔ)ukf 自詼趙能：00.5-

23、0.57 (9*二eeg 掘轅203 ioc 6008301 00012001400(b) 靜態(tài)航向角偏差酬ukf方法1.5一自適應(yīng)ukf方法期璽能91.5200400600800100012001400耕點(diǎn)（個）(c) 動態(tài)俯仰角偏差3-2觥ukf施自駆mm 期鏗020340c6c010m1203 u0co!二匸【£蘇祓w(d) 動態(tài)航向角偏差圖3動靜態(tài)姿態(tài)角偏差本實(shí)驗(yàn)是使用加速度計，磁強(qiáng)計和陀螺儀，靜 態(tài)是直接把微型航姿系統(tǒng)放在雙軸轉(zhuǎn)臺上。動態(tài)采 用的是搖擺實(shí)驗(yàn)，采用標(biāo)準(zhǔn)ukf和自適應(yīng)ukf算 法對傳感器數(shù)據(jù)融合后，靜態(tài)動態(tài)仿真結(jié)果如圖(4) 所示：圖屮表明，自適應(yīng)ukf較標(biāo)準(zhǔn)u

24、kf對噪聲 的在線估計精度高，在靜態(tài)的自適應(yīng)程度較動態(tài) 高。對噪聲的精確估計后，使微型航姿系統(tǒng)的靜態(tài) 俯仰角精度在±0.2°,航向角精度在土0.5°o動態(tài) 俯仰角精度土0.8°,動態(tài)航向角精度±1°。已經(jīng)滿952co40cs30it)儆1200um(a)靜態(tài)俯仰圖r7170t羽宅*»600 a» 1occ 1203 14co采滋(個)26$(b)靜態(tài)航向角1co冊1i111 加11'1ico4cc 6m 800m (t)梯umi自和imf旋1»j120014(0(c)動態(tài)俯仰角loo - *1 卅

25、111卅 15edlki訪法ij bgguff 施10001200 uof(d)動態(tài)航向角 圖4動靜態(tài)姿態(tài)角本論文針對傳統(tǒng)ukf因?yàn)榧俣ǖ南闰?yàn)噪聲與 實(shí)際系統(tǒng)噪聲不符導(dǎo)致精度下降，提出了一種基于 自適應(yīng)無跡卡爾曼濾波器(aukf),構(gòu)建以新的協(xié)方 差矩陣與相應(yīng)估計值誤差為成本函數(shù)，將更新的協(xié) 方差反饋到實(shí)測系統(tǒng)，對自主研制的微型航姿系統(tǒng) 的噪聲進(jìn)行在線估計，實(shí)驗(yàn)仿真表明，靜態(tài)精度提 高了 40%,動態(tài)精度提高了 30%,驗(yàn)證了在線估計 模型的正確性和可行性。有很好的工程應(yīng)用價值。 參考文獻(xiàn)：1 徐慧娟，吳美平，羅兵.ekf和ukf ins/gps組合導(dǎo)航中的應(yīng)用分析j.航天控制,2006,26(6):7-10.xu huijuan, wu mciping, luo bing.analysis of extended and unscented kalman filtering for ins/gps integrationfj.a erospace con trol,2006,26:710.2 張欣，口越，趙常均.多旋翼姿態(tài)解算中的改進(jìn)自適應(yīng)擴(kuò) 展kalman算法j.光學(xué)精密工程,2014zhang xin, bai yuc, zhao changjun.improved adaptive extended kalman algorithm