H∞魯棒自適應CKF算法在組合導航中的應用

梁新宇，吳建德，黃國勇，孫 磊

LIANG Xinyu1，2,WU Jiande1，2,HUANG Guoyong1，2,SUN Lei1，2

1.昆明理工大學 信息工程與自動化學院，昆明 650500

2.云南省礦物管道輸送工程技術研究中心，昆明 650500

1.Faculty of Information Engineering&Automation,Kunming University of Science and Technology,Kunming 650500,China

2.Engineering Research Center for Mineral Pipeline Transportation,Kunming 650500,China

1 引言

組合導航系統就是采用數據融合技術將各導航子系統以適當方式組合起來，取長補短，以達到提高系統精度和改善系統可靠性等目的。目前，工程上組合導航采用的數據融合方法主要是卡爾曼濾波（Kalman Filter，KF）和擴展卡爾曼濾波（Extended Kalman Filter，EKF）[1]。EKF以對非線性函數進行泰勒級數展開且只保留一階項的線性化方法，進而應用線性卡爾曼濾波框架進行遞推估計，因此EKF存在一階線性化近似精度偏低，需要計算雅克比矩陣以及要求非線性函數連續可微等自身無法克服的理論局限性[2]。為此，Julier等人基于“近似非線性函數的概率密度分布易于近似非線性函數本身”這一思路，提出了以UT變換來逼近最優狀態估計框架中非線性狀態后驗分布的估計方法——無跡卡爾曼濾波（Unscented Kalman Filter，UKF）[3]，在系統非線性程度較強的情況下，相比于傳統的EKF，UKF數值穩定性較強，濾波精度較高。但對于高維系統，UKF易出現協方差陣非正定的情況，導致濾波發散[4]。基于球面徑向規則的容積卡爾曼濾波（Cubature Kalman Filter，CKF）是UKF的特殊形式[2]，是2009年由加拿大學者Arasaratnam等人[5-6]提出的貝葉斯近似非線性濾波算法，應用球面徑向容積準則來逼近最優框架中的狀態后驗分布，進行相應的數值積分運算。在系統處于任何非線性程度下，球面徑向容積規則都能以較高精度逼近非線性系統函數的狀態后驗均值及協方差，因此，CKF的精度高于通常的EKF，尤其適用于系統非線性程度較強的狀態估計問題，且無需計算非線性函數的雅可比矩陣。由于CKF中各積分點權值相同且為正數，因此其數值穩定性優于UKF[7]，更適用于解決高維組合導航系統的狀態估計問題，且CKF能夠更精確地保留系統的一二階矩信息，具有更高的濾波精度[8]。然而CKF在迭代過程中由于計算機的舍入誤差等原因易出現協方差陣的非正定性，進而導致計算發散的問題，且在系統非線性程度較強的情況下，由于系統狀態模型及噪聲統計特性的不確定性，也會對濾波效果產生嚴重影響，導致系統魯棒性降低，甚至出現濾波故障。

近年來，許多學者針對CKF存在的濾波發散的問題進行了大量的研究。其中，文獻[9]在標準CKF的基礎上，引入了矩陣的QR分解，采用平方根迭代的思想，結合強跟蹤濾波的理論框架，提出了一種自適應強跟蹤容積卡爾曼濾波算法，有效地改善了CKF的濾波性能；文獻[10]則將H∞魯棒濾波思想應用于標準的CKF，提出了一種基于CKF的新型魯棒濾波算法，并用于列車組合定位中，驗證了該結合方法對于改善濾波性能的有效性；文獻[11]針對實際應用中，由于約束條件γ的選取較小而導致Riccati不等式無解的情況，采用奇異值分解的方法，放寬了對約束條件γ的選取，在一定程度上改善了濾波的穩定性；文獻[12]基于新息協方差匹配原理構建的魯棒CKF，通過定義數據質量檢測函數，根據測量數據質量選擇魯棒CKF或標準CKF作為子系統的最優濾波算法，仿真結果表明，該方法在一定程度上提高了導航子系統的魯棒性。

對此，本文基于簡化的CKF濾波算法，引入數值穩定性較強的奇異值分解方法，對系統狀態協方差陣進行分解迭代，保證了算法中矩陣計算的數值穩定性；并結合H∞濾波思想，基于矩陣不等式的理論，將約束條件γ的自適應選取方法用于該算法中，提出了一種H∞魯棒自適應CKF算法，在系統狀態模型及噪聲統計特性不確定的情況下，有效改善了濾波算法的穩定性，提高了系統的魯棒性及其自適應能力。

2 簡化的SVD-CKF算法

2.1 簡化的CKF算法

在GNSS/INS組合導航系統中，觀測方程一般是線性的，而在標準的CKF算法中，觀測方程是非線性的，非線性的量測更新計算復雜度高，不利于在硬件設備上實現。因此，當標準CKF算法用于線性觀測系統時，將線性的觀測方程帶入CKF算法中，經過簡化，可有效降低計算復雜度，便于算法在硬件設備中的實現。

故針對GNSS/INS組合導航，考慮如下離散時間非線性-線性動態系統：

式中，xk∈Rn為n×1維的系統狀態向量，zk∈Rm為m×1維的量測向量，f(?)為非線性函數，Hk為線性量測陣。wk-1和vk分別為狀態系統和觀測系統的均值為0，協方差為Qk-1和Rk的高斯白噪聲，且相互獨立。

當觀測方程為線性時，標準CKF算法中的量測更新部分可簡化為如下形式：

由上可知，經過簡化的量測更新部分都退化為標準卡爾曼濾波的表達式，得到了簡化的CKF算法，該方法是針對組合導航系統觀測方程為線性的特性進行簡化，在濾波精度上并未降低，滿足組合導航的要求。

2.2 簡化的SVD-CKF算法

標準CKF算法經過多次遞推，隨著濾波步數的增加，計算的舍入誤差逐漸累積，易使系統狀態協方差陣Pk失去正定性甚至失去對稱性，進而導致濾波發散。奇異值分解具有良好的數值穩定性，用其進行系統狀態協方差陣的分解迭代，能有效提高濾波算法的穩定性，改善組合導航非線性動態系統的濾波性能[13]。

基于奇異值分解且觀測方程為線性時，簡化的SVD-CKF算法步驟如下：

（1）計算點集和權值。三階球面徑向準則對應的點集和權值為：

其中，i=1,2,…,m，m表示基本容積點個數，三階球面徑向容積準則，容積點數是狀態維數的2倍，即m=2n，n為狀態維數；[1]表示完整全對稱點集，以n=2為例，[1]i表示如下點集中的第i個元素：

（2）時間更新。對簡化的CKF算法中的協方差陣進行奇異值分解，取平方根[14]，即：

計算基本容積點：

計算狀態傳播容積點：

計算狀態預測值：

計算狀態預測協方差陣：

（3）量測更新。對狀態預測協方差陣進行奇異值分解，即：

計算量測預測值：

簡化的CKF算法通過狀態預測協方差陣Pk/k-1求解增益矩陣Kk和系統狀態協方差陣Pk，而在SVDCKF算法中這樣求解不利于分解后的協方差矩陣迭代，故采用以下形式求解系統狀態協方差陣Pk和增益矩陣Kk[14]。

計算系統狀態協方差陣：

計算增益矩陣：

對系統狀態協方差陣Pk進行奇異值分解，得：Pk=，對進行cholesky分解，得則增益矩陣Kk的求解形式可進一步寫成[15]：

計算狀態更新值：

3 H∞魯棒自適應CKF算法

3.1 H∞濾波基本原理

H∞濾波針對系統模型及噪聲統計特性的不確定性，而引入H∞范數思想，使得從干擾輸入到濾波誤差輸出的H∞范數最小化，進而使系統在最壞干擾情況下的估計誤差實現最小[16]。

定義如下代價函數[17]：

其中，wk，vk表示系統噪聲和觀測噪聲，均為白噪聲且相互獨立，Qk，Rk為對應方差；x0為狀態初值，方差為P0為其估計值。

其余各項與此類似。

通常情況下，最優H∞濾波問題難以求解得到解析形式的解[17]，可以尋求次優迭代算法，設計一門限值γ，滿足，即如下Riccati不等式[18]：

其中，Pk為協方差矩陣，Hk為觀測矩陣，Lk為系數矩陣，取單位陣。在H∞濾波器中，約束條件γ控制狀態估計在最不利條件下的估計誤差，γ越小，系統的魯棒性越強；γ越大，H∞濾波的特性越接近于標準的卡爾曼濾波[18]。

3.2 約束條件γ的自適應選取

約束條件γ對濾波精度和魯棒性都有重要影響。通常參數γ根據工程實踐經驗初始設置為固定值，從而使濾波器性能具有一定的保守性，無法適應組合導航系統應用環境可能存在的變化，不能保證在估計誤差較小的同時系統仍具有較強的魯棒性[19]。因此，需對γ的取值進行自適應優化。

根據矩陣不等式相關理論及H∞魯棒濾波約束條件γ與rk之間存在的反比關系，確定γ值的自適應選取條件[19]。rk為濾波新息向量，如下式：

定理1[19]設A和B為2個n階Hermite矩陣，A＞0，B≥0，則。這里 λ1(A)表示 A的最大特征值。

根據定理1，由上述Riccati不等式得：γ2＞λ1(A)，其中定義，進而得γ的值為：

式中，β＞0為相關系數，與系統的實際情況有關，需通過實驗來確定。確定β后，系數α的值就僅與濾波新息相關。

圖1 H∞魯棒自適應CKF算法流程圖

3.3 H∞魯棒自適應CKF算法

當H∞濾波存在時，系統狀態協方差陣Pk滿足如下的遞推形式[20]：

用H∞濾波系統狀態協方差陣Pk的遞推式，替換簡化的SVD-CKF算法中的Pk，并進行約束條件γ值的自適應選取，進而計算Pk的更新式，即可構成H∞魯棒自適應CKF算法，流程圖如圖1所示。

該算法針對線性觀測方程組合導航系統，對標準CKF算法量測更新部分進行簡化，降低了計算復雜度；同時，采用數值穩定性較強的SVD方法，對系統狀態協方差矩陣進行分解迭代，改善了算法穩定性，但也額外增加了運算復雜度；進而，結合H∞濾波理論，并對約束條件進行了自適應選取，其中存在一系列矩陣求逆等運算過程，同樣也增加了算法的運算復雜度。在系統狀態模型及噪聲統計特性不確定的情況下，有效改善了濾波算法的穩定性，提高了系統的魯棒性及其自適應能力，總體上提升了組合導航系統的濾波精度。

4 數值仿真

假設固定翼飛機做機動飛行，先后完成爬升、變速、平飛和轉彎等飛行狀態。初始位置為東經107.002°，北緯29.498°，高度300 m；初始速度為50 m/s，方向正北。其中，陀螺常值漂移為0.1（°/h），一階馬爾可夫過程相關時間為3 600 s，加計常值誤差為1.0－4g，一階馬爾可夫過程相關時間為1 800 s。GNSS水平位置誤差均方根為10 m，高度誤差均方根20 m，速度誤差均方根0.1 m/s。INS初始水平位置誤差為50 m，高度誤差100 m，初始速度誤差0.5 m/s。GNSS采樣周期為1 s，INS采樣周期為0.02 s，濾波周期為1 s，仿真時間1 200 s。

濾波過程以慣導系統為主系統，用線性的GNSS系統對其進行閉環修正，采用標準CKF算法、H∞魯棒CKF算法和H∞魯棒自適應CKF算法分別進行仿真實驗。

系統狀態向量xk為：

式中，φE,φN,φU為INS輸出的姿態角，vE,vN,vU為東北天方向的速度，l,λ,h 為東北天方向的位置，εbx,εby,εbz為陀螺隨機漂移。

狀態方程為：

其中，f(?)為非線性函數，由捷聯慣導力學編排方程和姿態誤差方程得到，wk-1為系統噪聲。

量測向量zk為：

式中，vEG,vNG,vUG為GNSS輸出的東北天方向的速度，lG,λG,hG為東北天方向的位置。

觀測方程為：

其中，Hk=[I6×606×6]，vk為觀測噪聲。

實驗中，模擬飛機飛行過程如圖2。

圖2 真實航跡及算法導航航跡

圖3～圖5為三種算法的對比仿真結果，對比三種算法可以看出，H∞魯棒自適應CKF算法在姿態角度及東北天方向速度、位置的解算上均具有較高的精度，且相比于標準CKF算法和H∞魯棒CKF算法，能夠在特殊條件下表現出較好的穩定性和魯棒性。其中，在姿態角度誤差及東北天方向速度誤差的對比實驗中，雖然三種算法精度相當，但當系統在700～800 s時間內發生了爬升、轉彎等一系列強非線性飛行狀態時，標準CKF算法穩定性差，出現了較大的波動，而H∞魯棒自適應CKF算法則表現出了較強的濾波穩定性及魯棒性；在東北天方向位置誤差的比較中，H∞魯棒自適應CKF算法也表現出了同樣的效果，且總體精度優于其他兩種算法。統計三種算法的運行時間，其中標準CKF算法運行時間為16.055 6 s，H∞魯棒CKF算法運行時間為14.071 2 s，H∞魯棒自適應CKF算法運行時間為13.455 5 s。由此可見，H∞魯棒自適應CKF算法的運算復雜度并未增加。

對比三種算法的均方根誤差（RMSE），同樣可以看出H∞魯棒自適應CKF算法相比于其他兩種算法的優勢所在。其中，表1為三種算法東北天方向速度與位置RMSE比較。

圖3 姿態角度誤差比較

圖4 東北天方向速度誤差比較

圖5 東北天方向位置誤差比較

5 結束語

本文算法針對線性觀測方程組合導航系統，在對標準CKF算法量測更新部分進行簡化的基礎上，采用數值穩定性較強的奇異值分解方法，分解系統狀態協方差矩陣，優化迭代過程；同時，結合H∞濾波理論，并進行約束條件γ值的自適應選取，進而計算系統狀態協方差陣的更新式。在系統狀態模型及噪聲統計特性不確定的情況下，保證了濾波精度和算法迭代的穩定性。

將該算法應用于GNSS/INS組合導航系統的數值仿真實驗。結果表明，當系統發生大幅度爬升、轉彎等一系列強非線性飛行狀態時，本文算法在保證了濾波精度和算法穩定性的同時，有效提高了系統的魯棒性及其自適應能力。

由于在約束條件β的自適應選取部分，相關系數β的值需根據系統的實際情況通過實驗來確定。因此，研究實現γ值自適應選取的方法，進一步提高本文算法的自適應能力，是下一步需要解決的問題。

表1 三種算法東北天方向速度與位置RMSE比較

參考文獻：

[1]趙琳，王小旭，丁繼成，等.組合導航系統非線性濾波算法綜述[J].中國慣性技術學報，2009，17（1）：46-52.

[2]王小旭，潘泉，黃鶴，等.非線性系統確定采樣型濾波算法綜述[J].控制與決策，2012，27（6）：812-801.

[3]Julier S J，Uhlmann J K，Durrant-Whyte H F.A new method for the nonlinear transformation of means and covariances[J].IEEE Transactions on Automatic Control，2000，45（3）：477-482.

[4]Liu Xing，Jiang Shoushan.Research on target tracking based on unscented Kalman filter[J].Sensors&Transducers，2013，153（6）：13-21.

[5]Arasaratnam I，Haykin S.Cubature Kalman filters[J].IEEE Transactions on Automatic Control，2009，54（6）：1254-1269.

[6]Arasaratnam I，Haykin S，Hurd R T.Cubature Kalman filtering for continuous-discrete system：Theory and simulations[J].IEEE Transactions on Sigal Processing，2010，58（10）：4977-4993.

[7]崔乃剛，張龍，王小剛，等.自適應高階容積卡爾曼濾波在目標跟蹤中的應用[J].航空學報，2015，36（12）：3885-3895.

[8]Dai H D，Dai S W，Cong Y C，et al.Performance comparison of EKF/UKF/CKF for the tracking of ballistic target[J].Telkomnika Indonesian J of Electrical Engineering，2012，10（7）：1692-1699.

[9]趙利強，羅達燦，王建林，等.自適應強跟蹤容積卡爾曼濾波算法[J].北京化工大學學報，2013，40（3）：98-103.

[10]劉江，蔡伯根，唐濤，等.基于CKF的GNSS/INS列車組合定位魯棒濾波算法[J].交通運輸工程學報，2010，10（5）：102-107.

[11]張秋昭，張書畢，劉志平，等.基于奇異值分解的魯棒容積卡爾曼濾波及其在組合導航中的應用[J].控制與決策，2014，29（2）：341-346.

[12]徐樹生，林孝工.基于魯棒CKF的多傳感器全信息融合算法[J].電機與控制學報，2013，17（2）：90-97.

[13]高社生，王建超，焦雅琳.自適應SVD-UKF算法及在組合導航的應用[J].中國慣性技術學報，2010，18（6）：737-741.

[14]孫磊，黃國勇，李越.改進的強跟蹤SVD-UKF算法在組合導航中的應用[J].計算機工程與應用，2017，53（10）：225-229.

[15]張友民，戴冠中，張洪才.基于SVD的推廣卡爾曼濾波及其在飛行狀態和參數估計中的應用[J].控制理論與應用，1996，13（1）：106-114.

[16]Dai H，Li J X，Jin H M.Application of robust kalman filtering to integrated navigation based on inertial navigation system and dead reckoning[C]//International Conference on Artificial Intelligence and Computational Intelligence，Sanya，2010.

[17]侯代文，殷福亮，陳喆.基于sigma點H∞濾波的說話人跟蹤方法[J].信號處理，2009，25（3）：374-378.

[18]朱英，梁彥，楊猛，等.自適應補償H∞濾波器在組合導航中的應用[J].火力與指揮控制，2010，35（10）：15-19.

[19]劉曉光，胡靜濤，王鶴.基于自適應H∞濾波的組合導航方法研究[J].儀器儀表學報，2014，35（5）：1013-1021.

[20]李兆銘，楊文革，丁丹，等.基于SVD的多終端實時定軌自適應魯棒CKF算法[J].儀器儀表學報，2016，37（3）：490-496.