改進(jìn)的Sage-Husa自適應(yīng)算法及應(yīng)用

鄧洪高，馬海淘，紀(jì)元法，孫希延

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

1 引言

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

Sage-Husa自適應(yīng)濾波增加了噪聲統(tǒng)計特性的計算過程，隨著濾波的進(jìn)行，在某個時間段內(nèi)，因觀測量誤差增大而導(dǎo)致新息中引入大量誤差，可能會使噪聲陣出現(xiàn)負(fù)定性的情況，引發(fā)濾波結(jié)果發(fā)散，降低系統(tǒng)的穩(wěn)定性和可靠性[4]。

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

2 組合導(dǎo)航系統(tǒng)數(shù)學(xué)模型

建立如下的BDS/INS組合導(dǎo)航系統(tǒng)狀態(tài)方程

(1)

式中：狀態(tài)變量為

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

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

(2)

高斯白噪聲隨機(jī)誤差變量為

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

(3)

誤差系數(shù)矩陣G為

(4)

狀態(tài)系數(shù)矩陣F為

(5)

其中，為9個導(dǎo)航參數(shù)矩陣

(6)

(7)

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

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

(8)

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

(9)

3 Sage-Husa自適應(yīng)卡爾曼濾波算法

3.1 Sage-Husa自適應(yīng)卡爾曼濾波算法

由上述內(nèi)容可知，對組合導(dǎo)航系統(tǒng)離散化后可表示為

(10)

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

E(Vk)=rkvar(Vk)=Rk

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

(11)

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

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

3.2 簡化Sage-Husa 自適應(yīng)卡爾曼濾波算法

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

(12)

4 Sage-Husa自適應(yīng)濾波算法的改進(jìn)

4.1 量測異常檢測

在實際的工程應(yīng)用中，通常是采用基于協(xié)方差匹配的濾波異常檢測法，來實現(xiàn)對量測數(shù)據(jù)異常與動力學(xué)模型異常的檢測。

其中，基于協(xié)方差匹配技術(shù)的濾波發(fā)散的判斷依據(jù)為[10]

(13)

假設(shè)R(k)=R(k-1)，可推導(dǎo)得

(14)

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

(15)

濾波發(fā)散判斷為

(16)

4.2 最佳加權(quán)系數(shù)的選擇

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

在最嚴(yán)格的收斂判斷條件下，計算出次優(yōu)濾波方法中新的狀態(tài)估計均方誤差陣Pk/k-1的加權(quán)系數(shù)，通過新的狀態(tài)估計均方誤差陣Pk/k-1對原來的狀態(tài)估計均方誤差陣Pk/k-1進(jìn)行更正，有效的抑制濾波器的發(fā)散。加權(quán)后新的狀態(tài)估計均方誤差陣Pk/k-1為[12]

(17)

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

(18)

推出Ck的表達(dá)式為

(19)

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

4.3 改進(jìn)的Sage-Husa自適應(yīng)卡爾曼濾波具體算法

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

步驟1：狀態(tài)一步預(yù)測。

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

(20)

步驟2：計算新息。

(21)

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

(22)

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

(23)

(24)

步驟5：計算濾波增益。

(25)

步驟6：計算狀態(tài)估計。

(26)

步驟7：計算狀態(tài)估計均方誤差陣。

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

(27)

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

(28)

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

5 仿真與分析

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

圖1 組合導(dǎo)航東向和北向速度誤差對比

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

圖2 組合導(dǎo)航東向和北向位置誤差對比

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

6 結(jié)論

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

1)由于模型誤差的影響，導(dǎo)致濾波發(fā)散，通過引進(jìn)次優(yōu)濾波的方法，利用次優(yōu)濾波算法對Sage-Husa自適應(yīng)濾波算法的狀態(tài)估計均方誤差陣進(jìn)行優(yōu)化，提高了濾波器的精度和實用性。

2)改進(jìn)Sage-Husa自適應(yīng)濾波算法能抑制濾波發(fā)散，并且還能同時提高系統(tǒng)的穩(wěn)定性和可靠性。

3)改進(jìn)Sage-Husa自適應(yīng)濾波算法相較于其它兩種算法，能使該組合導(dǎo)航系統(tǒng)適應(yīng)更多對導(dǎo)航性能要求苛刻的任務(wù)中，使濾波性能更加完善。