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

2、提高了 30%,驗(yàn)證了自適應(yīng)ukf噪聲在線估計(jì)模型的準(zhǔn)確性和可行性。在實(shí)際測(cè)試系統(tǒng)中，對(duì)于時(shí)變?cè)肼?協(xié)方差不確定吋，自適應(yīng)ukf噪聲在線估計(jì)的魯棒性得到明顯改善。關(guān)鍵詞：ukf ； aukf；聯(lián)合估計(jì)；成本函數(shù)；梯度下降算法；魯棒性中圖分類號(hào)：tn212文獻(xiàn)標(biāo)識(shí)碼：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ù)雜性和時(shí)變性，為了讓 該系統(tǒng)得到精確的姿態(tài)角新息，準(zhǔn)確的估計(jì)狀態(tài)和 參數(shù)是很關(guān)鍵的。但傳統(tǒng)的估計(jì)方法具有明顯的缺 陷，如僅適用于線性時(shí)不變系統(tǒng)、對(duì)目標(biāo)模型的強(qiáng) 依賴性、對(duì)突變狀態(tài)跟蹤的滯后性等，這些都大大 限制英工程應(yīng)用。所以對(duì)航姿系統(tǒng)的噪聲進(jìn)行實(shí)時(shí) 的估計(jì)。將成為提高微型姿態(tài)儀精度的關(guān)鍵技術(shù)。 近些年來(lái)，人們?cè)谠诰€建模領(lǐng)域取得了驕人的成 績(jī)，參考模型控制領(lǐng)域正在成為一個(gè)重要的研究方 向。對(duì)于非線性系統(tǒng)來(lái)說(shuō)，普遍運(yùn)用的估計(jì)器就 是擴(kuò)展卡爾曼濾波（ekf）。

8、盡管它被廣泛應(yīng)用，但 它在估計(jì)過(guò)程中仍然冇一些缺陷，比如動(dòng)態(tài)狀態(tài)的 可導(dǎo)性，對(duì)偏差和散度敏感。無(wú)跡r爾曼濾波器 （ukf）,使用非線性動(dòng)態(tài)方程直接代替了線性方程。 ukf與ekf相比,具有相同的復(fù)雜度,都是o（l3） 階。因?yàn)閡kf使用的非線性方程并沒(méi)有線性化， 因此不需要估計(jì)雅可比行列式或者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)信息會(huì)被時(shí)變和不確

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

10、kf濾波算法11 ukf濾波的算法原理假設(shè)微型航姿系統(tǒng)的離散方程和觀測(cè)方程分 別為：xhk、-1-1(i)zk=h(xk) + ek(2)對(duì)以知道，ukf算法中的觀測(cè)方程也是非線性 函數(shù)。狀態(tài)向量笛和觀測(cè)向量乙所包含坐標(biāo)向量 數(shù)量，根據(jù)實(shí)際的微型航姿系統(tǒng)三維姿態(tài)數(shù)據(jù)確 定。1.2標(biāo)準(zhǔn)ukf的計(jì)算步驟(1) 利用初始狀態(tài)估計(jì)，設(shè)計(jì)最初的2n+l個(gè) sig伽點(diǎn)：ukf會(huì)將噪聲項(xiàng)添加到狀態(tài)項(xiàng)屮，即狀 態(tài)擴(kuò)維，并將驅(qū)動(dòng)噪聲陣q擴(kuò)到p陣中。進(jìn)行狀態(tài)擴(kuò) 充的原因是考慮噪聲對(duì)系統(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) 利用過(guò)程模型變換這些sgam點(diǎn)，即對(duì)狀態(tài) 變量進(jìn)行u變換：xf(k + 1/ k) = fxt (kl k)yu(k)yk(5)式中/卜為u變換的具體方程，步驟如下：(3)(4)(6)=匹十(電亙埜)-.(心1,.,勵(lì)(?)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ù)個(gè)數(shù),兄二小加+歸-加為尺 度因子，其中的a表示sig伽點(diǎn)到乞的距離，一般 將這個(gè)數(shù)值設(shè)置為104<a<l, k為常數(shù)，一般設(shè)

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

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

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

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

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

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

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

19、姿態(tài)角的導(dǎo)數(shù)，對(duì)磁力計(jì)和加速度動(dòng)態(tài)測(cè)量的 誤差進(jìn)行補(bǔ)償，此時(shí)，在對(duì)自適應(yīng)ukf的狀態(tài)模 型進(jìn)行設(shè)計(jì)時(shí)，采用陀螺儀的數(shù)據(jù)，但是在k+1時(shí) 刻姿態(tài)角通過(guò)該時(shí)刻角速度在釆樣時(shí)間內(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)角，它是通過(guò)磁力計(jì)和 加速度進(jìn)行解算的，把這個(gè)解算出來(lái)的姿態(tài)角作為 自適應(yīng)ukf算法的

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

21、的誤差，通過(guò)反饋，能夠克服長(zhǎng)時(shí)間陀螺 的積累誤差。4仿真驗(yàn)證為了驗(yàn)證自適應(yīng)ukf算法的效果，實(shí)驗(yàn)對(duì)自 主研制的微型航姿系統(tǒng)進(jìn)行數(shù)據(jù)釆集，將算法運(yùn)用 到該系統(tǒng)中。采樣時(shí)間選擇30s,采樣頻率為50h乙 為了驗(yàn)證該算法的優(yōu)越性，將自適應(yīng)ukf算法和 普通的ukf算法進(jìn)行對(duì)比，分別做了靜態(tài)和動(dòng)態(tài) 實(shí)驗(yàn)。由于橫滾角和俯仰角在算法的原理上一樣， 為避免贅述，所以本實(shí)驗(yàn)主要對(duì)航向和俯仰進(jìn)行驗(yàn) 證。圖3分別是靜態(tài)和動(dòng)態(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動(dòng)態(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算法在動(dòng)靜態(tài)實(shí)驗(yàn)中都比標(biāo) 準(zhǔn)的ukf算法對(duì)誤差估計(jì)的精度要高。ukf不能 得到較好的估計(jì)性能；而自適應(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)（個(gè)）(c) 動(dòng)態(tài)俯仰角偏差3-2觥ukf施自駆mm 期鏗020340c6c010m1203 u0co!二匸【£蘇祓w(d) 動(dòng)態(tài)航向角偏差圖3動(dòng)靜態(tài)姿態(tài)角偏差本實(shí)驗(yàn)是使用加速度計(jì)，磁強(qiáng)計(jì)和陀螺儀，靜 態(tài)是直接把微型航姿系統(tǒng)放在雙軸轉(zhuǎn)臺(tái)上。動(dòng)態(tài)采 用的是搖擺實(shí)驗(yàn)，采用標(biāo)準(zhǔn)ukf和自適應(yīng)ukf算 法對(duì)傳感器數(shù)據(jù)融合后，靜態(tài)動(dòng)態(tài)仿真結(jié)果如圖(4) 所示：圖屮表明，自適應(yīng)ukf較標(biāo)準(zhǔn)u

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

25、111卅 15edlki訪法ij bgguff 施10001200 uof(d)動(dòng)態(tài)航向角 圖4動(dòng)靜態(tài)姿態(tài)角本論文針對(duì)傳統(tǒng)ukf因?yàn)榧俣ǖ南闰?yàn)噪聲與 實(shí)際系統(tǒng)噪聲不符導(dǎo)致精度下降，提出了一種基于 自適應(yīng)無(wú)跡卡爾曼濾波器(aukf),構(gòu)建以新的協(xié)方 差矩陣與相應(yīng)估計(jì)值誤差為成本函數(shù)，將更新的協(xié) 方差反饋到實(shí)測(cè)系統(tǒng)，對(duì)自主研制的微型航姿系統(tǒng) 的噪聲進(jìn)行在線估計(jì)，實(shí)驗(yàn)仿真表明，靜態(tài)精度提 高了 40%,動(dòng)態(tài)精度提高了 30%,驗(yàn)證了在線估計(jì) 模型的正確性和可行性。有很好的工程應(yīng)用價(jià)值。 參考文獻(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