改進的Sage-Husa自適應算法及應用

鄧洪高，馬海淘，紀元法，孫希延

(桂林電子科技大學，廣西 桂林 541004)

1 引言

慣性導航系統(inertial navigation system)和衛星導航系統(satellite navigation system)各自都在技術上具有其一些優缺點，比如傳統的慣性導航系統其最大的優點之一就是技術上的自主性強，隱蔽性好[1]，缺點之二就是誤差可能會使其隨著一段時間不斷地積累而逐漸增大；衛星導航系統的優點之一就是它們的導航精度很高，且這個誤差也不會因為時間而增大；但各個系統單獨在使用過程中都會因為受到一定程度上的限制，而兩個系統之間互補能力較強，因此在慣性/衛星導航系統在當前被公認的最佳組合導航系統。Kalman自從研究出來以來，得到了廣闊的應用場景，標準卡爾曼濾波對于系統很多個狀態估計參數都要求設置為已知，才能夠直接求得對狀態估計的最優理解，比如對于系統噪聲的各種統計學特性、系統模型和對于量測的噪聲都要求設置為已知，并且都是準確的，而往往在實際的應用中對于系統模型進行參數設置是不夠準確的，原因為由于系統噪聲、量測時間噪聲受多種影響因素的直接影響導致是未知的，為了解決問題，提出了Sage-Husa自適應濾波[2-3]。在進行濾波的同時，利用量測數據對噪聲統計特性進行更正，來提高高階濾波的精度，減少估計誤差。

Sage-Husa自適應濾波增加了噪聲統計特性的計算過程，隨著濾波的進行，在某個時間段內，因觀測量誤差增大而導致新息中引入大量誤差，可能會使噪聲陣出現負定性的情況，引發濾波結果發散，降低系統的穩定性和可靠性[4]。

針對標準卡爾曼濾波和Sage-Husa自適應卡爾曼濾波不能同時滿足實時在線估計狀態量測噪聲陣和抑制濾波發散的問題，本文提出一種改進的Sage-Husa自適應濾波，采用引入量測異常檢測處理的方法，保證了噪聲陣的半正定性或正定性[5-6]，抑制濾波器的發散，能夠提高Sage-Husa自適應算法對組合導航模型的適應能力，本文分別運用標準KF、Sage-Husa自適應濾波和改進的Sage-Husa自適應濾波在相同條件下對系統進行濾波，比較不同算法的速度誤差和位置誤差，得出該算法提升了導航系統的定位精度。

2 組合導航系統數學模型

建立如下的BDS/INS組合導航系統狀態方程

(1)

式中：狀態變量為

X=[φE，φN，φU，δvE，δvN，δvU，

δL，δλ，δh，εbx，εby，εbz，?x，?y，?z]T

(2)

高斯白噪聲隨機誤差變量為

W=[ωgx，ωgy，ωgz，0，0，0，ωax，ωay，ωaz]

(3)

誤差系數矩陣G為

(4)

狀態系數矩陣F為

(5)

其中，為9個導航參數矩陣

(6)

(7)

在經過卡爾曼濾波算法的修正后，為了能得到更高精度的位置和速度信息

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

(8)

其中Z=[VINS-VBDSPINS-PBDS]T，VINS,VBDS分別為慣導系統計算出的速度信息和BDS輸出的速度信息；PINS,PBDS分別為慣導系統計算出的位置信息和BDS輸出的位置信息H=[HV,HP]T，其中HV=[03×3diag(1,1,1) 03×9]，HP=[03×6diag[Rm,RncosL,1] 03×3]，因此可以得到BDS/INS組合導航系統的量測方程為

(9)

3 Sage-Husa自適應卡爾曼濾波算法

3.1 Sage-Husa自適應卡爾曼濾波算法

由上述內容可知，對組合導航系統離散化后可表示為

(10)

其中，Wk和Vk為帶有時變統計特性的噪聲陣，設統計特性為：E(Wk)=qkvar(Wk)=Qk

E(Vk)=rkvar(Vk)=Rk

對于上述所表示的系統，Sage-Husa自適應卡爾曼濾波算法的步驟如下

(11)

式(10)中，dk=(1-b)/(1-bk+1)，b為遺忘因子，且0

上述式中，從表面上看起來Sage-Husa 自適應濾波算法可以在Q和R都是不確定的情況下，按照式(11)的流程可以把Q和R同時都計算出來[7]，但在實際上，Sage-Husa自適應濾波算法是無法把它們同時估計計算出來的，只能在其中一個參數已知的條件下，估計出另一個參數，通過式(11)可知但新息ek發生異常時，會同時影響Q和R的計算結果，很難保證濾波結果的準確性。另外從Q和R的計算可以看出，因為減號的存在，可能會使Q和R隨時為非正定性，從而引發濾波發散現象。因此對算法進行改進顯得很有必要的。

3.2 簡化Sage-Husa 自適應卡爾曼濾波算法

為了提高BDS/INS系統的穩定性和濾波的計算效率，一般在濾波過程中會認為系統噪聲是已知固定的[8]，這時僅僅需要對量測噪聲R進行估計計算，Sage-Husa自適應濾波算法也就變成了一種簡化的Sage-Husa自適應濾波算法，簡化后的算法還有很好的自適應性。因為系統噪聲是穩定的，即Q是常量，q=0，r=0，經過簡化的Sage-Husa自適應濾波算法為[9]

(12)

4 Sage-Husa自適應濾波算法的改進

4.1 量測異常檢測

在實際的工程應用中，通常是采用基于協方差匹配的濾波異常檢測法，來實現對量測數據異常與動力學模型異常的檢測。

其中，基于協方差匹配技術的濾波發散的判斷依據為[10]

(13)

假設R(k)=R(k-1)，可推導得

(14)

所以，由上式(13)、(14)能推出最嚴格收斂判斷為

(15)

濾波發散判斷為

(16)

4.2 最佳加權系數的選擇

次優濾波是引進對模型預測誤差方差陣的修正，在濾波過程中主動改變Q和R的加權系數，達到減小舊信息值的權重，從而提高新信息值的權重。在Sage-Husa自適應濾波算法中，量測噪聲Rk的計算式中的減號，容易使量測噪聲Rk失去非正定性，導致濾波發散[11]。式(15)成立時，表明此時原來的狀態估計均方誤差陣Pk/k-1已經不再適應當前濾波環境，應該采取加大當前觀察數據的權重的方法來適應當前實際的濾波環境。

在最嚴格的收斂判斷條件下，計算出次優濾波方法中新的狀態估計均方誤差陣Pk/k-1的加權系數，通過新的狀態估計均方誤差陣Pk/k-1對原來的狀態估計均方誤差陣Pk/k-1進行更正，有效的抑制濾波器的發散。加權后新的狀態估計均方誤差陣Pk/k-1為[12]

(17)

把式(17)代入式(15)得

(18)

推出Ck的表達式為

(19)

在BDS和INS組合導航時，其中動態量測值數據處理過程中，檢測到濾波異常時，在Sage-Husa自適應卡爾曼濾波收斂條件下，用Ck調整狀態估計均方誤差陣Pk/k-1，能提高濾波的精度，并且還能保證此濾波算法的收斂性，抑制濾波發散[13]。

4.3 改進的Sage-Husa自適應卡爾曼濾波具體算法

當新息失配嚴重時，往往伴隨量測Zk出現異常波動，用Sage-Husa自適應卡爾曼濾波算法直接濾波[14]，會出現濾波發散，通過新的狀態估計均方誤差陣對原來的狀態估計均方誤差陣進行更正，能有效抑制了濾波器的發散。提高了實際應用中，Sage-Husa自適應卡爾曼濾波算法在復雜不穩定環境中的穩定性和可靠性。組合導航中系統噪聲一般被認為是穩定的，即Q是常量，設q=0，q=0，以下為算法具體流程：

步驟1：狀態一步預測。

k/k-1=Φk，k-1k-1

(20)

步驟2：計算新息。

(21)

步驟3：量測濾波異常檢測。

(22)

步驟4：對狀態一步預測均方誤差陣，如果式(22)不成立，用式(23)進行求解，否則用式(24)進行求解。

(23)

(24)

步驟5：計算濾波增益。

(25)

步驟6：計算狀態估計。

(26)

步驟7：計算狀態估計均方誤差陣。

Pk=(I-KkHk)Pk/k-1

(27)

步驟8：計算時變量測噪聲。

(28)

綜上所述，當新息失配嚴重時，往往伴隨量測出現異常波動，導致濾波發散，與Sage-Husa自適應濾波算法相比，改進的Sage-Husa自適應濾波算法引入了濾波異常檢測，當檢測到濾波異常時，利用次優濾波算法對Sage-Husa自適應濾波算法的狀態估計均方誤差陣進行優化，在最嚴格的收斂判斷條件下，計算出次優濾波方法中新的狀態估計均方誤差陣的加權系數，通過新的狀態估計均方誤差陣對原來的狀態估計均方誤差陣進行更正，改變了在濾波系統中，模型的不準和噪聲統計特征不確定等情況下對估計均方誤差陣的影響[15]，有效地抑制了此濾波器的信號發散，提高此濾波系統的穩定性與可靠度。

5 仿真與分析

在相同的仿真條件下，采用KF、Sage-Husa自適應濾波算法和改進的Sage-Husa自適應濾波算法分別對BDS/INS系統進行濾波估計，為了更加直觀的對比這三種不同濾波的性能，通過對比三種不同濾波算法校準后輸出的位置和速度誤差來驗證改進Sage-Husa自適應濾波算法的效果。圖1為東向和北向速度誤差，圖2為東向和北向位置誤差。

圖1 組合導航東向和北向速度誤差對比

由圖1可知，在濾波全部過程中，三種濾波算法的誤差都沒有隨著時間積累，但相對于KF，Sage-Husa自適應濾波算法和改進的Sage-Husa自適應濾波算法明顯穩定性更好，這是由于KF濾波是采用固定的和，當與實際模型誤差有較大差別時會讓誤差增大，甚至引發濾波發散。其中改進的Sage-Husa自適應濾波算法速度誤差最小，KF濾波和Sage-Husa自適應濾波算法速度誤差較小，這是由于改進的Sage-Husa自適應濾波算法不僅可以抑制濾波的發散，而且在歷史觀察信息誤差較大時，逐漸減小歷史觀察信息對當前的影響，令速度誤差絕對值明顯降低，提高系統模型的穩定性和可靠性。因此，改進的算法相比如另外兩種濾波算法，濾波效果更佳。

圖2 組合導航東向和北向位置誤差對比

由圖2可以看出改進的Sage-Husa自適應濾波算法無論是在濾波收斂時間上，還是濾波效果上，都是優于另外兩種濾波的。具體的來說，在BDS/INS組合導航系統中，由于慣導器件陀螺儀、加速度計的精度值較低，并且復雜的外部環境的影響，使得系統模型的噪聲統計特性不準確，從而讓卡爾曼濾波器設置的參數與實際濾波條件不符合，致狀態估計的精度下降，使得卡爾曼濾波的效果逐漸減弱，最后導致導航誤差增大。

6 結論

Sage-Husa自適應濾波算法相較于標準KF算法能增強系統在導航性能要求苛刻任務中的穩定性和可靠性。但當新息失配嚴重時，往往伴隨量測出現異常波動，使得卡爾曼濾波器的設置參數與當前實際濾波條件不符，致狀態估計的精度下降，使得卡爾曼濾波的效果逐漸減弱，最后導致導航誤差增大。本文針對這些問題，提出了最佳加權系數的求解，修正模型預測誤差方差陣，改進Sage-Husa自適應濾波算法的結構。通過仿真對比分析標準KF算法、Sage-Husa自適應濾波算法和改進Sage-Husa自適應濾波算法的濾波效果，可以得出如下結論。

1)由于模型誤差的影響，導致濾波發散，通過引進次優濾波的方法，利用次優濾波算法對Sage-Husa自適應濾波算法的狀態估計均方誤差陣進行優化，提高了濾波器的精度和實用性。

2)改進Sage-Husa自適應濾波算法能抑制濾波發散，并且還能同時提高系統的穩定性和可靠性。

3)改進Sage-Husa自適應濾波算法相較于其它兩種算法，能使該組合導航系統適應更多對導航性能要求苛刻的任務中，使濾波性能更加完善。