GPS／INS組合導(dǎo)航缺星情況下的卡爾曼濾波改進(jìn)算法

朱立新，孟 ?

（1.解放軍電子工程學(xué)院，安徽 合肥30037；2.解放軍72671部隊(duì)，山東 濟(jì)南250022）

于二維狀態(tài)，即n=2，有：

0 引言

GPS／INS組合導(dǎo)航系統(tǒng)克服了各自缺點(diǎn)，使組合后的導(dǎo)航精度高于兩個(gè)系統(tǒng)單獨(dú)工作的精度［1］。當(dāng)接收機(jī)進(jìn)行大角度運(yùn)動(dòng)，或是在城市、峽谷等復(fù)雜環(huán)境中，衛(wèi)星信號(hào)容易受到遮擋或者干擾［2］，此時(shí)以偽距和偽距率為觀測(cè)量的GPS／INS緊組合能夠利用少于4顆衛(wèi)星提供的觀測(cè)值來使組合模式得以維持［3］。

針對(duì)GPS／INS 緊組合導(dǎo)航模型的非線性特點(diǎn)，采用狀態(tài)方程為線性，量測(cè)方程為非線性的線性／非線 性 混 合 模 型［4］。EKF（Extended Kalman filter，擴(kuò)展卡爾曼濾波）和UKF（Unscented Kalman filter，無跡卡爾曼濾波）是典型的非線性濾波，EKF將非線性方程按泰勒級(jí)數(shù)展開，取一階截?cái)嘧鳛樵匠痰慕啤＿@種方法雖然能夠在一定程度上解決非線性系統(tǒng)的狀態(tài)估計(jì)問題，但存在明顯缺點(diǎn)：首先，略去了高階項(xiàng)，帶來了截?cái)嗾`差，這種截?cái)嗾`差嚴(yán)重影響估計(jì)精度；其次，需要計(jì)算雅克比矩陣，在方程非線性程度比較高時(shí)該矩陣的求取十分復(fù)雜，甚至難以得到。UKF的計(jì)算量和EKF相當(dāng)，性能上優(yōu)于EKF，且無需計(jì)算雅可比陣，其應(yīng)用領(lǐng)域不斷擴(kuò)展；但UKF 在進(jìn)行迭代濾波過程中需要矩陣分解和求逆運(yùn)算，UKF狀態(tài)估計(jì)協(xié)方差矩陣難以保持正定性。

本文針對(duì)以上問題，在GPS／INS 緊組合導(dǎo)航衛(wèi)星信號(hào)缺失情況下，提出了基于強(qiáng)跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法。

1 GPS／INS緊組合模型

衛(wèi)星信號(hào)正常情況下，緊組合是組合程度較深的組合方式，其主要特點(diǎn)是GPS接收機(jī)和慣性導(dǎo)航系統(tǒng)相互輔助。用GPS接收機(jī)給出的衛(wèi)星星歷數(shù)據(jù)和慣性導(dǎo)航系統(tǒng)給出的位置和速度，解算對(duì)應(yīng)于慣性導(dǎo)航系統(tǒng)位置和速度的偽距ρI 和偽距率˙ρI。把ρI 和˙ρI 與GPS接收機(jī)測(cè)量的ρG 和˙ρG 相比較作為觀測(cè)量，通過組合卡爾曼濾波器估計(jì)慣性導(dǎo)航系統(tǒng)和GPS接收機(jī)的誤差量，然后對(duì)兩個(gè)系統(tǒng)進(jìn)行校正。其原理框圖如圖1所示［5］。

圖1 緊組合工作框圖Fig.1 Work diagram of tightly-coupled

1.1 狀態(tài)方程

GPS誤差狀態(tài)變量為2維的XG（t）；，INS誤差XI（t）包括平臺(tái)誤差角（3維）、速度誤差（3維）、位置誤 差（3 維）、INS 元 件 誤 差（9 維）共 計(jì)18 維。GPS／INS組合導(dǎo)航系統(tǒng)的狀態(tài)方程可寫作：

即

其中：

分別表示姿態(tài)誤差、速度誤差、位置誤差、陀螺漂移誤差（常值偏移和隨機(jī)偏移）、加速度計(jì)誤差、GPS鐘差等效距離和等效距離變化率，F(xiàn) 是狀態(tài)轉(zhuǎn)移矩陣，G 為噪聲系數(shù)矩陣，W 為噪聲向量，具體參見參考文獻(xiàn)［6］。

1.2 量測(cè)方程

設(shè)載體在地球大地坐標(biāo)系中的慣導(dǎo)輸出位置可表示為經(jīng)度λI、緯度LI、高度hI，則載體真實(shí)位置可表示為λ=λI-Δλ，L=LI-ΔL，h=hI-Δh，將其轉(zhuǎn)換為地心地球固連（ECEF）坐標(biāo)系為：

則衛(wèi)星到載體真實(shí)距離ρj（j=1，2，3，4）可表示為：

GPS接收機(jī)測(cè)得到衛(wèi)星j 之間的偽距可表示為：

即：

將x、y、z帶入上式得：

同理，對(duì)于偽距率可得：

帶入ρ·j最終得到ρ·jG 為： 將x、y、z帶入上式即可得到偽距率的量測(cè)方程，以接收機(jī)輸出的偽距信號(hào)作為觀測(cè)量的量測(cè)方程（限于篇幅不再展開）表示為：

2 基于強(qiáng)跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法

CKF是由加拿大學(xué)者Ienkaran Arasaratnam提出的基于確定性采樣的非線性濾波［7］，利用數(shù)值積分方法對(duì)目標(biāo)狀態(tài)后驗(yàn)概率進(jìn)行近似，通過確定一系列積分點(diǎn)和權(quán)值來直接計(jì)算后驗(yàn)概率密度，在計(jì)算量和精度之間獲得了有效折衷。學(xué)者研究表明，對(duì)于維數(shù)高于3維的非線性系統(tǒng)，CKF 的濾波精度高于UKF，建議采用CKF最為濾波方法［8］。如前文所述，采用的組合導(dǎo)航模型狀態(tài)維數(shù)遠(yuǎn)高于3維，所以本文選用CKF。

CKF應(yīng)用于GPS／INS組合導(dǎo)航中表現(xiàn)出兩個(gè)問題：一是當(dāng)前時(shí)刻量測(cè)值對(duì)于一步預(yù)測(cè)值的修正作用受到以前時(shí)刻觀測(cè)值過強(qiáng)的抑制，致使濾波值不能較好地跟蹤真值變化［9］；二是由于計(jì)算機(jī)“截?cái)嘈?yīng)”引入的代數(shù)運(yùn)算誤差，導(dǎo)致誤差協(xié)方差陣的正定性和對(duì)稱性喪失，從而CKF濾波過程中的矩陣分解、求逆會(huì)進(jìn)一步惡化該現(xiàn)象，使濾波不穩(wěn)定。

針對(duì)上述兩個(gè)問題，分別引入強(qiáng)跟蹤濾波理論和均方根策略對(duì)CKF進(jìn)行改進(jìn)，將改進(jìn)的算法應(yīng)用于GPS／INS緊組合導(dǎo)航中的衛(wèi)星信號(hào)缺失情況下，驗(yàn)證算法的有效性。

2.1 強(qiáng)跟蹤濾波器原理

假設(shè)狀態(tài)空間的系統(tǒng)模型和觀測(cè)模型為：

其中，xk為k時(shí)刻系統(tǒng)的狀態(tài)向量，zk為觀測(cè)值，wk和vk+1分別為系統(tǒng)噪聲和量測(cè)噪聲，且wk～N（0，Qk），vk～N（0，Rk），f為狀態(tài)轉(zhuǎn)移函數(shù)，h 為量測(cè)函數(shù)。

針對(duì)引言中提出的CKF 在組合導(dǎo)航應(yīng)用中的第一個(gè)問題，消除模型誤差所致濾波發(fā)散的一種思路是采用漸消記憶濾波，即在濾波過程中人為地相對(duì)突出新數(shù)據(jù)的作用，貶低舊數(shù)據(jù)的作用?；谠摾碚摶A(chǔ)，學(xué)者提出了一種帶次優(yōu)漸消因子的擴(kuò)展卡爾曼濾波，成為強(qiáng)跟蹤濾波器（strong tracking filter，STF）［10-11］。

STF在狀態(tài)預(yù)測(cè)協(xié)方差陣Pk+1|k中引入漸消因子λk+1，實(shí)時(shí)調(diào)整增益矩陣Kk+1，使得同時(shí)滿足以下兩個(gè)條件：

如果模型與實(shí)際系統(tǒng)完全匹配時(shí)，上面兩式完全成立。若模型不確定則會(huì)造成濾波器的狀態(tài)估計(jì)值偏離系統(tǒng)的狀態(tài)，此時(shí)，在線調(diào)整增益矩陣，強(qiáng)迫式（15）成立，可以強(qiáng)迫強(qiáng)跟蹤濾波器保持對(duì)系統(tǒng)狀態(tài)的跟蹤。

強(qiáng)跟蹤濾波器的基本方程為：

式（16）中，λk+1為次優(yōu)漸消因子，其計(jì)算方法通常采用次優(yōu)算法，目的是為了提高算法的實(shí)時(shí)性。設(shè)ek+1=zk+1-^zk+1|k為輸出殘差序列，則：

式（17）中，tr（·）為對(duì)矩陣求跡，Vk+1為輸出殘差序列的協(xié)方差矩陣，實(shí)際計(jì)算中Vk+1是未知的，由下式進(jìn)行估計(jì)：

式（18）中，0＜ρ≤1為遺忘因子，通常取ρ=0.95。

若已知狀態(tài)預(yù)測(cè)協(xié)方差矩陣Pk+1|k，估計(jì)值的自相關(guān)協(xié)方差矩陣Pzz，k+1|k和估計(jì)值的互相關(guān)協(xié)方差矩陣Pxz，k+1|k，則上文中Nk+1和Mk+1可以等效表示為：

2.2 均方根濾波策略

針對(duì)引言中提出的CKF 在組合導(dǎo)航應(yīng)用中的第二個(gè)問題，均方根策略是弱化矩陣病態(tài)影響的有效措施，在均方根容積卡爾曼濾波更新步驟中，參與迭代的是預(yù)測(cè)和后驗(yàn)誤差協(xié)方差矩陣的均方根因子，而非協(xié)方差矩陣。而且利用最小二乘法計(jì)算增益，避免了矩陣求逆運(yùn)算；利用三角分解或QR 分解獲取協(xié)方差矩陣的三角分解因子，而非均方根因子，這都增強(qiáng)了濾波的穩(wěn)定性。

一般的QR分解算法表示為S=Tria（A），其中S為下三角矩陣，矩陣A 和S 的關(guān)系為：假如R 是AT的QR 分解所得上三角矩陣，則S=RT。

2.3 強(qiáng)跟蹤SRCKF

把強(qiáng)跟蹤濾波原理和均方根策略與CKF 算法相結(jié)合，構(gòu)造一種強(qiáng)跟蹤SRCKF（該算法命名具有一定局限性），算法具體步驟如下。

2.3.1 時(shí)間更新和計(jì)算漸消因子

1）假設(shè)k 時(shí)刻的后驗(yàn)概率p（xk）= N，Pk|k）已知，通過因式分解誤差協(xié)方差：

2）構(gòu)造Cubature點(diǎn)ξi表示集合［1］的第i列，對(duì)

于二維狀態(tài)，即n=2，有：

3）計(jì)算傳播后的積分點(diǎn)

4）計(jì)算k+1時(shí)刻狀態(tài)預(yù)測(cè)值

5）計(jì)算k+1時(shí)刻狀態(tài)協(xié)方差預(yù)測(cè)值

其中Sk+1|k為協(xié)方差的平方根系數(shù)。

式中，SQ，k為Qk的平方根系數(shù)，加權(quán)的的中心矩陣為且

計(jì)算漸消因子λk+1：

6）計(jì)算傳播后的積分點(diǎn)

7）計(jì)算k+1時(shí)刻量測(cè)估計(jì)值

8）計(jì)算k+1 時(shí)刻估計(jì)值的自相關(guān)協(xié)方差矩陣Pzz，k+1|k

其中，Szz，k+1|k為協(xié)方差的平方根系數(shù)。

式中，SR，k+1為Rk+1的平方根系數(shù)，加權(quán)的的中心矩陣為Zk+1|k且

9）計(jì)算k+1 時(shí)刻估計(jì)值的互相關(guān)協(xié)方差矩陣Pxz，k+1|k

其中：

按照2.1 節(jié)中的算法，可以計(jì)算出漸消因子λk+1，計(jì)算引入漸消因子的狀態(tài)協(xié)方差預(yù)測(cè)值Pk+1|k

2.3.2 量測(cè)更新

1）對(duì)狀態(tài)協(xié)方差預(yù)測(cè)值Pk+1|k進(jìn)行分解，得：

2）執(zhí)行2.3.1部分中的6）、8）、9）步驟，計(jì)算量測(cè)估計(jì)值以及估計(jì)值的自相關(guān)和互相關(guān)協(xié)方差矩陣。

3）計(jì)算濾波增益矩陣

4）狀態(tài)更新

5）計(jì)算相關(guān)協(xié)方差矩陣及其平方根系數(shù)

3 仿真驗(yàn)證與結(jié)果分析

為了驗(yàn)證改進(jìn)算法的有效性，針對(duì)衛(wèi)星信號(hào)缺失情況下GPS／INS組合導(dǎo)航的導(dǎo)航精度進(jìn)行仿真實(shí)驗(yàn)。

設(shè)計(jì)仿真時(shí)長(zhǎng)為500s的飛行過程，其中包括勻速、加速、減速、轉(zhuǎn)彎、爬升、俯沖、平飛等過程，具體飛行軌跡如圖2所示。

仿真參數(shù)設(shè)置如下：SINS 的陀螺隨機(jī)漂移εb為0.01（°）／h；陀螺儀一階馬爾科夫過程隨機(jī)噪聲εr為0.01（°）／h；加速度計(jì)漂移誤差Δ 為10-4g；GPS隨機(jī)偽距誤差為10 m，隨機(jī)偽距率測(cè)量誤差為0.05m／s；設(shè)模擬飛行器的采樣周期為0.01s，捷聯(lián)慣性導(dǎo)航的更新頻率為50 Hz，GPS接收機(jī)的更新頻率為1Hz，濾波器的數(shù)據(jù)融合頻率同樣為1 Hz。由設(shè)定的仿真軌跡和仿真參數(shù)可以看出，在200～400s有加速、轉(zhuǎn)彎、俯沖等大角度機(jī)動(dòng)，在此情況下容易出現(xiàn)衛(wèi)星信號(hào)缺失情況，仿真實(shí)驗(yàn)中設(shè)置在該時(shí)間段內(nèi)衛(wèi)星數(shù)目為3顆，驗(yàn)證改進(jìn)算法在正常情況下和缺星情況下的有效性。限于篇幅，圖3、圖4為正常情況下強(qiáng)跟蹤SRCKF 和強(qiáng)跟蹤UKF 濾波精度對(duì)比（僅以東向速度誤差和經(jīng)度誤差為例）。

圖2 飛行軌跡仿真圖Fig.2 The figure of flight path Simulation

圖3 東向速度誤差曲線圖Fig.3 The curve of eastern velocity error

圖4 經(jīng)度誤差曲線圖Fig.4 The curve of longitude error

由以上仿真結(jié)果可以看出，在組合導(dǎo)航中，針對(duì)算法應(yīng)用過程中存在的同一問題，對(duì)CKF 和UKF進(jìn)行改進(jìn)，改進(jìn)的CKF濾波精度高于改進(jìn)UKF，驗(yàn)證了文獻(xiàn)［8］的結(jié)論。

圖5—圖10為缺星情況下誤差仿真結(jié)果，表1和表2為不同情況下導(dǎo)航信息的均方根誤差對(duì)比。

圖5 經(jīng)度誤差曲線圖Fig.5 The curve of longitude error

圖6 緯度誤差曲線圖Fig.6 The curve of latitude error

圖7 高度誤差曲線圖Fig.7 The curve of height error

圖8 東向速度誤差曲線圖Fig.8 The curve of eastern velocity error

圖9 北向速度誤差曲線圖Fig.9 The curve of northern velocity error

圖10 天向速度誤差曲線圖Fig.10 The curve of vertical velocity error

表1 正常情況下位置、速度均方根誤差（RMSE）Tab.1 Position and speed RMSE under normal circumstances

表2 缺星情況下位置、速度均方根誤差（RMSE）Tab.2 Position and speed RMSE under signal invalidation circumstances

由圖5—圖10以及表1、表2的仿真結(jié)果可知，GPS／INS緊組合導(dǎo)航模式能夠在衛(wèi)星信號(hào)缺失情況下維持組合模式，提供一定精度范圍內(nèi)的導(dǎo)航信息。

基于強(qiáng)跟蹤濾波思想和均方根策略的改進(jìn)CKF算法，通過提高當(dāng)前時(shí)刻觀測(cè)值的作用有效地減小了由于模型不確定性和在濾波過程中的計(jì)算問題所帶來的導(dǎo)航誤差。在衛(wèi)星信號(hào)正常情況下，和CKF算法相比，強(qiáng)跟蹤SRCKF 能夠提供更高精度的導(dǎo)航信息，導(dǎo)航精度能夠提高20%～30%；在衛(wèi)星信號(hào)缺失情況下，導(dǎo)航精度有所降低，改進(jìn)算法能夠提供更加精確的導(dǎo)航信息，盡量減小由于衛(wèi)星信號(hào)缺失所帶來的影響，導(dǎo)航精度能夠提高30%～40%，改進(jìn)的CKF 算法在衛(wèi)星信號(hào)缺失情況下，更能發(fā)揮其優(yōu)點(diǎn)。

組合導(dǎo)航對(duì)濾波算法的實(shí)時(shí)性有很高的要求，圖11對(duì)兩種算法的單次濾波迭代時(shí)間（該時(shí)間指在特定仿真環(huán)境下的時(shí)間，不能反映應(yīng)用中的運(yùn)行時(shí)間，僅供相同條件下不同算法參考對(duì)照）進(jìn)行對(duì)比，可知強(qiáng)跟蹤SRCKF 單次濾波迭代時(shí)間略高于CKF，這是由于算法在更新階段需要計(jì)算漸消因子λk+1，從而導(dǎo)致計(jì)算量有所增加，但仍然可以滿足實(shí)時(shí)性的要求?？梢姀?qiáng)跟蹤SRCKF是以計(jì)算量為代價(jià)來提高濾波精度。

圖11 濾波時(shí)間對(duì)比Fig.11 Filtering Time Contrast

4 結(jié)論

本文提出了基于強(qiáng)跟蹤濾波理論和均方根策略的容積卡爾曼濾波算法，該算法利用強(qiáng)跟蹤濾波突出了新數(shù)據(jù)的作用，提高了算法在模型不確定時(shí)的魯棒性；用均方根策略保證了協(xié)方差陣的正定性和對(duì)稱性。仿真實(shí)驗(yàn)表明：與CKF 算法相比，強(qiáng)跟蹤SRCKF 能夠提供更高精度的導(dǎo)航信息，且在衛(wèi)星信號(hào)缺失情況下，更能發(fā)揮其優(yōu)點(diǎn)。雖然單次濾波迭代時(shí)間增加了40%左右，但是，就采用強(qiáng)跟蹤策略的非線性濾波算法而言，該算法的實(shí)時(shí)性仍然可以滿足要求。

［1］全偉.慣性／天文／衛(wèi)星組合導(dǎo)航技術(shù)［M］.北京：國(guó)防工業(yè)出版社，2011.

［2］Grewal M S，Weill L R，Andrews A P.Global Positioning Systems，Inertial Navigation and Integration［M］.New York：Wiley Interscience，2007.

［3］許剛，黃國(guó)榮，彭興釗，等.衛(wèi)星信號(hào)失效條件下SINS／GPS不同組合方式的性能比較［J］.計(jì)算機(jī)應(yīng)用研究，2012（10）：3888-3896.

［4］尹建君，張建秋，林青.Unscented卡爾曼濾波-卡爾曼濾波算法［J］.系 統(tǒng) 工 程 與 電 子 技 術(shù).2008，30（4）：617-620.

［5］劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用［M］.西安：西北工業(yè)大學(xué)出版社，2010.

［6］王惠南.GPS 導(dǎo)航原理與應(yīng)用［M］.北京：科學(xué)出版社，2003.

［7］Ienkaran Arasaratnam，Simon Haykin.Cubature Kalman filters［J］.IEEE Trans on Automatic Control，2009，54（6）：1254-1269.

［8］孫楓，唐李軍.Cubature卡爾曼濾波與Unscented 卡爾曼濾波估計(jì)精度比較［J］.控制與決策，2013，28（2）：303-308.

［9］李莉.衰減記憶擴(kuò)展卡爾曼濾波在目標(biāo)跟蹤中的應(yīng)用［J］.電子測(cè)量技術(shù)，2011，34（2）：36-38，65.

［10］周東華，席裕庚，張鐘俊.一種帶多重次優(yōu)漸消因子的擴(kuò)展 卡 爾 曼 濾 波 器［J］.自 動(dòng) 化 學(xué) 報(bào)，1991，16（7）：689-695.

［11］金天，王玉寶，叢麗，等.基于模糊邏輯的自適應(yīng)強(qiáng)跟蹤UKF定位濾波算法［J］.高技術(shù)通 訊，2012，22（4）：348-354.