EKF和UKF在非線性組合導航系統中的對比研究

吳春光，李雙明，潘玉純

（92941部隊，葫蘆島　125000）

?

EKF和UKF在非線性組合導航系統中的對比研究

吳春光，李雙明，潘玉純

（92941部隊，葫蘆島125000）

摘要：采用組合導航系統在對目標的位置、航向進行測量時，其狀態模型和觀測模型中存在非線性問題，采用基于EKF 和UKF的非線性濾波算法以改善傳統Kalman濾波的估計精度，并保證算法收斂性。通過建立組合導航的狀態方程和量測方程，分別采用EKF和UKF對狀態方程中的非線性部分進行離散化，仿真實驗結果表明，采用基于UKF的非線性導航算法能有效提高導航位置精度和系統穩定性。

關鍵詞：EKF；UKF；組合導航

導航系統在航海領域有著廣泛的應用［1，2］，在實時獲取航行時的位置、速度、航向等狀態信息方面具有重要實際意義。在船舶導航系統中常用慣導和GPS組合導航系統，既能實現依靠慣導提供多種精確的導航參數信息，又確保在海上長航保持導航精度以及避免復雜環境的電子干擾［3，4］。導航系統本質上是非線性系統，通常采用的線性誤差狀態方程是在假定慣導系統姿態誤差為小量的條件下導出，但當慣性器件精度較差或者載體做大機動運動時慣導系統失準角很大，此時導航系統中的非線性因素不可忽略，工程上常用非線性濾波算法進行處理［5］。本文通過仿真計算，對比分析EKF 和UKF兩種濾波算法在非線性組合導航系統中的應用。

1　組合導航系統數學模型

組合導航系統的實質是一個多傳感器的數據融合系統，使用擴展卡爾曼濾波器的核心是建立導航系統的狀態方程和量測方程，濾波器選取各導航參數的誤差量作為狀態變量［6，7］。本文選取位置誤差、速度誤差、姿態角誤差作為非線性組合導航系統的狀態量，并將陀螺隨機漂移誤差和加速度計零偏誤差作為擴充狀態量。

1.1系統狀態方程

選擇東、北、天地理坐標系為導航坐標系，建立系統的狀態方程：其中，X（t）=［φE，φN，φU，δVE，δVN，δV，δL，δλ，δh，εE，εN，εU，?E，?N，?U］T為狀態向量，φE、φN、φU為姿態誤差角，δVE、δVN、δVU為速度誤差，δL、δλ、δh為位置誤差，εE、εN、εU為陀螺的隨機常值漂移，?E、?N、?U為加速度計零偏。

系統過程白噪聲矩陣W（t）為：

系統狀態轉移矩陣F（t）為：

式（3）中FINS由慣導基本誤差方程決定，

系統噪聲傳播矩陣G（t）為：

1.2系統量測方程

本文采用位置、速度組合模式，建立位置量測方程和速度量測方程。

1.2.1位置量測方程

導航系統給出的實時位置誤差包含緯度、經度和高程。以L、λ和h分別代表載體真實緯度、經度和高程；LI、λI和hI代表慣導輸出緯度、經度和高程；LG、λG和hG代表GPS接收機輸出緯度、經度和高程；NN、NE和NU為GPS接收機導航系下的位置量測誤差，則有：

取慣導和GPS輸出的位置差值作為系統量測值，則位置量測方程為：

式中，HP（t）=［03×6?diag［（Re+h），（Re+h）cos L，1］?03×6］，VP（t）=［NN，NE，NU］T。

1.2.2速度量測方程

以VE、VN和VU分別代表載體真實東北天方向速度；VIE、VIN和VIU代表慣導輸出的東北天方向速度；VGE、VGN和VGU代表GPS接收機輸出的東北天方向速度。則有：

式中，NVE，NVN，NVU為GPS接收機沿東北天方向速度量測誤差，取慣導和GPS輸出的速度差值作為系統量測值，則速度量測方程為：

式中，HV（t）=［03×3?diag［1，1，1］?03×9］，VV（t）=［NVE，NVN，NVU］T。

由式（6）和式（8）建立系統的量測方程為：

1.3離散卡爾曼濾波

卡爾曼濾波是從一系列不完全且包含噪聲的量測值中估計動態系統的狀態，首先便于系統的描述計算，采用離散化方程來描述系統，離散后系統方程如下：

式中，ΦK，K-1為K-1到K時刻的n×n階系統轉移矩陣；GK-1為n×r階系統噪聲矩陣；WK-1為K-1時刻的r維的系統噪聲；HK為K時刻的m×n階系統量測矩陣；VK為K時刻的m維的測量噪聲。且要求｛WK｝和｛VK｝為互不相關均值為零，方差分別為QK和RK的白噪聲序列。濾波算法中初始狀態向量的一、二階統計特性為E｛X0｝=mX0，Var｛X0｝=CX0，卡爾曼濾波要求mX0和CX0為已知量，且X0和和｛WK｝與｛VK｝都不相關。離散卡爾曼濾波方程組如下：｛WK｝與｛VK｝都不相關。離散卡爾曼濾波方程組如下：

通過式（1）、（9）即可對導航系統方程離散化。

2　非線性濾波算法

傳統線性卡爾曼濾波會導致濾波狀態發散，而采用非線性卡爾曼濾波算法EKF和UKF會取得較好效果［8，9］。

2.1擴展卡爾曼濾波（EKF）算法

擴展卡爾曼濾波是用泰勒級數展開，生成狀態值X和量測值Z的聯合分布的高斯近似，從而解決非線性問題。考慮非線性系統模型為：

擴展卡爾曼濾波方程組如下：

2.2無跡卡爾曼濾波（UKF）算法

類似于擴展卡爾曼濾波使用泰勒級數展開，無跡卡爾曼濾波是用無跡變換（UT，unscented transform）生成狀態值X和量測值Z的聯合分布的高斯近似來解決非線性問題。對式（12）中UT變換生成聯合高斯分布：

以及相應權值：

ωm

（0）=λ/（n+λ）

ωc（0）=λ/（n+λ）+（1-α2+β）

ωm

（i）=1/［2（n+λ）］，i=1，...，2n

ωc（i）=1/［2（n+λ）］，i=1，...，2n

其中：參數λ=α2（n+κ）-n，α、β、κ均為正數。

（2）每個σ點通過非線性函數h：Z（i）=h（X（i）），i=0，...，2n。從而量測值Z的估計均值和協方差為：

以及X和Z的互協方差為：

寫成矩陣形式為：

其中：X為σ點矩陣，c=α2（n+κ），矩陣W= （1-［ωm...ωm］）×diag（ω（0）c...ω（2n）

c）×（1-［ωm...ωm］）T向量ωm=［ω（0）m...ω（2n）m］T。

由此，無跡卡爾曼濾波方程組如下：

（2）更新過程（計算預測均值μK，量測值協方差矩陣SK）：

（計算濾波增益和跟新均值和協方差）

3　仿真實驗

根據海上航行數據做仿真實驗，分別用標準卡爾曼濾波、擴展卡爾曼濾波和無跡卡爾曼濾波進行濾波和平滑處理。仿真步長t=1s，取200個采樣點。目標從原點出發，速度（，）=（1，0）（s/m）；在40s時，目標以轉向比ω=1左轉；在100s時，目標停止轉動，保持1min勻速直線運動；在110s時，目標以轉向比ω=-1右轉；在140s時，目標停止轉動，保持60s勻速直線運動。目標運動軌跡和三種濾波算法結果如圖1和圖2所示。

圖1　目標真實軌跡和采樣量測數據

圖2　使用三種濾波算法對目標位置估計結果對比

圖3給出三種濾波算法對目標位置估計的均方誤差MSE對比。在給出的三種濾波方法中，EKF 和UKF的濾波估計在仿真結果中表現基本相同，且都比標準KF效果更好，其中UKF更接近于真實軌跡值，由表1給出的平均MSE估計值也可以明顯的反映出來。

圖3　三種濾波算法位置估計MSE（均方誤差）結果對比

表1　三種濾波算法位置估計的平均MSE（均方誤差）

4　結論

本文介紹的非線性組合導航系統是根據某一具體工程應用而設計，建立了組合導航系統數學模型，對EKF和UKF濾波算法在組合導航系統中的應用進行了對比研究。仿真結果表明，在非線性導航系統中使用UKF濾波算法的導航定位精度和系統穩定性上要優于EKF，對于提高海上導航定位精度、實時獲得目標姿態信息具有重要意義。

參考文獻

［1］李旻.艦艇導航技術的設備與發展［J］.艦船電子工程，2015，35（5）：14-16.

［2］郭姣，李天偉，黎才鑫，等.艦載導航裝備綜合效能評估指標體系的研究［J］.現代電子技術，2014，37（17）：140-143.

［3］朱棟山.船用SINS/GPS/CNS組合導航信息融合算法研究［D］.北京：國防科學技術大學，2012.

［4］張曉亮.GPS/SINS組合導航系統應用研究［D］.南京：南京理工大學，2013.

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

［6］嚴恭敏.捷聯慣導算法及車載組合導航系統研究［D］.西安：西北工業大學，2004.

［7］馬濤.光纖捷聯慣導及其衛星深組合導航算法研究［D］.哈爾濱：哈爾濱工程大學，2013.

［8］趙思浩，陸明泉，馮振明.EKF與UKF在緊耦合組合導航系統中的應用［J］.系統工程與電子技術，2009（10）：2450-2454.

［9］Nicola Ceccarelli，Fabrizio Scortecci. Comparison of EKF and UKF for spacecraft localization via angle measurements［J］.IEEE transactions on Aerospace and electronic systems，2011，47（1）：75-84.

Comparison of EKF and UKF Applied in Nonlinear Navigation System

WU Chunguang，LI Shuangming，PAN Yuchun
（92941 Troops，Huludao 125000）

Abstract：For realizing the real-timemeasurement of the object on position or heading，and achieving real-time data，the nonlinear integrated navigation system based on EKF and UKF was used instead of the Kalman filter. We discussed the working principle and system composition，then represented system model and analyzed the application of EKF and UKF. The experimental results indicate that using nonlinear integrated navigationsystem based on UKF can effectively improve the navigation accuracy and system stability.

Key words：EKF；UKF；nonlinear integrated navigation system

中圖分類號：V249.3

文獻標識碼：A

文章編號：1672-9870（2016）02-0082-04

收稿日期：2015-09-24

作者簡介：吳春光（1975-），男，高級工程師，E-mail：cg2196@126.com