基于改進(jìn)的迭代容積卡爾曼濾波姿態(tài)估計(jì)

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

基于改進(jìn)的迭代容積卡爾曼濾波姿態(tài)估計(jì)

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

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

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

姿態(tài)估計(jì)；改進(jìn)的迭代容積卡爾曼濾波；容積數(shù)值積分理論；狀態(tài)擴(kuò)維；估計(jì)精度

針對(duì)星敏感器與陀螺儀組成的非線(xiàn)性姿態(tài)估計(jì)系統(tǒng)，基于非線(xiàn)性濾波的姿態(tài)估計(jì)方法［1-4］得到了廣泛的應(yīng)用.擴(kuò)展卡爾曼濾波（extended Kalman filter，EKF）由于其結(jié)構(gòu)簡(jiǎn)單、易于實(shí)現(xiàn)等優(yōu)點(diǎn)被廣泛應(yīng)用于姿態(tài)估計(jì)中.但是，EKF存在一定的局限性:1）由模型線(xiàn)性化引入的截?cái)嗾`差會(huì)導(dǎo)致濾波精度下降，且需要計(jì)算較為復(fù)雜的雅克比矩陣；2）當(dāng)初始狀態(tài)誤差較大或系統(tǒng)模型非線(xiàn)性程度較高時(shí)，會(huì)嚴(yán)重影響濾波精度甚至導(dǎo)致濾波發(fā)散.為了改善EKF估計(jì)性能，文獻(xiàn)［5］基于Gauss-Newton迭代理論，提出了迭代EKF（iterated extended Kalman filter，IEKF）算法，在量測(cè)過(guò)程中通過(guò)迭代步驟充分利用新的量測(cè)信息，降低非線(xiàn)性量測(cè)的影響，從而提高估計(jì)精度，但是它始終是以EKF為基礎(chǔ)，對(duì)狀態(tài)估計(jì)精度提高的程度有限.

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

1 非線(xiàn)性姿態(tài)估計(jì)模型

1.1 陀螺測(cè)量模型

假設(shè)陀螺的測(cè)量模型為

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

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

根據(jù)文獻(xiàn)［15］的四元數(shù)定義規(guī)則，姿態(tài)運(yùn)動(dòng)學(xué)方程為

式中:q=[q1q2q3q4]T=[ρTq4]T為姿態(tài)四元數(shù)；ρ為四元數(shù)向量部分；ω為真實(shí)角速率；［ω×］為由向量ω的分量構(gòu)成的反對(duì)稱(chēng)矩陣．

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

采用乘性誤差來(lái)定義誤差四元數(shù)為

式中:q為真實(shí)四元數(shù)；＾q為估計(jì)四元數(shù)；Δρ為誤差四元數(shù)的矢量部分．

令ω=＾ω+δω，其中＾ω為估計(jì)角速率，δω為誤差角速率，為陀螺漂移估計(jì)值，對(duì)式（3）進(jìn)行求導(dǎo)可得

式中:＾q-1為＾q的共軛四元數(shù)，由式（2）可知＾q·=＾q，結(jié)合四元數(shù)的基本性質(zhì)可得

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

利用四元數(shù)乘法，可以求出

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

將誤差四元數(shù)矢量部分Δρ與陀螺漂移估計(jì)Δβ組成狀態(tài)向量，x=［ΔρT，ΔβT］T，建立誤差四元數(shù)的非線(xiàn)性狀態(tài)方程為

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

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

為獲得姿態(tài)信息，至少需要兩個(gè)不平行參考矢量的觀測(cè)值，目前工程應(yīng)用中常采用3個(gè)不平行參考矢量的觀測(cè)值，則非線(xiàn)性量測(cè)模型為

式中:zk為擴(kuò)維的量測(cè)向量；h（xk）為與狀態(tài)有關(guān)的非線(xiàn)性函數(shù)；vk為均值等于零，方差為Rk的高斯白噪聲．

2 基于IICKF的姿態(tài)估計(jì)

2.1 ICKF濾波算法

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

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

采用容積數(shù)值積分理論來(lái)獲取基本的容積點(diǎn)與相應(yīng)的權(quán)值

式中:m為容積點(diǎn)總數(shù)，且m=2n；εj為第j個(gè)容積點(diǎn)；設(shè)n維單位向量e=［1，0，…，0］T，使用符號(hào)［1］表示對(duì)e的元素進(jìn)行全排列和改變?cè)胤?hào)所產(chǎn)生的點(diǎn)集，稱(chēng)為完整全對(duì)稱(chēng)點(diǎn)集，［1］j表示點(diǎn)集中［1］的第j個(gè)點(diǎn)；ωj為對(duì)應(yīng)點(diǎn)的權(quán)值．

ICKF算法步驟如下.

1）時(shí)間更新.容積點(diǎn)及容積點(diǎn)經(jīng)狀態(tài)方程的傳遞值為

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

狀態(tài)預(yù)測(cè)及方差預(yù)測(cè)為

首先計(jì)算第i次迭代的容積點(diǎn)及容積點(diǎn)經(jīng)量測(cè)方程的傳遞值為

計(jì)算量測(cè)預(yù)測(cè)、新息方差及協(xié)方差分別為

其次計(jì)算第i次迭代的濾波增益、狀態(tài)估計(jì)及方差估計(jì)分別為

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

設(shè)迭代終止時(shí)的迭代次數(shù)為N，則k時(shí)刻的狀態(tài)估計(jì)與方差估計(jì)為

2.2 IICKF算法的提出

2.2.1 問(wèn)題分析

從上述迭代容積卡爾曼濾波可以看出，在每一步迭代過(guò)程中，量測(cè)預(yù)測(cè)方差和互協(xié)方差分別為

在ICKF中，量測(cè)噪聲在每一步迭代中都是假設(shè)與狀態(tài)及狀態(tài)估計(jì)不相關(guān)，因此有

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

首先計(jì)算第i次迭代的容積點(diǎn)及容積點(diǎn)經(jīng)量測(cè)方程的傳遞值為

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

從上述方法可以看出，量測(cè)噪聲經(jīng)過(guò)擴(kuò)維參與整個(gè)運(yùn)算，能夠很好地解決狀態(tài)估計(jì)在更新迭代后與量測(cè)噪聲相關(guān)的問(wèn)題，提高估計(jì)精度，但是相對(duì)于ICKF，通過(guò)狀態(tài)擴(kuò)維將容積點(diǎn)從原來(lái)的2n個(gè)變?yōu)?（n+p）個(gè)，同時(shí)每次迭代都要進(jìn)行均方根運(yùn)算，引入了大的計(jì)算量，因此，為了在保證估計(jì)精度的前提下，降低計(jì)算量，本文提出一種通過(guò)容積點(diǎn)進(jìn)行迭代的策略，避免每步迭代中都采用均方根計(jì)算來(lái)產(chǎn)生容積點(diǎn)，這樣能夠提高量測(cè)更新的實(shí)時(shí)性．

2.2.2 容積點(diǎn)迭代策略

文獻(xiàn)［9-11，14］中建立的迭代更新函數(shù)都是以狀態(tài)為變量來(lái)進(jìn)行迭代的，通過(guò)高斯近似來(lái)求取采樣點(diǎn).為此，本文根據(jù)文獻(xiàn)［16］中無(wú)跡回歸非線(xiàn)性數(shù)據(jù)一致理論，建立關(guān)于容積點(diǎn)的迭代更新函數(shù)

式中:z為量測(cè)值；χj為需要求的迭代容積點(diǎn)．

迭代更新函數(shù)是關(guān)于2n個(gè)容積點(diǎn)的獨(dú)立最優(yōu)問(wèn)題，為了求解該問(wèn)題，采用Gauss-Newton迭代理論可以求得

則式（32）變換為

式（40）為量測(cè)方程線(xiàn)性化后得到的結(jié)果，這種線(xiàn)性化引入了高階的截?cái)嗾`差，如果量測(cè)方程的非線(xiàn)性程度較高，那么會(huì)引起較大的傳遞誤差，影響估計(jì)精度.因此需要采用精度較高的容積數(shù)值積分理論近似量測(cè)的方差Pizz以及協(xié)方差Pixz．式（41）為容積點(diǎn)迭代策略，可以看出容積點(diǎn)可以直接進(jìn)行迭代更新，而以往的迭代策略都是通過(guò)對(duì)狀態(tài)進(jìn)行迭代，然后通過(guò)高斯假設(shè)以及平方根的運(yùn)算來(lái)求取容積點(diǎn)，這樣就增加了計(jì)算量．

2.2.3 IICKF算法

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

1）時(shí)間更新．根據(jù)式（13）～（16）得出一步預(yù)測(cè)的狀態(tài)和方差

3）求取擴(kuò)維后的容積點(diǎn)

計(jì)算第i次迭代容積點(diǎn)經(jīng)量測(cè)方程的傳遞值為

計(jì)算第i次迭代的量測(cè)預(yù)測(cè)、新息方差及協(xié)方差分別為

計(jì)算第i次迭代的容積點(diǎn)以及方差分別為

4）迭代終止條件.通過(guò)容積點(diǎn)計(jì)算第i次迭代的狀態(tài)估計(jì)為

迭代終止條件為

設(shè)迭代終止時(shí)的迭代次數(shù)為N，則k時(shí)刻的狀態(tài)估計(jì)與方差估計(jì)為

2.3 IICKF姿態(tài)估計(jì)算法實(shí)現(xiàn)

設(shè)采樣時(shí)間為Δt，采用四階龍格庫(kù)塔法將式（10）、（11）所示的非線(xiàn)性連續(xù)系統(tǒng)離散化，有

則基于IICKF姿態(tài)估計(jì)算法具體流程如下.

1）設(shè)tk-1時(shí)刻系統(tǒng)的狀態(tài)估計(jì)值為x＾k-1以及狀態(tài)協(xié)方差陣Pk-1，則利用IICKF算法得到tk時(shí)刻的狀態(tài)估計(jì)x＾k和狀態(tài)協(xié)方差陣Pk．

3）再利用以上步驟，不斷循環(huán)，實(shí)時(shí)地估計(jì)系統(tǒng)狀態(tài)，并進(jìn)行校正，從而求得姿態(tài)及陀螺漂移信息.

3 仿真與分析

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

為驗(yàn)證算法的有效性，采用MEKF算法、文獻(xiàn)［15］的ICKF算法以及本文的IICKF算法進(jìn)行仿真實(shí)驗(yàn).由捷聯(lián)慣導(dǎo)工具箱生成一條載體的運(yùn)動(dòng)軌跡，如圖1所示，軌跡包括滑跑、加速、爬高、轉(zhuǎn)彎等，然后對(duì)應(yīng)該軌跡由星敏感器等產(chǎn)生相應(yīng)的仿真信號(hào)，進(jìn)行仿真.仿真結(jié)果見(jiàn)圖2～4.

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

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

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

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

由圖2～4可以看出，IICKF和ICKF的估計(jì)精度以及收斂速度始終高于MEKF算法，而IICKF算法與ICKF算法的收斂速度相當(dāng)，這是因?yàn)镮ICKF和ICKF算法都是以CKF算法為基礎(chǔ)，其精度以及收斂速度都要高于EKF算法.比較IICKF算法和ICKF算法，從表1可以看出，IICKF算法的估計(jì)精度要高于ICKF算法，這是由于ICKF在量測(cè)每步迭代后會(huì)造成狀態(tài)與量測(cè)噪聲相關(guān)，影響估計(jì)精度，而IICKF算法則是采用將噪聲與狀態(tài)擴(kuò)維，解決了量測(cè)迭代后狀態(tài)與量測(cè)噪聲相關(guān)的問(wèn)題，能夠提高估計(jì)精度.同時(shí)也可以看出IICKF算法一次迭代時(shí)間略高于ICKF算法，這是由于在量測(cè)更新階段，雖然采用了狀態(tài)與量測(cè)噪聲擴(kuò)維的方法，容積點(diǎn)由原來(lái)的2n個(gè)變?yōu)?（n+p）個(gè)，但是新的容積點(diǎn)迭代策略的應(yīng)用，避免了每次迭代都進(jìn)行均方根運(yùn)算，有利于提高實(shí)時(shí)性.

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

4 結(jié) 論

1）在分析現(xiàn)有迭代濾波策略存在問(wèn)題的基礎(chǔ)上，在量測(cè)更新中，用狀態(tài)擴(kuò)維理論解決每步迭代后存在狀態(tài)與量測(cè)噪聲相關(guān)的問(wèn)題，提出一種新的容積點(diǎn)迭代策略，直接進(jìn)行容積點(diǎn)的迭代，避免了傳統(tǒng)迭代策略中容積點(diǎn)都是通過(guò)高斯近似以及平方根運(yùn)算的局限，可以降低擴(kuò)維帶來(lái)的計(jì)算量.

2）通過(guò)將新的容積點(diǎn)迭代理論與CKF算法相結(jié)合，利用容積數(shù)值積分理論近似非線(xiàn)性函數(shù)的均值和方差，并將其應(yīng)用于由星敏感器和陀螺組成的姿態(tài)估計(jì)系統(tǒng)中，通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了提出的算法能夠有效地提高姿態(tài)估計(jì)的精度.

［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］喬相偉，周衛(wèi)東，吉宇人.用四元數(shù)狀態(tài)切換無(wú)跡卡爾曼濾波器估計(jì)的飛行器姿態(tài)［J］.控制理論與應(yīng)用，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］程水英，毛云祥.迭代無(wú)味卡爾曼濾波器［J］.數(shù)據(jù)采集與處理，2009，24（增1）:43-48.

［10］程水英，余莉.迭代無(wú)味卡爾曼濾波器的算法實(shí)現(xiàn)與應(yīng)用評(píng)價(jià)［J］.系統(tǒng)工程與電子技術(shù)，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］穆靜，蔡遠(yuǎn)利.迭代容積卡爾曼濾波算法及其應(yīng)用［J］.系統(tǒng)工程與電子技術(shù)，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.

國(guó)家自然科學(xué)基金資助項(xiàng)目（61104036）；哈爾濱市科技創(chuàng)新人才研究專(zhuān)項(xiàng)基金項(xiàng)目（RC2014XK009013）.

錢(qián)華明（1965—），男，教授，博士生導(dǎo)師.

黃 蔚，huangwei2393＠163.com.