四元數約束的容積卡爾曼濾波及其應用

錢華明，黃 蔚，孫 龍

(哈爾濱工程大學自動化學院，150001哈爾濱)

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

在姿態估計領域，姿態描述參數主要有四元數、歐拉角、方向余弦以及修正羅德里格斯參數等.其中，四元數在計算量上相對較小，不存在三角函數以及超越函數方面的運算，同時也沒有歐拉角描述姿態時存在的奇異性問題，因此，以四元數作為姿態描述參數的非線性姿態估計濾波問題在國內外已成為熱點問題［9-13］.由于四元數作為狀態變量時存在歸一化的約束，如果不考慮約束直接作濾波處理，濾波精度差，甚至會導致協方差奇異.文獻［14］提出重構系統狀態參數，從而降低系統維數，避免了四元數約束.文獻［15］提出將羅德里格斯參數與四元數相切換，避免了四元數的規范化問題.這些方法并沒有將約束條件與濾波相結合，而文獻［16］提出了約束下的卡爾曼濾波，將線性等式狀態約束與卡爾曼濾波相結合.由于四元數約束本質上屬于非線性約束，因此，文獻［17］將其線性化作為線性等式約束與濾波結合，但是這種線性化引入了截斷誤差，影響濾波精度.文獻［18］提出兩步投影理論來解決非線性狀態等式約束情況.文獻［19］提出增益修正理論與擴展卡爾曼濾波結合來解決非線性狀態約束問題，但它是以擴展卡爾曼濾波為主要框架.而本文從最優濾波本質出發，將CKF與增益修正理論相結合，利用四元數約束條件與最小均方誤差估計準則構造最小約束代價函數，從而能夠保證四元數滿足歸一化的條件.

因此，本文采用四元數作為姿態描述參數，針對其構成的非線性姿態估計模型，提出了一種四元數約束下的CKF算法(quaternion constrained cubature kalman filter，QCCKF).利用三階球面 -相徑容積規則來近似計算系統狀態的后驗均值和協方差，針對狀態變量存在四元數約束的問題，采用最小約束代價函數來修正濾波增益，推導出了QCCKF算法的遞推公式.然后通過仿真實驗驗證了本文提出的QCCKF算法的有效性和正確性.

1 問題描述

考慮非線性系統為

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

上式為在量測數據z1，z2，…，zk已知的情況下狀態向量xk的概率密度函數．針對式(1)的狀態假設，使得J(xk)最大的狀態向量值就是系統(1)的最優狀態估計［20］．為解決這個問題，可以采用一種次優的方法去近似非線性函數的均值和方差，如無跡卡爾曼濾波(UKF)、中心差分濾波、CKF等.

但是由于狀態變量xk中存在四元數約束qTq=1．因此，需要對無約束條件下的濾波進行

kk修正，使得在有四元數約束的情況下式(2)最大，從而得到最優的狀態估計．問題是在四元數約束下，如何采用基于最小均方誤差估計準則來修正濾波增益，求四元數約束下的容積卡爾曼濾波公式．

2 QCCKF算法

2.1 三階球面-相徑容積規則

由于上面3個多元積分很難求得解析解，所以通常需要進行近似計算.而對形如:Ⅰ=∫g(x)N(x;，Px)dx的積分函數，通常無法準確求出其積分值，只能采用一些近似的方法，本文則采用三階球面 -相徑容積規則對該積分值進行近似求取．主要的求取方法就是利用狀態變量x的先驗均值和方差信息，利用容積規則得到一系列帶權值的容積點，再將求得的容積點通過非線性函數f(·)進行傳播得到相應結果，最后利用得出的結果計算近似的積分值．

1)對Px做喬列斯基分解

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

式中:m表示容積點總數，其為狀態維數的2倍，即m=2n;第i個容積點用ξi表示，該容積點的產生可以采用如下方式，假設 e= ［1，0，…，0］T為單位向量，其維數為n，符號［1］被采用來表示對單位向量e中的元素實行全排列或是將其中的元素符號進行改變所得到的點集，該點集是完整全對稱的，符號［1］i則被描述為點集［1］中的第i個點;ωi為對應點的權值．

3)利用非線性函數f(·)對容積點αi進行傳播，可得 γi=f(αi)．

2.2 QCCKF算法實現

引理1對于非線性系統(1)，基于貝葉斯估計的狀態估計以及狀態誤差協方差陣為

式中Kk∈Rn×m為濾波狀態增益陣，

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

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

濾波增益Kq存在約束條件，會影響濾波增益Kk的結果．因此，由最小均方誤差估計準則，定義最小約束代價函數為

約束條件為

為求解上述問題，給出引理2以及定理3.

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

該引理的證明見參考文獻［21］.

式中:

證 明

式中:

構造拉格朗日代價函數:

式中λk為拉格朗日乘子，為使上式最小，對其求導可得

由式(12)、(13)可得

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

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

求解可以得出

針對上述符號的選取，考慮到濾波的穩定性以及(Pzz+λkrk|k-1rTk|k-1)的可逆性，λk取一個正數，即上述公式中取負號:

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

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

1)已知k-1時刻狀態xk-1的統計特性為，求取容積點為

式中Pk-1=

2)容積點αi，k-1進過非線性函數f(·)傳遞得

4)根據狀態xk的一步預測統計特性N(xk;k|k-1，Pk|k-1)，求取相應的容積點為

式中Pk|k-1=

5)量測更新，經過非線性函數h(·)傳遞得

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

3 姿態估計模型

3.1 陀螺量測模型

若飛行器的質心本體坐標系與陀螺測量坐標系相重合，則陀螺模型可以表示為

3.2 系統狀態方程

四元數乘法有不同的定義準則，本文采用文獻［14］的四元數乘法定義準則，則飛行器姿態運動學方程可表示為

由四元數表示的載體姿態矩陣為

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

式中

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

式中:qk為k時刻的姿態四元數;k為k時刻的陀螺量測輸出值;βk為k時刻的陀螺漂移值;Δt為采

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

3.3 系統量測方程

星敏感器的測量模型為

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

式中:Zk為擴維的量測向量;h(xk)為與狀態有關的非線性函數;vk為零均值的高斯白噪聲，其方差為σ2sⅠ6×6．

4 仿真實驗及分析

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

為說明本文算法的有效性，將本文提出的QCCKF算法與傳統CKF算法以及文獻［15］提出的USQUE算法進行仿真對比與分析.首先，由軌跡發生器模塊模擬出一條飛行器的飛行軌跡，如圖1所示，滑跑、加速、爬高、轉彎等是飛行器的主要飛行動作，然后利用SINS模塊以及星敏感器模塊產生飛行軌跡相應的仿真測量信號，再通過濾波模塊進行姿態估計.圖2～4為仿真結果.

圖1 載體運動軌跡

圖2 橫滾角姿態誤差

圖3 俯仰角姿態誤差

圖4 偏航角姿態誤差

從圖中可以看出，QCCKF算法估計精度明顯優于常規CKF以及USQUE算法.這是由于狀態變量存在四元數約束條件，導致濾波增益也存在約束，從而影響常規CKF的估計效果，而USQUE算法雖然將四元數與三維的修正羅德里格參數進行來回切換，避開了四元數存在歸一化約束的問題，但它是以UKF算法為基礎.

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

表1 三種濾波算法性能對比

5 結語

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

［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］穆靜，蔡遠利.迭代容積卡爾曼濾波算法及其應用［J］.系統工程與電子技術，2011，33(7):1454-1457.

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

［6］孫楓，唐李軍.Cubature粒子濾波［J］.系統工程與電子技術，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］錢華明，葛磊，黃蔚，等.基于貝葉斯估計噪聲相關下的CKF設計［J］.系統工程與電子技術，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］錢華明，黃蔚，孫龍，等.基于多重次漸消因子的強跟蹤UKF姿態估計［J］.系統工程與電子技術，2013，35(3):580-585.

［13］錢華明，黃蔚，葛磊，等.基于四元數平方根容積卡爾曼濾波的姿態估計［J］.北京航空航天大學學報，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.