改進的強跟蹤UKF在組合導航中的應用研究

符 強，趙鴻悅，孫希延，3，紀元法，3*

(1. 桂林電子科技大學廣西精密導航技術與應用重點實驗室，廣西 桂林 541004；2. 衛星導航定位與位置服務國家地方聯合工程研究中心，廣西 桂林 541004；3. 衛星導航系統與裝備技術國家重點實驗室，河北 石家莊 050000)

1 引言

在組合導航緊耦合模型中，通常采用偽距和偽距率作為量測值，而這類量測值具有非線性特點[1]。UKF算法是目前處理非線性系統的一種有效算法[2]。然而標準UKF也有其局限性，如對系統模型精確性要求較高，在系統狀態突變或模型不準確情況下，UKF濾波精度會大大降低甚至發散[3][4]。為了克服UKF的局限性，有學者將強跟蹤濾波和UKF濾波相結合，提高了系統在復雜環境下的穩定性，但每個濾波周期都要經過三次UT變換，計算量復雜，嚴重影響實時性[5]；文獻[6]對UKF濾波做出簡化，并在大失準角條件下實現了靜基座下SINS初始對準。文獻[7]采用假設檢驗的方法對系統狀態做出檢測，再利用漸消因子做出自適應調整。文獻[8]對強跟蹤中次優漸消因子的求解方法做出改進，提高了強跟蹤UKF的實用性。

針對強跟蹤UKF在緊耦合模型中的局限性，特別是模型不匹配[9]、濾波性能欠佳及計算過程中迭代不穩定的問題，本文提出一種改進的強跟蹤UKF算法，能夠提高UKF對緊耦合模型的適應能力，與傳統的強跟蹤UKF相比，該算法改進UKF與強跟蹤的結合方式，增強濾波的迭代穩定性，改善了漸消因子的求解方式，減少了冗余計算，提高了濾波性能。本文分別運用標準UKF、強跟蹤UKF和改進的強跟蹤UKF在相同條件下對系統進行濾波，比較不同算法的定位誤差。

2 組合導航系統模型

本文選取東北天坐標系作為導航坐標系，狀態方程中采用17個狀態參數，具體的狀態量為

(1)

X(t)=F(t)X(t)+W(t)

(2)

其中，F(t)表示狀態轉移矩陣，X(t)表示系統狀態誤差量，W(t)為系統過程噪聲。

將INS計算的偽距和偽距率和GPS的偽距和偽距率之差作為量測值Z，量測方程可以表示為

Z(t)=H(t)X(t)+V(t)

(3)

式中，H(t)表示偽距、偽距率的非線性觀測方程，V(t)表示白噪聲。

3 標準UKF算法

由前面可知，組合導航離散系統可表示為：

(4)

其中，Fk表示k時刻的狀態轉移矩陣，Xk表示k時刻的狀態向量，Hk表示觀測方程，Zk表示k時刻的量測向量，Wk和Vk表示高斯白噪聲且互不相關。

標準UKF對系統方程的濾波步驟為：

1)初始化

2)利用均值、方差得到Sigma采樣點集及其對應的權值。

3)將點集做一步預測

4)將Sigma點集的預測值加權求和，得到系統狀態量的一步預測和協方差。

5)根據一步預測值，再次使用UT變換，產生新的Sigma點集。

6)將新產生的Sigma點集帶入觀測方程，得到預測的觀測值。

7)將步驟6)的Sigma點集的觀測預測值加權求和得到系統預測的均值及協方差。

8)計算卡爾曼增益

9)更新系統的狀態向量和協方差。

4 改進的強跟蹤UKF算法

傳統結構下的強跟蹤UKF在每個濾波周期中都需要進行三次UT變換[11]，每次變換都會有大量計算，這會嚴重影響濾波性能。本文通過重構漸消矩陣的求解方式和改進強跟蹤UKF的濾波結構，增強算法與模型的匹配度，提高濾波迭代的穩定性和濾波精度。

4.1 漸消矩陣的求解

組合導航系統除了受自身的非線性因素影響外，主要是受到量測噪聲的影響，標準UKF對系統模型的嚴格要求，在大量的量測噪聲影響下很難精確定位，強跟蹤是一種有效的解決辦法。強跟蹤濾波應滿足如下條件[12]

(5)

在UKF中估計誤差與預測誤差為

(6)

(7)

(8)

根據殘差序列的定義可知

εk=Zk-k/k-1

(9)

根據量測方程可知

Zk=H(Xk)

(10)

(11)

則由式(10)、式(11)得殘差序列表示為

(12)

那么k+j時刻的殘差矩陣可以表示為

(13)

根據強跟蹤的正交性原理可知

(14)

將式(12)、式(13)帶入可知[7]

(P-KkVk)

(15)

要保證正交性，即式(16)成立即可

P-KkVk=0

(16)

根據卡爾曼增益公式，式(17)可變形為

Kk(P-Vk)=0

(17)

由于增益矩陣為非零矩陣，式可以簡化為

P-Vk=0

(18)

將協方差展開可得

(19)

定義Nk，Mk為

Nk=Vk-Rk

(20)

(21)

將式(19)用Nk，Mk表示為λkMk=Nk，對兩邊求跡，得到漸消因子τk

(22)

再根據式(23)、式(24)將求得的漸消因子組成漸消矩陣λk=diag(λ1，1，λ2，2，…λn，n)。

(23)

λi，i=τkκi

(24)

本文根據強跟蹤的正交性原則，重新構筑漸消因子的求解方式，再組成漸消矩陣對每個狀態量做針對性調整，利用漸消矩陣直接作用于預測協方差陣，避免了傳統算法先求出標準狀態協方差、再通過UT變換重新計算預測協方差和互協方差，減少了UT變換次數，有效降低了計算量。

4.2 ISTUKF具體算法

由于INS/GNSS緊耦合系統的狀態方程具有線性特征[13]， 而UT變換得到的Sigma點集是圍繞狀態量的均值對稱分布，并且對稱點之間的權值相同。因此在狀態更新中采用UT變換，實際上是將Sigma點集進行線性映射，此時求得的映射后的Sigma點集與根據一步預測值的均值和方差獲取的Sigma點集是等價的，因此UT變換并未起到作用，反而帶來了額外的計算量。因此對強跟蹤UKF濾波算法進行結構分解，采用KF與UKF混合濾波的方式與強跟蹤濾波相結合，減少強跟蹤在狀態方程中的UT變換的次數。以下為具體流程：

步驟1：已知k-1時刻的狀態值k-1與狀態協方差Pk-1。

步驟2：時間更新，對系統狀態量的一步預測及標準協方差矩陣

k/k-1=Fk/k-1k-1

(25)

(26)

步驟3：計算Sigma點及其權值

(27)

(28)

(29)

步驟5：由步驟4得到的Sigma點集進行加權求和得到系統預測值的均值和標準協方差。

(30)

(31)

(32)

步驟6：利用漸消矩陣修正協方差值

Pk/k-1=λkP′k/k-1

(33)

P=λkP′

(34)

P=λkP′

(35)

步驟7：計算卡爾曼增益矩陣、更新狀態估計值和協方差矩陣，完成一個整周濾波。

(36)

(37)

(38)

綜上所述，與傳統的強跟蹤UKF算法相比，改進的強跟蹤UKF根據狀態方程的線性特征，將第一次UT變換省略。第二次UT變換相同，得到一步預測值的Sigma點集。根據正交性原理，改進漸消矩陣的求解方式和作用位置，令漸消矩陣直接作用于預測協方差，保證了增益矩陣受到漸消矩陣的影響，從而減少了第三次UT變換，使濾波周期中只有一次UT變換。

5 仿真與分析

在相同仿真條件下，分別驗證改進的強跟蹤UKF濾波在不同環境條件下的效果，將算法與標準UKF及強跟蹤UKF對飛行器整個飛行軌跡進行濾波估計。

實驗1：理想環境下模擬載體飛行軌跡，載體的起始位置設置為北緯-21.786°，東經為170.463°，高度為105 m，載體軌跡包含加速、減速、轉彎、下降和勻速等飛行狀態，飛行速度最高200 m/s，飛行軌跡如圖1所示。仿真共進行180 s，慣導系統的采樣周期0.01 s。加速度一階馬爾科夫過程相關時間為300s。衛星接收機的濾波和輸出周期均為1 s，速度誤差為0.1 m/s，水平方向誤差為5 m，天向誤差為5 m。分別利用3種濾波算法進行濾波，為了能夠直觀對比不同濾波的性能，本文將標準UKF、強跟蹤UKF和強跟蹤混合濾波3種濾波的速度誤差、位置誤差和姿態誤差進行比較。圖2為3種濾波的位置誤差。

圖1 載體飛行軌跡

圖2 東向位置誤差對比

圖3 北向位置誤差對比

由圖2可知，三種濾波處理后的位置誤差比較穩定，沒有隨時間進行積累，其中ISTUKF與UKF的位置誤差較小，STUKF的誤差最大。這是因為STUKF中的強跟蹤由于需要保持殘差向量的正交性，漸消矩陣對協方差矩陣做出調整，削弱了歷史觀測值對當前狀態的影響，由于強跟蹤濾波在載體處于模型確定狀態下歷史觀測信息利用較少，而文中ISTUKF比STUKF更貼近緊耦合模型。因此，STUKF濾波精度會低于標準UKF與ISTUKF。

實驗2：模擬復雜環境下的載體飛行軌跡，與理想環境相比，復雜環境下系統模型精確度下降，設計模型不確定性為

(39)

由圖4、圖5可知，三種濾波處理后的位置誤差與理想條件下相比變大，但也趨于穩定。其中UKF的東向、北向位置誤差相對于其它兩種算法較大。這是由于慣性器件受環境影響，產生大量的量測噪聲，標準UKF難以適用于此類環境。而強跟蹤濾波根據殘差矩陣變化求出漸消矩陣，并通過漸消矩陣加強當前時刻觀測量的影響力，削弱舊觀測量及其噪聲的影響力，進而調整增益矩陣，使得系統在復雜環境下穩定性更強。因此STUKF和ISTUKF誤差較小，又ISTUKF比STUKF在濾波中有較少的近似計算，與模型更匹配，因此ISTUKF性能相對STUKF有小幅改善。

圖4 東向位置誤差對比

圖5 北向位置誤差對比

6 結論

傳統的強跟蹤UKF算法相較于標準UKF能夠增強系統在復雜環境下的穩定性和魯棒性，但是該算法在組合導航緊耦合應用中存在模型不匹配、冗余計算及濾波精度降低的問題，針對這些問題，本文改進漸消因子的求解方式，調整漸消矩陣作用位置，改進強跟蹤UKF濾波結構。通過仿真對比標準UKF、強跟蹤UKF和ISTUKF的濾波效果，根據實驗得到以下結論：

1)在理想環境下，強跟蹤UKF與其它兩種算法相比定位誤差較大。ISTUKF與標準UKF誤差接近。

2)在環境因素對系統影響較大的情況下，改進的強跟蹤UKF濾波和強跟蹤UKF濾波定位誤差較標準UKF有所改善。

3)改進的強跟蹤濾波使組合導航系統對不同環境的適應性更強，濾波性能更完善。

綜上所述，本文提出的ISTUKF算法能夠改善傳統算法的不足。既能保證系統在不同環境下的適應性，具有較高的實際應用價值。