四元數(shù)約束的容積卡爾曼濾波及其應(yīng)用

錢(qián)華明，黃 蔚，孫 龍

(哈爾濱工程大學(xué)自動(dòng)化學(xué)院，150001哈爾濱)

針對(duì)非線(xiàn)性系統(tǒng)狀態(tài)估計(jì)問(wèn)題，文獻(xiàn)［1］提出了一種新的非線(xiàn)性濾波，容積卡爾曼濾波(cubature kalman filter，CKF).其核心思想是針對(duì)帶高斯白噪聲的非線(xiàn)性系統(tǒng)，利用三階球面-相徑容積規(guī)則來(lái)求取非線(xiàn)性函數(shù)的統(tǒng)計(jì)特性，即均值和方差，估計(jì)精度在理論上能夠達(dá)到泰勒展開(kāi)的三階精度，算法優(yōu)點(diǎn)為實(shí)現(xiàn)簡(jiǎn)單、收斂性好以及精度高等.然而CKF的理論推導(dǎo)中，狀態(tài)變量都是假設(shè)不存在約束條件，但在實(shí)際應(yīng)用中，由于存在幾何約束、運(yùn)動(dòng)約束等問(wèn)題，使得狀態(tài)變量可能存在某些線(xiàn)性或非線(xiàn)性的約束條件，如車(chē)載導(dǎo)航系統(tǒng)可能受道路條件的約束，四元數(shù)存在規(guī)范化等.現(xiàn)有關(guān)于 CKF的文獻(xiàn)［2-8］，并未考慮狀態(tài)存在約束的情況.因此，討論狀態(tài)約束下的CKF設(shè)計(jì)方法，在實(shí)際應(yīng)用中具有重要意義.

在姿態(tài)估計(jì)領(lǐng)域，姿態(tài)描述參數(shù)主要有四元數(shù)、歐拉角、方向余弦以及修正羅德里格斯參數(shù)等.其中，四元數(shù)在計(jì)算量上相對(duì)較小，不存在三角函數(shù)以及超越函數(shù)方面的運(yùn)算，同時(shí)也沒(méi)有歐拉角描述姿態(tài)時(shí)存在的奇異性問(wèn)題，因此，以四元數(shù)作為姿態(tài)描述參數(shù)的非線(xiàn)性姿態(tài)估計(jì)濾波問(wèn)題在國(guó)內(nèi)外已成為熱點(diǎn)問(wèn)題［9-13］.由于四元數(shù)作為狀態(tài)變量時(shí)存在歸一化的約束，如果不考慮約束直接作濾波處理，濾波精度差，甚至?xí)?dǎo)致協(xié)方差奇異.文獻(xiàn)［14］提出重構(gòu)系統(tǒng)狀態(tài)參數(shù)，從而降低系統(tǒng)維數(shù)，避免了四元數(shù)約束.文獻(xiàn)［15］提出將羅德里格斯參數(shù)與四元數(shù)相切換，避免了四元數(shù)的規(guī)范化問(wèn)題.這些方法并沒(méi)有將約束條件與濾波相結(jié)合，而文獻(xiàn)［16］提出了約束下的卡爾曼濾波，將線(xiàn)性等式狀態(tài)約束與卡爾曼濾波相結(jié)合.由于四元數(shù)約束本質(zhì)上屬于非線(xiàn)性約束，因此，文獻(xiàn)［17］將其線(xiàn)性化作為線(xiàn)性等式約束與濾波結(jié)合，但是這種線(xiàn)性化引入了截?cái)嗾`差，影響濾波精度.文獻(xiàn)［18］提出兩步投影理論來(lái)解決非線(xiàn)性狀態(tài)等式約束情況.文獻(xiàn)［19］提出增益修正理論與擴(kuò)展卡爾曼濾波結(jié)合來(lái)解決非線(xiàn)性狀態(tài)約束問(wèn)題，但它是以擴(kuò)展卡爾曼濾波為主要框架.而本文從最優(yōu)濾波本質(zhì)出發(fā)，將CKF與增益修正理論相結(jié)合，利用四元數(shù)約束條件與最小均方誤差估計(jì)準(zhǔn)則構(gòu)造最小約束代價(jià)函數(shù)，從而能夠保證四元數(shù)滿(mǎn)足歸一化的條件.

因此，本文采用四元數(shù)作為姿態(tài)描述參數(shù)，針對(duì)其構(gòu)成的非線(xiàn)性姿態(tài)估計(jì)模型，提出了一種四元數(shù)約束下的CKF算法(quaternion constrained cubature kalman filter，QCCKF).利用三階球面 -相徑容積規(guī)則來(lái)近似計(jì)算系統(tǒng)狀態(tài)的后驗(yàn)均值和協(xié)方差，針對(duì)狀態(tài)變量存在四元數(shù)約束的問(wèn)題，采用最小約束代價(jià)函數(shù)來(lái)修正濾波增益，推導(dǎo)出了QCCKF算法的遞推公式.然后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了本文提出的QCCKF算法的有效性和正確性.

1 問(wèn)題描述

考慮非線(xiàn)性系統(tǒng)為

式中:xk和zk分別為系統(tǒng)n維狀態(tài)向量和m維量測(cè)向量;f(·)為系統(tǒng)方程中的非線(xiàn)性狀態(tài)函數(shù);h(·)則為量測(cè)方程中的非線(xiàn)性量測(cè)函數(shù);系統(tǒng)噪聲wk是高斯白噪聲，其均值為零，方差為Qk-1，而vk則為量測(cè)噪聲，其均值為零，方差為Rk，系統(tǒng)噪聲和量測(cè)噪聲互不相關(guān)．

上式為在量測(cè)數(shù)據(jù)z1，z2，…，zk已知的情況下?tīng)顟B(tài)向量xk的概率密度函數(shù)．針對(duì)式(1)的狀態(tài)假設(shè)，使得J(xk)最大的狀態(tài)向量值就是系統(tǒng)(1)的最優(yōu)狀態(tài)估計(jì)［20］．為解決這個(gè)問(wèn)題，可以采用一種次優(yōu)的方法去近似非線(xiàn)性函數(shù)的均值和方差，如無(wú)跡卡爾曼濾波(UKF)、中心差分濾波、CKF等.

但是由于狀態(tài)變量xk中存在四元數(shù)約束qTq=1．因此，需要對(duì)無(wú)約束條件下的濾波進(jìn)行

kk修正，使得在有四元數(shù)約束的情況下式(2)最大，從而得到最優(yōu)的狀態(tài)估計(jì)．問(wèn)題是在四元數(shù)約束下，如何采用基于最小均方誤差估計(jì)準(zhǔn)則來(lái)修正濾波增益，求四元數(shù)約束下的容積卡爾曼濾波公式．

2 QCCKF算法

2.1 三階球面-相徑容積規(guī)則

由于上面3個(gè)多元積分很難求得解析解，所以通常需要進(jìn)行近似計(jì)算.而對(duì)形如:Ⅰ=∫g(x)N(x;，Px)dx的積分函數(shù)，通常無(wú)法準(zhǔn)確求出其積分值，只能采用一些近似的方法，本文則采用三階球面 -相徑容積規(guī)則對(duì)該積分值進(jìn)行近似求?。饕那笕》椒ň褪抢脿顟B(tài)變量x的先驗(yàn)均值和方差信息，利用容積規(guī)則得到一系列帶權(quán)值的容積點(diǎn)，再將求得的容積點(diǎn)通過(guò)非線(xiàn)性函數(shù)f(·)進(jìn)行傳播得到相應(yīng)結(jié)果，最后利用得出的結(jié)果計(jì)算近似的積分值．

1)對(duì)Px做喬列斯基分解

2)求取容積點(diǎn) (i=1，2，…，m)

式中:m表示容積點(diǎn)總數(shù)，其為狀態(tài)維數(shù)的2倍，即m=2n;第i個(gè)容積點(diǎn)用ξi表示，該容積點(diǎn)的產(chǎn)生可以采用如下方式，假設(shè) e= ［1，0，…，0］T為單位向量，其維數(shù)為n，符號(hào)［1］被采用來(lái)表示對(duì)單位向量e中的元素實(shí)行全排列或是將其中的元素符號(hào)進(jìn)行改變所得到的點(diǎn)集，該點(diǎn)集是完整全對(duì)稱(chēng)的，符號(hào)［1］i則被描述為點(diǎn)集［1］中的第i個(gè)點(diǎn);ωi為對(duì)應(yīng)點(diǎn)的權(quán)值．

3)利用非線(xiàn)性函數(shù)f(·)對(duì)容積點(diǎn)αi進(jìn)行傳播，可得 γi=f(αi)．

2.2 QCCKF算法實(shí)現(xiàn)

引理1對(duì)于非線(xiàn)性系統(tǒng)(1)，基于貝葉斯估計(jì)的狀態(tài)估計(jì)以及狀態(tài)誤差協(xié)方差陣為

式中Kk∈Rn×m為濾波狀態(tài)增益陣，

該引理的詳細(xì)證明見(jiàn)參考文獻(xiàn)［1］.從引理1可以看出，求得狀態(tài)估計(jì)值以及狀態(tài)誤差協(xié)方差陣需要知道濾波增益Kk，在傳統(tǒng)的線(xiàn)性卡爾曼濾波以及非線(xiàn)性濾波如UKF、CKF中，利用最小均方誤差估計(jì)可以求出Kk=PxzP-1zz，從而得到狀態(tài)估計(jì)，但是這都未考慮到狀態(tài)變量存在約束的情況.現(xiàn)將狀態(tài)估計(jì)方程(3)分為四元數(shù)部分與非四元數(shù)部分:

式中 Kq∈Rnq×m，Kβ∈Rnβ×m，rk|k-1=zk-k|k-1．由于狀態(tài)變量中的四元數(shù)部分存在歸一化，即qTkqk=1，就會(huì)使得濾波增益中四元數(shù)部分Kq也存在約束條件，如:

濾波增益Kq存在約束條件，會(huì)影響濾波增益Kk的結(jié)果．因此，由最小均方誤差估計(jì)準(zhǔn)則，定義最小約束代價(jià)函數(shù)為

約束條件為

為求解上述問(wèn)題，給出引理2以及定理3.

引理2假設(shè)矩陣A、B、C、D中，A與D為可逆的方陣，則有

該引理的證明見(jiàn)參考文獻(xiàn)［21］.

式中:

證 明

式中:

構(gòu)造拉格朗日代價(jià)函數(shù):

式中λk為拉格朗日乘子，為使上式最小，對(duì)其求導(dǎo)可得

由式(12)、(13)可得

為了求出λk，利用引理2，將式(15)變形為

將式(17)代入到式(14)中得

求解可以得出

針對(duì)上述符號(hào)的選取，考慮到濾波的穩(wěn)定性以及(Pzz+λkrk|k-1rTk|k-1)的可逆性，λk取一個(gè)正數(shù)，即上述公式中取負(fù)號(hào):

利用定理3求得的濾波增益Kk帶入到式(3)和式(4)中，從而得到在四元數(shù)約束條件下的基于貝葉斯估計(jì)的狀態(tài)估計(jì)及狀態(tài)誤差協(xié)方差陣.

對(duì)于式(5)～(9)的積分，采用三階球面-相徑容積規(guī)則求取近似解，同時(shí)根據(jù)上述的兩個(gè)定理，得到四元數(shù)約束下的CKF濾波公式.濾波遞推公式如下:

1)已知k-1時(shí)刻狀態(tài)xk-1的統(tǒng)計(jì)特性為，求取容積點(diǎn)為

式中Pk-1=

2)容積點(diǎn)αi，k-1進(jìn)過(guò)非線(xiàn)性函數(shù)f(·)傳遞得

4)根據(jù)狀態(tài)xk的一步預(yù)測(cè)統(tǒng)計(jì)特性N(xk;k|k-1，Pk|k-1)，求取相應(yīng)的容積點(diǎn)為

式中Pk|k-1=

5)量測(cè)更新，經(jīng)過(guò)非線(xiàn)性函數(shù)h(·)傳遞得

7)根據(jù)式(18)～(20)，利用定理3求得濾波增益Kk，然后代入到引理1的式(3)和式(4)中求出狀態(tài)估計(jì)k|k和狀態(tài)誤差協(xié)方差陣Pk．

3 姿態(tài)估計(jì)模型

3.1 陀螺量測(cè)模型

若飛行器的質(zhì)心本體坐標(biāo)系與陀螺測(cè)量坐標(biāo)系相重合，則陀螺模型可以表示為

3.2 系統(tǒng)狀態(tài)方程

四元數(shù)乘法有不同的定義準(zhǔn)則，本文采用文獻(xiàn)［14］的四元數(shù)乘法定義準(zhǔn)則，則飛行器姿態(tài)運(yùn)動(dòng)學(xué)方程可表示為

由四元數(shù)表示的載體姿態(tài)矩陣為

A(q)=(q24- ρTρ)Ⅰ3×3+2ρρT-2q4［ρ ×］．將姿態(tài)四元數(shù)q(t)與陀螺漂移β(t)組成狀態(tài)向量，x(t)=［q(t)Tβ(t)T］T，建立四元數(shù)的非線(xiàn)性狀態(tài)方程為

式中

將式(23)進(jìn)行離散化，可得

式中:qk為k時(shí)刻的姿態(tài)四元數(shù);k為k時(shí)刻的陀螺量測(cè)輸出值;βk為k時(shí)刻的陀螺漂移值;Δt為采

樣周期;wk為零均值的高斯白噪聲序列，其方差根據(jù)文獻(xiàn)［9］為

3.3 系統(tǒng)量測(cè)方程

星敏感器的測(cè)量模型為

式中:zik為星敏感器的輸出;A(q(tk))為在tk時(shí)刻真實(shí)的姿態(tài)矩陣→■;ri為星敏感器的參考向量;vik為零均值的高斯白噪聲;i為取不同參考矢量所對(duì)應(yīng)的數(shù)字;為獲得姿態(tài)信息，至少需要2個(gè)不平行參考矢量的觀測(cè)值，則非線(xiàn)性量測(cè)模型為

式中:Zk為擴(kuò)維的量測(cè)向量;h(xk)為與狀態(tài)有關(guān)的非線(xiàn)性函數(shù);vk為零均值的高斯白噪聲，其方差為σ2sⅠ6×6．

4 仿真實(shí)驗(yàn)及分析

本文的姿態(tài)估計(jì)仿真平臺(tái)是由捷聯(lián)慣導(dǎo)和星敏感器組成，其主要模塊有軌跡發(fā)生器模塊、SINS模塊、星敏感器模塊以及濾波模塊.仿真實(shí)驗(yàn)中的初始參數(shù)按如下條件設(shè)置:假設(shè)飛行器在地球表面上的初始地理位置為東經(jīng)126°，北緯45°，飛行初始高度為0，初始航向角度為90°，初始速度為0，仿真實(shí)驗(yàn)時(shí)間設(shè)為600 s，陀螺量測(cè)噪聲設(shè)為σv=0.05(°)/h，陀 螺 漂 移 噪 聲 設(shè) 為 σu=星敏感器的測(cè)量噪聲標(biāo)準(zhǔn)差為σs=15″，其輸出頻率為2 Hz;陀螺漂移的初始值設(shè)為β=［111］T(°)/h，姿態(tài)角的初始時(shí)刻誤差設(shè)為［0.5 0.5 15］T(°)．同時(shí)，假設(shè)濾波模塊中姿態(tài)角與陀螺漂移的初始估計(jì)值均為零，與之對(duì)應(yīng)的初始方差陣分別設(shè)為 (0.2°)2Ⅰ3×3和(1.2(°)/h)2Ⅰ3×3．

為說(shuō)明本文算法的有效性，將本文提出的QCCKF算法與傳統(tǒng)CKF算法以及文獻(xiàn)［15］提出的USQUE算法進(jìn)行仿真對(duì)比與分析.首先，由軌跡發(fā)生器模塊模擬出一條飛行器的飛行軌跡，如圖1所示，滑跑、加速、爬高、轉(zhuǎn)彎等是飛行器的主要飛行動(dòng)作，然后利用SINS模塊以及星敏感器模塊產(chǎn)生飛行軌跡相應(yīng)的仿真測(cè)量信號(hào)，再通過(guò)濾波模塊進(jìn)行姿態(tài)估計(jì).圖2～4為仿真結(jié)果.

圖1 載體運(yùn)動(dòng)軌跡

圖2 橫滾角姿態(tài)誤差

圖3 俯仰角姿態(tài)誤差

圖4 偏航角姿態(tài)誤差

從圖中可以看出，QCCKF算法估計(jì)精度明顯優(yōu)于常規(guī)CKF以及USQUE算法.這是由于狀態(tài)變量存在四元數(shù)約束條件，導(dǎo)致濾波增益也存在約束，從而影響常規(guī)CKF的估計(jì)效果，而USQUE算法雖然將四元數(shù)與三維的修正羅德里格參數(shù)進(jìn)行來(lái)回切換，避開(kāi)了四元數(shù)存在歸一化約束的問(wèn)題，但它是以UKF算法為基礎(chǔ).

表1顯示的是通過(guò)100次的蒙特卡羅仿真，3種濾波算法的性能對(duì)比，可以看出QCCKF整體性能均優(yōu)于CKF和USQUE算法，同時(shí)滿(mǎn)足四元數(shù)約束條件.QCCKF算法相比較USQUE算法一次迭代時(shí)間減少約二分之一，雖然QCCKF算法狀態(tài)維數(shù)為n=7，其采樣點(diǎn)為14個(gè)，USQUE算法采用的是三維的誤差四元素，其狀態(tài)維數(shù)為n=6，則其采樣點(diǎn)為13個(gè)，因此從采樣點(diǎn)數(shù)目來(lái)看QCCKF算法的運(yùn)算效率略有下降，但是由于USQUE算法頻繁地在四元數(shù)與三維的修正羅德里格斯參數(shù)之間進(jìn)行切換，對(duì)姿態(tài)估計(jì)系統(tǒng)的實(shí)時(shí)性造成了嚴(yán)重影響.

表1 三種濾波算法性能對(duì)比

5 結(jié)語(yǔ)

常規(guī)CKF算法未考慮狀態(tài)變量存在狀態(tài)約束的情況，這樣就限制了常規(guī)CKF的應(yīng)用.針對(duì)其在狀態(tài)約束下存在濾波精度差的問(wèn)題，將四元數(shù)約束與常規(guī)CKF相結(jié)合，提出了一種四元數(shù)約束下的CKF算法.該算法基于貝葉斯估計(jì)理論，采用三階球面-相徑容積規(guī)則來(lái)近似計(jì)算系統(tǒng)狀態(tài)的后驗(yàn)均值和協(xié)方差，并將四元數(shù)約束轉(zhuǎn)化為增益約束，利用最小約束代價(jià)函數(shù)來(lái)修正濾波增益，從而得到了四元數(shù)約束下的CKF濾波公式.仿真結(jié)果表明，相比于CKF和USQUE算法，該算法具有更好的估計(jì)性能和更高的估計(jì)精度.

［1］ARASARATNAM I，HAYKIN S.Cubature Kalman filter［J］.IEEE Transactions on Automatic Control，2009，54(6):1254-1269.

［2］ARASARATNAM I，HAYKIN S，HURD T R.Cubature Kalman filtering for continuous-discrete systems:theory and simulations［J］.IEEE Transactions on Signal Processing，2010，58(10):4977-4993.

［3］PAKKI K，CHANDRA B，GU D W.Cubature information filter and its applications［C］//Proceedings of the 2011 American Control Conference. Piscataway:IEEE，2011:3609-3614.

［4］穆靜，蔡遠(yuǎn)利.迭代容積卡爾曼濾波算法及其應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，2011，33(7):1454-1457.

［5］穆靜，蔡遠(yuǎn)利.平方根容積卡爾曼濾波算法及其應(yīng)用［J］.兵工自動(dòng)化，2011，30(6):11-13.

［6］孫楓，唐李軍.Cubature粒子濾波［J］.系統(tǒng)工程與電子技術(shù)，2011，33(11):2554-2557.

［7］PESONEN H，PICHE R.Cubature-based Kalman filter for positioning［C］//Proceedings of the 7th Workshop on Positioning，Navigation and Communication.Dresden:WPNC，2010:45-49.

［8］錢(qián)華明，葛磊，黃蔚，等.基于貝葉斯估計(jì)噪聲相關(guān)下的CKF設(shè)計(jì)［J］.系統(tǒng)工程與電子技術(shù)，2012，34(11):2214-2218.

［9］CHOUKROUN D，BAR-ITZHACK I Y，OSHMAN Y.Novel quaternion Kalman filter［J］.IEEE Transactions on Aerospace and Electronic Systems，2006，42(1):174-190.

［10］CRASSIDIS J L，MARKLEY F L，CHENG Yang.Survey of nonlinear attitude estimation methods［J］.Journal of Guidance，Control，and Dynamics，2007，30(1):12-28.

［11］AHMADI M，KHAYATIAN A，KARIMAGHAEE P.Attitude estimation by divided difference filter in quaternion space ［J］.Acta Astronautica，2012，75(1):95-107.

［12］錢(qián)華明，黃蔚，孫龍，等.基于多重次漸消因子的強(qiáng)跟蹤UKF姿態(tài)估計(jì)［J］.系統(tǒng)工程與電子技術(shù)，2013，35(3):580-585.

［13］錢(qián)華明，黃蔚，葛磊，等.基于四元數(shù)平方根容積卡爾曼濾波的姿態(tài)估計(jì)［J］.北京航空航天大學(xué)學(xué)報(bào)，2013，39(5):645-649.

［14］MARKLEY F L.Attitude error representations for Kalman filtering ［J］.Journal of Guidance，Control，and Dynamics，2003，26(2):311-317.

［15］CRASSIDIS J L，MARKLEY F L.Unscented filtering for spacecraft attitude estimation ［J］.Journal of Guidance，Control，and Dynamics，2003，26(4):536-542.

［16］SIMON D，CHIA T.Kalman filtering with state equality constraints［J］.IEEE Transactions on Aerospace and Electronic Systems，2002，38(1):128-136.

［17］TEIXEIRA B O S，CHANDRASEKAR J，TORRES L，et al.State estimation for linear and nonlinear equality-constrained systems［J］.International Journal of Control，2009，82(5):918-936.

［18］JULIER S J，Jr LAVIOLA J J.On Kalman filtering with nonlinear equality constraints［J］.IEEE Transactions on Signal Processing，2007，55(6):2774-2784.

［19］ZANETTI R，MAJJI M，BISHOP R H，et al.Normconstrained Kalman filtering［J］.Journal of Guidance，Control，and Dynamics，2009，32(5):1458-1465.

［20］RAWLINGS J B，BAKSHI B R.Particle filtering and moving horizon estimation ［J］. Computers and Chemical Engineering，2006，30(10/11/12):1529-1541.

［21］SIMON D.Optimal state estimation:Kalman，H infinity，and nonlinear approaches［M］.New York:Wiley-Interscience，2006:11-12.