張召友，郝燕玲，吳 旭

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

卡爾曼濾波是實現(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)非線性度增加，此時線性卡爾曼濾波已不再適用，而擴展卡爾曼濾波(extended Kalman filter，EKF)也難以保證系統(tǒng)性能.隨著近年非線性濾波的快速發(fā)展，較EKF精度更高的確定性采樣卡爾曼濾波被用于處理組合導(dǎo)航中的非線性融合問題.確定性采樣卡爾曼濾波是一類利用有限固定采樣點對高斯系統(tǒng)中狀態(tài)的一二階矩進行近似，并經(jīng)過非線性函數(shù)傳播后進行加權(quán)求和估計的濾波算法.最為常用的3種確定性采樣算法為［3］無跡卡爾曼濾波(unscented Kalman filter，UKF)、中心差分卡爾曼濾波(central difference Kalman filter，CDKF)、容積卡爾曼濾波(cubature Kalman filter，CKF)算法.

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

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

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

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

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

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

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

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

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

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

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

總結(jié)以上分析，CKF的復(fù)雜度fCKF總計為

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

CDKF的復(fù)雜度fCDKF總計為

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

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

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

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

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

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

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

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

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

2 仿真實驗與分析

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

狀態(tài)方程選取大方位失準(zhǔ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)為位置誤差，ε和▽分別為陀螺和加速度計常值漂移，bt和dt分別為時鐘偏置和漂移.

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

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

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

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

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

圖4 載體位置誤差

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

3 結(jié)論

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

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

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é)報，2012:44(7):135-138.

［3］王小旭，潘泉，黃鶴，等.非線性系統(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］趙恒，蘇永清，葉萍.快速傳遞對準(zhǔn)濾波器設(shè)計及其計算復(fù)雜度分析［J］.彈箭與制導(dǎo)學(xué)報，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.