基于改進的迭代容積卡爾曼濾波姿態估計

錢華明，黃 蔚，孫 龍

基于改進的迭代容積卡爾曼濾波姿態估計

錢華明，黃 蔚，孫 龍

（哈爾濱工程大學自動化學院，150001哈爾濱）

為了充分利用新的量測信息，提高姿態估計的精度，在分析現有迭代濾波策略存在問題的基礎上，采用一種新的容積點迭代策略，將其與容積卡爾曼濾波算法相結合，提出了一種改進的迭代容積卡爾曼濾波（improved iterated cubature Kalman filter，IICKF）算法.該算法采用容積數值積分理論近似非線性函數的均值與方差，利用狀態擴維理論來解決量測迭代中量測噪聲與狀態相關的問題，同時利用一種新的容積點迭代策略，即在量測迭代過程中直接采用容積點迭代，避免每步迭代都進行均方根計算來產生容積點，克服傳統迭代策略是基于高斯近似產生采樣點的局限，有效地降低擴維帶來的計算量.仿真結果表明：該算法的估計精度高于乘性擴展卡爾曼濾波（multiplicative extended Kalman filter，MEKF）以及迭代容積卡爾曼濾波（iterated cubature Kalman filter，ICKF）算法，該算法的提出有助于提高姿態估計的精度.

姿態估計；改進的迭代容積卡爾曼濾波；容積數值積分理論；狀態擴維；估計精度

針對星敏感器與陀螺儀組成的非線性姿態估計系統，基于非線性濾波的姿態估計方法［1-4］得到了廣泛的應用.擴展卡爾曼濾波（extended Kalman filter，EKF）由于其結構簡單、易于實現等優點被廣泛應用于姿態估計中.但是，EKF存在一定的局限性:1）由模型線性化引入的截斷誤差會導致濾波精度下降，且需要計算較為復雜的雅克比矩陣；2）當初始狀態誤差較大或系統模型非線性程度較高時，會嚴重影響濾波精度甚至導致濾波發散.為了改善EKF估計性能，文獻［5］基于Gauss-Newton迭代理論，提出了迭代EKF（iterated extended Kalman filter，IEKF）算法，在量測過程中通過迭代步驟充分利用新的量測信息，降低非線性量測的影響，從而提高估計精度，但是它始終是以EKF為基礎，對狀態估計精度提高的程度有限.

為了克服EKF算法的局限性，有學者提出無跡卡爾曼濾波算法（unscented Kalman filter，UKF）［6］.UKF通過采用Sigma點近似非線性系統的均值與方差，相比于一階線性化近似的EKF，該算法避免了求取雅克比矩陣，且估計精度能夠達到二階.然而，UKF算法對于初始誤差較大和系統非線性程度高的情況，也存在與EKF類似的缺點.為此，文獻［7］采用與IEKF同樣的迭代理論，提出了一種迭代Sigma點卡爾曼濾波.文獻［8］提出另一種迭代UKF算法，通過使用更新的估計信息來產生新的Sigma點，然后采用與UKF同樣的量測更新策略.類似的算法在文獻［9-11］中均有報道，但是在這些迭代策略中，每步迭代時狀態都引入了量測噪聲，破壞了迭代過程狀態與噪聲不相關的假設，影響了估計精度，同時每次迭代都是通過狀態進行迭代，且都是通過高斯近似以及均方根的運算來產生Sigma點.文獻［12-13］提出了一種新的非線性濾波，容積卡爾曼濾波（cubature Kalman filter，CKF），其核心思想是針對非線性高斯系統，采用容積數值積分理論近似狀態的后驗均值和協方差，可以保證在理論上以三階多項式逼近任何非線性高斯狀態的后驗均值和方差，相比于UKF，在高維情況下，具有實現簡單，濾波精度高，收斂性好等優點.但同樣存在與UKF類似的問題.文獻［14］將Gauss-Newton迭代理論與CKF相結合，提出了一種迭代的容積卡爾曼濾波算法（iterated cubature Kalman filter，ICKF），但并未改變迭代策略中存在的問題.為此，本文以非線性程度較高的姿態估計系統為背景，提出了一種改進的迭代容積卡爾曼濾波算法（improved iterated cubature Kalman filter，IICKF）.通過分析以往迭代濾波中存在的迭代過程中狀態與噪聲相關的問題，提出采用狀態擴維方法解決該問題，該方法直接對容積點進行迭代更新，避免通過求取高斯近似和均方根的方法來產生容積點，能夠充分利用量測信息，從而提高對狀態的估計精度.仿真結果表明，IICKF估計精度要高于ICKF.

1 非線性姿態估計模型

1.1 陀螺測量模型

假設陀螺的測量模型為

式中:ω?（t）為陀螺的量測輸出值；ω（t）為陀螺的真實角速率；β（t）為陀螺漂移；ηv（t）、ηu（t）為互不相關的零均值白噪聲，均方差分別為

1.2 系統狀態方程

根據文獻［15］的四元數定義規則，姿態運動學方程為

式中:q=[q1q2q3q4]T=[ρTq4]T為姿態四元數；ρ為四元數向量部分；ω為真實角速率；［ω×］為由向量ω的分量構成的反對稱矩陣．

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

采用乘性誤差來定義誤差四元數為

式中:q為真實四元數；＾q為估計四元數；Δρ為誤差四元數的矢量部分．

令ω=＾ω+δω，其中＾ω為估計角速率，δω為誤差角速率，為陀螺漂移估計值，對式（3）進行求導可得

式中:＾q-1為＾q的共軛四元數，由式（2）可知＾q·=＾q，結合四元數的基本性質可得

將式（2）、（5）及ω=＾ω+δω代入式（4）可得

利用四元數乘法，可以求出

令陀螺漂移估計誤差為Δβ，則δω=ω-＾ω= -Δβ-ηv，因此可以求出

將誤差四元數矢量部分Δρ與陀螺漂移估計Δβ組成狀態向量，x=［ΔρT，ΔβT］T，建立誤差四元數的非線性狀態方程為

1.3 系統量測方程

星敏感器的測量模型為

為獲得姿態信息，至少需要兩個不平行參考矢量的觀測值，目前工程應用中常采用3個不平行參考矢量的觀測值，則非線性量測模型為

式中:zk為擴維的量測向量；h（xk）為與狀態有關的非線性函數；vk為均值等于零，方差為Rk的高斯白噪聲．

2 基于IICKF的姿態估計

2.1 ICKF濾波算法

考慮非線性系統為

式中:xk、zk分別為系統n維狀態向量和p維量測向量；f（·）、h（·）分別為系統非線性狀態函數和量測函數；系統噪聲wk-1與量測噪聲vk分別是均值為零，協方差為Qk-1和Rk的互不相關的高斯白噪聲．

采用容積數值積分理論來獲取基本的容積點與相應的權值

式中:m為容積點總數，且m=2n；εj為第j個容積點；設n維單位向量e=［1，0，…，0］T，使用符號［1］表示對e的元素進行全排列和改變元素符號所產生的點集，稱為完整全對稱點集，［1］j表示點集中［1］的第j個點；ωj為對應點的權值．

ICKF算法步驟如下.

1）時間更新.容積點及容積點經狀態方程的傳遞值為

式中:Pk-1=Sk-1（Sk-1）T．

狀態預測及方差預測為

首先計算第i次迭代的容積點及容積點經量測方程的傳遞值為

計算量測預測、新息方差及協方差分別為

其次計算第i次迭代的濾波增益、狀態估計及方差估計分別為

最后迭代終止條件.迭代終止條件為

設迭代終止時的迭代次數為N，則k時刻的狀態估計與方差估計為

2.2 IICKF算法的提出

2.2.1 問題分析

從上述迭代容積卡爾曼濾波可以看出，在每一步迭代過程中，量測預測方差和互協方差分別為

在ICKF中，量測噪聲在每一步迭代中都是假設與狀態及狀態估計不相關，因此有

而從狀態迭代方程（23）可以看出，在每一次迭代后，狀態估計值都加入了量測噪聲信息，再將其用于下一步迭代求解量測預測方差及互協方差，就會使得式（28）～（30）不成立，會影響最后的估計精度．因此，為解決這個問題，可以采用狀態擴維理論，在量測迭代更新前，將預測狀態與量過程如下．

首先計算第i次迭代的容積點及容積點經量測方程的傳遞值為

然后計算第i次迭代的濾波增益、狀態估計及方差估計分別為

從上述方法可以看出，量測噪聲經過擴維參與整個運算，能夠很好地解決狀態估計在更新迭代后與量測噪聲相關的問題，提高估計精度，但是相對于ICKF，通過狀態擴維將容積點從原來的2n個變為2（n+p）個，同時每次迭代都要進行均方根運算，引入了大的計算量，因此，為了在保證估計精度的前提下，降低計算量，本文提出一種通過容積點進行迭代的策略，避免每步迭代中都采用均方根計算來產生容積點，這樣能夠提高量測更新的實時性．

2.2.2 容積點迭代策略

文獻［9-11，14］中建立的迭代更新函數都是以狀態為變量來進行迭代的，通過高斯近似來求取采樣點.為此，本文根據文獻［16］中無跡回歸非線性數據一致理論，建立關于容積點的迭代更新函數

式中:z為量測值；χj為需要求的迭代容積點．

迭代更新函數是關于2n個容積點的獨立最優問題，為了求解該問題，采用Gauss-Newton迭代理論可以求得

則式（32）變換為

式（40）為量測方程線性化后得到的結果，這種線性化引入了高階的截斷誤差，如果量測方程的非線性程度較高，那么會引起較大的傳遞誤差，影響估計精度.因此需要采用精度較高的容積數值積分理論近似量測的方差Pizz以及協方差Pixz．式（41）為容積點迭代策略，可以看出容積點可以直接進行迭代更新，而以往的迭代策略都是通過對狀態進行迭代，然后通過高斯假設以及平方根的運算來求取容積點，這樣就增加了計算量．

2.2.3 IICKF算法

將容積點的迭代策略、擴維理論以及CKF算法相結合，提出了一種精度更高的IICKF算法，算法步驟如下.

1）時間更新．根據式（13）～（16）得出一步預測的狀態和方差

3）求取擴維后的容積點

計算第i次迭代容積點經量測方程的傳遞值為

計算第i次迭代的量測預測、新息方差及協方差分別為

計算第i次迭代的容積點以及方差分別為

4）迭代終止條件.通過容積點計算第i次迭代的狀態估計為

迭代終止條件為

設迭代終止時的迭代次數為N，則k時刻的狀態估計與方差估計為

2.3 IICKF姿態估計算法實現

設采樣時間為Δt，采用四階龍格庫塔法將式（10）、（11）所示的非線性連續系統離散化，有

則基于IICKF姿態估計算法具體流程如下.

1）設tk-1時刻系統的狀態估計值為x＾k-1以及狀態協方差陣Pk-1，則利用IICKF算法得到tk時刻的狀態估計x＾k和狀態協方差陣Pk．

3）再利用以上步驟，不斷循環，實時地估計系統狀態，并進行校正，從而求得姿態及陀螺漂移信息.

3 仿真與分析

本文以捷聯慣導／星敏感器姿態估計系統為平臺，主要包括航跡仿真、捷聯慣性導航系統、星敏感器仿真、濾波系統等.仿真初始條件設置:假設載體初始位置為北緯45°，東經126°，高度為0，初始航向角為90°，初始橫滾角和俯仰角都為0°，速度為0；仿真時間為600 s；陀螺測量噪聲σv= 0.05°／h，陀螺漂移噪聲σu=0.003°／h，輸出頻率為100 Hz；星敏感器測量噪聲σs=18″，輸出頻率為1 Hz；初始陀螺漂移β=［1°1°1°］T／h；初始姿態角誤差設定為［0.5°0.5°15°］T；初始姿態與陀螺漂移的估計值均設為0；初始方差陣分別設為（0.2°）2I3×3和（1.2°／h）2I3×3．

為驗證算法的有效性，采用MEKF算法、文獻［15］的ICKF算法以及本文的IICKF算法進行仿真實驗.由捷聯慣導工具箱生成一條載體的運動軌跡，如圖1所示，軌跡包括滑跑、加速、爬高、轉彎等，然后對應該軌跡由星敏感器等產生相應的仿真信號，進行仿真.仿真結果見圖2～4.

圖1 載體運動軌跡

圖2 橫滾角姿態誤差

圖3 俯仰角姿態誤差

圖4 偏航角姿態誤差

由圖2～4可以看出，IICKF和ICKF的估計精度以及收斂速度始終高于MEKF算法，而IICKF算法與ICKF算法的收斂速度相當，這是因為IICKF和ICKF算法都是以CKF算法為基礎，其精度以及收斂速度都要高于EKF算法.比較IICKF算法和ICKF算法，從表1可以看出，IICKF算法的估計精度要高于ICKF算法，這是由于ICKF在量測每步迭代后會造成狀態與量測噪聲相關，影響估計精度，而IICKF算法則是采用將噪聲與狀態擴維，解決了量測迭代后狀態與量測噪聲相關的問題，能夠提高估計精度.同時也可以看出IICKF算法一次迭代時間略高于ICKF算法，這是由于在量測更新階段，雖然采用了狀態與量測噪聲擴維的方法，容積點由原來的2n個變為2（n+p）個，但是新的容積點迭代策略的應用，避免了每次迭代都進行均方根運算，有利于提高實時性.

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

4 結 論

1）在分析現有迭代濾波策略存在問題的基礎上，在量測更新中，用狀態擴維理論解決每步迭代后存在狀態與量測噪聲相關的問題，提出一種新的容積點迭代策略，直接進行容積點的迭代，避免了傳統迭代策略中容積點都是通過高斯近似以及平方根運算的局限，可以降低擴維帶來的計算量.

2）通過將新的容積點迭代理論與CKF算法相結合，利用容積數值積分理論近似非線性函數的均值和方差，并將其應用于由星敏感器和陀螺組成的姿態估計系統中，通過仿真實驗驗證了提出的算法能夠有效地提高姿態估計的精度.

［1］LEFFERTS E J，MARKLEY F L，SHUSTER M D. Kalman filtering for spacecraft attitude estimation［J］. Journal of Guidance Control and Dynamics，1982，5（5）:417-429.

［2］AHMADI M，KHAYATIAN A，KARIMAGHAEE P. Attitude estimationbydivideddifferencefilterin quaternion space［J］.Acta Astronautica，2012，75（1）:95-107.

［3］TANG Xiaojun，LIU Zhenbao，ZHANG Jiasheng. Square-root quaternion cubature Kalman filtering for spacecraft attitude estimation［J］.Acta Astronautica，2012，76（1）:84-94.

［4］喬相偉，周衛東，吉宇人.用四元數狀態切換無跡卡爾曼濾波器估計的飛行器姿態［J］.控制理論與應用，2012，29（1）:97-103.

［5］BELL B M，CATHEY F W.The iterated Kalman filter update as a Guass-Newton method［J］.IEEE Trans on Automatic Control，1993，38（2）:294-297.

［6］JULIER S J，UHLMANN J K，DURRANT-WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators［J］. IEEE Transactions on Automatic Control，2000，45（3）:477-482

［7］SIBLEY G，SUKHATME G，MATTHIES L.The iterated sigma point Kalman filter with applications to long range stereo［C］／／Proceedings of the Robotics:Science and Systems.Philadelphia:［s.n.］，2006.

［8］ZHAN Ronghui，WAN Jianwei.Iterated unscented Kalman filter for passive target tracking［J］.IEEE Tran on Aerospace and Electronic Systems，2007，43（3）: 1155-1163.

［9］程水英，毛云祥.迭代無味卡爾曼濾波器［J］.數據采集與處理，2009，24（增1）:43-48.

［10］程水英，余莉.迭代無味卡爾曼濾波器的算法實現與應用評價［J］.系統工程與電子技術，2011，33（11）:2546-2553.

［11］WANG Changyuan，ZHANG Jing，MU Jing.Maximum likelihood-based iterated divided difference filter for nonlinear systems from discrete noisy measurements［J］.Sensors，2012，12（1）:8912-8919.

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

［13］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.

［14］穆靜，蔡遠利.迭代容積卡爾曼濾波算法及其應用［J］.系統工程與電子技術，2011，33（7）:1454-1457.

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

［16］VACHHANI P，NARASIMHAN S，RENGASWAMY R. Robust and reliable estimation via unscented recursive nonlinear dynamic data reconciliation［J］.Journal of Process Control，2006，16（1）:1075-1086.

（編輯 魏希柱）

Attitude estimation based on improved iterated cubature Kalman filter

QIAN Huaming，HUANG Wei，SUN Long
（College of Automation，Harbin Engineering University，150001 Harbin，China）

To make use of the latest measurement information sufficiency，and to improve the accuracy of attitude estimation，based on the analysis of the current iterated filtering strategy，an improved iterated cubature Kalman filter（IICKF）is presented in this paper by combining a new cubature points iterated strategy with cubature Kalman filter.The filtering algorithm uses the cubature numerical integration theory to calculate the mean and variance of the nonlinear function，utilizing the state augmented method to solve the issue that the state is correlated with the measurement noise in the iterated process.A new cubature points iterated strategy is developed，which can directly iterate the cubature points，and thus avoids to generate cubature points by calculating the mean-squared root.It overcomes the limitation that sampling points are produced by the Gauss approximation in the traditional iterative strategy，which can reduce computational complexity. Simulation results show that IICKF is superior to multiplicative extended Kalman filter and iterated cubature Kalman filter in precision，which indicates that it can help to improve the accuracy of attitude estimation.

attitude estimation；improved iterated cubature Kalman filter；cubature numerical integration theory；state augmented method；precision

U666.12

A

0367-6234（2014）06-0116-07

2013-05-21；

2014-05-20.

國家自然科學基金資助項目（61104036）；哈爾濱市科技創新人才研究專項基金項目（RC2014XK009013）.

錢華明（1965—），男，教授，博士生導師.

黃 蔚，huangwei2393＠163.com.