張召友，郝燕玲，吳 旭

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

卡爾曼濾波是實(shí)現(xiàn)捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system，SINS)和全球定位系統(tǒng)(global positioning system，GPS)組合導(dǎo)航數(shù)據(jù)融合的首選方法［1-2］.低精度傳感器構(gòu)成的組合系統(tǒng)在復(fù)雜應(yīng)用環(huán)境中易產(chǎn)生大的誤差，導(dǎo)致系統(tǒng)非線(xiàn)性度增加，此時(shí)線(xiàn)性卡爾曼濾波已不再適用，而擴(kuò)展卡爾曼濾波(extended Kalman filter，EKF)也難以保證系統(tǒng)性能.隨著近年非線(xiàn)性濾波的快速發(fā)展，較EKF精度更高的確定性采樣卡爾曼濾波被用于處理組合導(dǎo)航中的非線(xiàn)性融合問(wèn)題.確定性采樣卡爾曼濾波是一類(lèi)利用有限固定采樣點(diǎn)對(duì)高斯系統(tǒng)中狀態(tài)的一二階矩進(jìn)行近似，并經(jīng)過(guò)非線(xiàn)性函數(shù)傳播后進(jìn)行加權(quán)求和估計(jì)的濾波算法.最為常用的3種確定性采樣算法為［3］無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)、中心差分卡爾曼濾波(central difference Kalman filter，CDKF)、容積卡爾曼濾波(cubature Kalman filter，CKF)算法.

精度是表征濾波性能的重要指標(biāo)，大量文獻(xiàn)已證實(shí)UKF、CDKF、CKF的精度相當(dāng)，均至少可達(dá)二階［4-5］.但在導(dǎo)航系統(tǒng)中，濾波多是在嵌入式計(jì)算機(jī)中實(shí)現(xiàn)，因此復(fù)雜度是影響其應(yīng)用的另一個(gè)重要參數(shù).先前對(duì)于非線(xiàn)性濾波復(fù)雜度的研究多是對(duì)單一算法的改進(jìn)與分析［6-8］，對(duì)于形式相近的確定性采樣算法之間缺乏系統(tǒng)的分析與比較.因此，本文將對(duì)3種典型確定性采樣算法進(jìn)行等效復(fù)雜度的理論分析，導(dǎo)出定量的計(jì)算表達(dá)式，并進(jìn)行橫向比較，得出算法實(shí)時(shí)性選擇的依據(jù).最后，將確定性采樣算法應(yīng)用于SINS/GPS的緊耦合組合導(dǎo)航，對(duì)其精度和復(fù)雜度進(jìn)行仿真，驗(yàn)證分析的正確性.

1 確定性采樣濾波的等效復(fù)雜度分析

濾波的精度是保證系統(tǒng)性能的前提，但當(dāng)精度滿(mǎn)足要求后濾波的易實(shí)現(xiàn)性與否是另一個(gè)制約因素，復(fù)雜度是表征易實(shí)現(xiàn)性的主要參數(shù).本節(jié)將對(duì)UKF、CDKF、CKF的復(fù)雜度及差異進(jìn)行分析，力圖得出一個(gè)精確的、定量的表述.

1.1 確定性采樣濾波中數(shù)值運(yùn)算的復(fù)雜度

分析算法復(fù)雜度最為有效的方法是對(duì)其浮點(diǎn)操作數(shù)(flops)的準(zhǔn)確統(tǒng)計(jì).一次flops定義為兩個(gè)浮點(diǎn)數(shù)進(jìn)行一次加、減、乘或除法運(yùn)算.但濾波中的很多運(yùn)算難以用加、減、乘、除來(lái)簡(jiǎn)單描述，如開(kāi)方、數(shù)值分解、指數(shù)運(yùn)算等，因此只能將其等效為相同運(yùn)行時(shí)間的flops，故稱(chēng)為等效復(fù)雜度分析.本文給出濾波中基本代數(shù)運(yùn)算的flops次數(shù)［1］:1)矩陣加減法.A∈Rn×m，B∈Rn×m，計(jì)算A±B需nm次flops.2)矩陣相乘.A∈Rn×m，B∈Rm×l，計(jì)算AB的flops為2mnl-nl次.3)矩陣求逆.A∈Rn×n，A-1的flops數(shù)為n3.4)Cholesky分解.A∈Rn×n，則chol(A)需要進(jìn)行n3次flops.

利用上述諸元素即可對(duì)確定性采樣卡爾曼濾波進(jìn)行復(fù)雜度的分析.

1.2 確定性采樣濾波的等效復(fù)雜度分析

3種確定性采樣算法的濾波形式相近，均包括樣本點(diǎn)產(chǎn)生、基于狀態(tài)方程與量測(cè)方程的均值與方差預(yù)測(cè)以及狀態(tài)與方差的后驗(yàn)估計(jì)等步驟.算法實(shí)現(xiàn)形式均為加性噪聲形式，UKF與CDKF的詳細(xì)公式參見(jiàn)文獻(xiàn)［4］，CKF的公式參見(jiàn)文獻(xiàn)［5］.按照濾波過(guò)程可對(duì)相應(yīng)算法進(jìn)行分析.由于具體的分析過(guò)程較為繁瑣，在此只給出CKF算法的分析過(guò)程如下.

狀態(tài)方程預(yù)測(cè)，如下式，獲得樣本點(diǎn)預(yù)測(cè)χk｜k-1需要4n3-2n2次flops(3種算法均以線(xiàn)性矩陣形式代換)，獲得狀態(tài)預(yù)測(cè)值1需要2n2次flops，預(yù)測(cè)協(xié)方差Pk｜k-1需要 2n3+4n2次 flops.

量測(cè)方程預(yù)測(cè)，如下式，獲得樣本點(diǎn)預(yù)測(cè)γk｜k-1需要 4n2l-2nl次 flops，獲得量測(cè)預(yù)測(cè)值yk｜k-1需要2nl次flops，預(yù)測(cè)量測(cè)協(xié)方差Pyk需要4nl2+3l2次flops，互協(xié)方差Pxkyk需要4n2l+2nl次flops.

計(jì)算后驗(yàn)估計(jì)，如下式，增益Kk需l3+(2nl2-nl)次flops，協(xié)方差更新Pk需要2nl2-nl+2n2l次flops，狀態(tài)更新需要2nl+l次flops.

總結(jié)以上分析，CKF的復(fù)雜度f(wàn)CKF總計(jì)為

同理，可對(duì)UKF和CDKF進(jìn)行分析，濾波參數(shù)的具體分析結(jié)果如表1所示.由表中數(shù)據(jù)可計(jì)算得到UKF的復(fù)雜度f(wàn)UKF總計(jì)為

CDKF的復(fù)雜度f(wàn)CDKF總計(jì)為

表1 確定性采樣卡爾曼濾波復(fù)雜度分析結(jié)果

由3個(gè)表達(dá)式可看出，CDKF、UKF、CKF 3種算法的復(fù)雜度均為O(n3)和O(l3)量級(jí)，即與狀態(tài)維數(shù)和量測(cè)維數(shù)均呈三次方關(guān)系.

通過(guò)對(duì)3種算法總體復(fù)雜度系數(shù)項(xiàng)的直觀比較可知:fUKF＞fCDKF和fUKF＞fCKF恒成立，即UKF的復(fù)雜度始終最高.而CDKF和CKF的復(fù)雜度難以通過(guò)系數(shù)項(xiàng)直接看出，所以將兩者求差得

式(1)仍為狀態(tài)維數(shù)n的三次方形式，不便于分析.假設(shè)狀態(tài)維數(shù)n和量測(cè)維數(shù)l存在n-l=k的關(guān)系，其中k為整數(shù)，則式(1)可分別整理為關(guān)于n和l的二次項(xiàng)形式，即

當(dāng)0≤k＜n時(shí)，狀態(tài)維數(shù)大于量測(cè)維數(shù)，由式(2)可知Δflops為關(guān)于n的開(kāi)口向上的拋物線(xiàn)，且n=1時(shí)Δflops＞0，所以Δflops為狀態(tài)維數(shù)n的單增函數(shù)，即狀態(tài)維數(shù)大于量測(cè)維數(shù)時(shí)，fCDKF＞fCKF恒成立，且隨著狀態(tài)維數(shù)增大，CDKF與CKF的差異也增大.

當(dāng)k≤0時(shí)，量測(cè)維數(shù)大于狀態(tài)維數(shù)，由式(3)知Δflops為關(guān)于k的開(kāi)口向下的拋物線(xiàn)，所以隨著k的不斷減小，會(huì)出現(xiàn)Δflops＜0的情況，即量測(cè)維數(shù)與狀態(tài)維數(shù)差異的不斷加大，CKF的復(fù)雜度會(huì)高于CDKF，即fCKF＞fCDKF.具體差異可根據(jù)實(shí)際系統(tǒng)的維數(shù)及前面的復(fù)雜度計(jì)算式進(jìn)行計(jì)算得出.

為更為直觀的揭示三者之間的差異，根據(jù)復(fù)雜度計(jì)算式繪出了圖1、2所示的狀態(tài)和量測(cè)維數(shù)分別變化時(shí)3種算法復(fù)雜度的變化曲線(xiàn).

圖1 確定性采樣卡爾曼濾波復(fù)雜度(l=10)

圖2 確定性采樣卡爾曼濾波復(fù)雜度(n=40)

2 仿真實(shí)驗(yàn)與分析

為驗(yàn)證UKF、CDKF和CKF算法復(fù)雜度分析的正確性及在精度上的一致性，分別將其應(yīng)用于SINS/GPS的緊耦合導(dǎo)航中.

狀態(tài)方程選取大方位失準(zhǔn)歐拉角非線(xiàn)性模型，具體見(jiàn)文獻(xiàn)［9］，狀態(tài)量為:x=［δψ，δθ，δγ，δVE，δVN，δVU，δL，δλ，δh，εx，εy，εz，▽x，▽y，▽z，bt，dt］T.其中，(δψ，δθ，δγ)為姿態(tài)誤差，(δVE，δVN，δVU)為速度誤差，(δL，δλ，δh)為位置誤差，ε和▽分別為陀螺和加速度計(jì)常值漂移，bt和dt分別為時(shí)鐘偏置和漂移.

利用4顆衛(wèi)星的偽距與偽距率作為觀測(cè)量，則衛(wèi)星i的量測(cè)值為yi，k=［ρi，k，ρi，k］，ρi，k為偽距，ρi，k為偽距率.具體非線(xiàn)性量測(cè)方程參見(jiàn)文獻(xiàn)［10］.

綜上可知，狀態(tài)維數(shù)為n=17，量測(cè)維數(shù)為l=8.根據(jù)前面對(duì)復(fù)雜度的分析，l=8時(shí)有fUKF＞fCDKF＞fCKF.

仿真中載體分別經(jīng)過(guò)靜止、加速、爬升等階段.初始位置為(126.67°，45.78°，100 m).初始失準(zhǔn)角為(5°，5°，20°).SINS傳感器各參數(shù)為:陀螺常值漂移100°/h，隨機(jī)漂移0.3°/;加速度計(jì)零偏0.001g(g=9.780 3 m/s2)，隨機(jī)噪聲偽距率量測(cè)噪聲為0.1 m/s;偽距量測(cè)噪聲為10 m;時(shí)鐘偏差為100 m;時(shí)鐘漂移噪聲為0.1 m/s.

隨機(jī)產(chǎn)生20組數(shù)據(jù)進(jìn)行蒙特卡羅仿真.圖3、4分別給出了緊耦合的姿態(tài)和位置估計(jì)誤差曲線(xiàn)(20次仿真平均值).表2給出了確定性采樣濾波在SINS/GPS緊耦合導(dǎo)航中的估計(jì)精度(20組均方跟誤差的平均值).由圖3、4可知，基于確定性采樣原理的UKF、CDKF、CKF 3種算法均可使姿態(tài)和位置快速收斂，并且精度相近，只有可忽略的輕微不同.UKF、CDKF、CKF 3種算法的單次導(dǎo)航迭代耗時(shí)(20次仿真平均值)分別為7.23、6.92、6.74 ms.可見(jiàn)，UKF的復(fù)雜度最高，CDKF次之，CKF最低，與理論分析結(jié)果一致.

圖3 載體姿態(tài)誤差

圖4 載體位置誤差

表2 SINS/GPS緊耦合導(dǎo)航估計(jì)性能

3 結(jié)論

1)針對(duì)確定性采樣非線(xiàn)性濾波的實(shí)時(shí)性問(wèn)題，對(duì)UKF、CDKF和CKF 3種常用算法進(jìn)行了復(fù)雜度分析，導(dǎo)出了復(fù)雜度計(jì)算的表達(dá)式.

2)通過(guò)進(jìn)一步的差異性比較，表明3種算法中UKF復(fù)雜度最高;當(dāng)狀態(tài)維數(shù)高于量測(cè)維數(shù)時(shí)，CDKF的復(fù)雜度低于UKF的復(fù)雜度，CKF的復(fù)雜度最低;量測(cè)維數(shù)相對(duì)狀態(tài)維數(shù)較高時(shí)，CDKF的復(fù)雜度會(huì)低于CKF的復(fù)雜度，在諸如雷達(dá)測(cè)距［11］等高維量測(cè)系統(tǒng)中選取CDKF可獲得最小的硬件開(kāi)銷(xiāo).

3)將3種算法應(yīng)用于SINS/GPS的緊耦合導(dǎo)航中，仿真結(jié)果表明了分析結(jié)果的正確性.

［1］GREWAL M S，ANDREW A P.Kalman filtering，theory and practice using Matlab［M］.2nd ed.New York:John Wiley＆Sons，2008:225-289.

［2］姬曉琴，高曉穎.低軌衛(wèi)星緊組合導(dǎo)航UKF方法［J］.哈爾濱工業(yè)大學(xué)學(xué)報(bào)，2012:44(7):135-138.

［3］王小旭，潘泉，黃鶴，等.非線(xiàn)性系統(tǒng)確定采樣型濾波算法綜述［J］.控制與決策，2012，27(6):801-812.

［4］MERWE R V D.Sigma-point Kalman filter for probabilistic inference in dynamic state-space models［D］.Portland:Oregon Health＆Science University，2004:108-110.

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

［6］趙恒，蘇永清，葉萍.快速傳遞對(duì)準(zhǔn)濾波器設(shè)計(jì)及其計(jì)算復(fù)雜度分析［J］.彈箭與制導(dǎo)學(xué)報(bào)，2011，31(3):31-34.

［7］HOLMES S A，KLEIN G，MURRAY D W.An O(N2)square root unscented Kalman filter for visual simultaneous localization and mapping［J］.IEEE Transactions on Pattern Analysis and Machine Intelligence，2009，31(7):1251-1263.

［8］KARLSSONR，SCHONT，GUSTAFSSONF.Complexity analysis of the marginalized particle filter［J］.IEEE Transactions on Signal Processing，2005，53(11):4408-4411.

［9］ALI J，MIRZA M.Performance comparison among some nonlinear filters for a low cost SINS/GPS integrated solution［J］.Nonlinear Dynamics，2010，61(3):491-502.

［10］LI Y，RIZOS C，WANG J，et al.Sigma-point Kalman filtering for tightly coupled GPS/INS integration［J］.Journal of the Institute of Navigation，2008，55(3):167-177.

［11］MCMANUS C，BARFOOT T.A serial approach to handling high-dimensional measurements in the sigmapoint Kalman filter［C］//Proceedings of Robotics Science and Systems.Los Angeles，CA:MIT Press，2011.