基于多重漸消因子的強(qiáng)跟蹤SVDCKF組合導(dǎo)航算法

熊 鑫，黃國勇，王曉東

(1.昆明理工大學(xué) 信息工程與自動(dòng)化學(xué)院，昆明 650500；2.云南省礦物管道輸送工程技術(shù)研究中心，昆明 650500)

0 引 言

由捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)與北斗衛(wèi)星導(dǎo)航系統(tǒng)(BeiDou navigation satellite system，BDS)組成的組合導(dǎo)航系統(tǒng)，在飛機(jī)和車輛載體的定位與姿態(tài)估計(jì)中有著廣泛的應(yīng)用[1]，目前擴(kuò)展卡爾曼濾波(extended Kalman filter, EKF)算法和無跡卡爾曼濾波(unscented Kalman filter, UKF)算法是解決導(dǎo)航中狀態(tài)估計(jì)的主要途徑[2-3]，但存在一定的局限性。根據(jù)貝葉斯理論以及球面徑向規(guī)則推導(dǎo)的容積卡爾曼濾波(cubature Kalman filter, CKF)算法是近年來提出的一種新型非線性濾波算法，CKF算法與UKF算法相比，都屬于確定采樣型濾波算法，但CKF算法估計(jì)精度更高，實(shí)時(shí)性更好[4-5]。然而，任何一種卡爾曼濾波器都要求精確已知系統(tǒng)噪聲的先驗(yàn)統(tǒng)計(jì)信息，所以在載體狀態(tài)出現(xiàn)突變，系統(tǒng)過程噪聲統(tǒng)計(jì)特性存在不確定性的情況下會出現(xiàn)濾波精度下降甚至發(fā)散的現(xiàn)象[6-7]。有大量學(xué)者對這一問題進(jìn)行了研究，并提出了基于變分貝葉斯的自適應(yīng)卡爾曼濾波器[8-9]、基于最大相關(guān)熵的卡爾曼濾波器[10]、基于Huber-M估計(jì)的濾波器[11]以及基于交互多模的自適應(yīng)濾波器[12-13]。變分貝葉斯自適應(yīng)卡爾曼濾波器可以通過分解系統(tǒng)狀態(tài)和噪聲方差的聯(lián)合后驗(yàn)分布來近似估計(jì)時(shí)變系統(tǒng)過程噪聲方差的統(tǒng)計(jì)量，但該濾波器中使用的方差參數(shù)設(shè)計(jì)的動(dòng)態(tài)模型是相對粗糙的，當(dāng)過程噪聲協(xié)方差頻繁變化時(shí)，估計(jì)精度會降低；基于最大熵的卡爾曼濾波器能夠通過最大化預(yù)測誤差和殘差的熵來抑制較大的系統(tǒng)過程噪聲不確定性，但在實(shí)際應(yīng)用中確定內(nèi)核帶寬大小的適當(dāng)值非常困難，這可能會降低估計(jì)性能；基于Huber-M估計(jì)的卡爾曼濾波器通過使用M估計(jì)器的代價(jià)函數(shù)來構(gòu)造加權(quán)矩陣，該加權(quán)矩陣可以重新調(diào)整先驗(yàn)狀態(tài)估計(jì)協(xié)方差，從而抑制系統(tǒng)過程噪聲不確定性，但其估計(jì)的精度有限；基于交互多模的自適應(yīng)濾波器針對可能的噪聲情況建立一組非線性模型來應(yīng)對噪聲的不確定性，但當(dāng)噪聲變化較大時(shí)則需要建立更多的模型來匹配，導(dǎo)致難以實(shí)現(xiàn)。

文獻(xiàn)[14]根據(jù)新息正交原理提出的一種強(qiáng)跟蹤濾波器，是目前最為可行的一種濾波器。與其他的濾波器相比，強(qiáng)跟蹤濾波器(strong tracking filter, STF)在模型參數(shù)變化或系統(tǒng)發(fā)生突變時(shí)具有較強(qiáng)的跟蹤能力，且對噪聲及初值的統(tǒng)計(jì)特性有較低的敏感性。文獻(xiàn)[15]將強(qiáng)跟蹤理論與CKF算法框架相結(jié)合提出了一種強(qiáng)跟蹤自適應(yīng)CKF算法，該算法通過引入單重次漸消因子對預(yù)測誤差協(xié)方差陣進(jìn)行調(diào)整以實(shí)現(xiàn)對狀態(tài)的強(qiáng)跟蹤，該算法能一定程度上解決系統(tǒng)過程噪聲統(tǒng)計(jì)特性不清的問題，但由于無法對每個(gè)濾波通道進(jìn)行不同的調(diào)節(jié)，不能較好地應(yīng)用于復(fù)雜的多變量系統(tǒng)。

針對BDS/INS組合導(dǎo)航系統(tǒng)在載體存在狀態(tài)突變時(shí)，由于系統(tǒng)過程噪聲統(tǒng)計(jì)特性的不確定性增加，導(dǎo)致標(biāo)準(zhǔn)CKF濾波精度下降的問題，本文以STF理論為基礎(chǔ)，并通過與CKF框架的結(jié)合，提出了一種基于多重漸消因子的強(qiáng)跟蹤奇異值分解的容積卡爾曼濾波(cubature Kalman filter using singular value decomposition，SVDCKF)組合導(dǎo)航算法。本算法采用卡方檢驗(yàn)對系統(tǒng)狀態(tài)進(jìn)行檢測，以判斷是否引入多重漸消因子對預(yù)測協(xié)方差陣進(jìn)行調(diào)整，并在系統(tǒng)狀態(tài)的協(xié)方差矩陣的分解迭代用數(shù)值穩(wěn)定性更強(qiáng)的SVD分解代替原來的Cholesky分解。最后通過INS/BDS仿真實(shí)驗(yàn)，對所提出算法的性能進(jìn)行評估,并與CKF算法[4]，STCKF算法[15]進(jìn)行比較，所得結(jié)果也表明了本文方法的有效性。

1 SVDCKF算法

對于標(biāo)準(zhǔn)CKF算法，在計(jì)算容積點(diǎn)時(shí)需對誤差協(xié)方差矩陣進(jìn)行Cholesky分解，在進(jìn)行多次濾波后，由于計(jì)算機(jī)的截?cái)嗾`差容易導(dǎo)致誤差協(xié)方差矩陣失去正定性和對稱性，使得濾波發(fā)散。奇異值分解數(shù)值計(jì)算的魯棒性更強(qiáng)，將奇異值分解代替原來的Cholesky分解對誤差協(xié)方差矩陣進(jìn)行分解迭代，能有效提高CKF算法的穩(wěn)定性[16]。

對于INS/BDS組合導(dǎo)航非線性離散系統(tǒng)建立如下

(1)

對于非線性系統(tǒng)(1)，SVDCKF的計(jì)算如下。

步驟1初始化。

(2)

步驟2時(shí)間更新。

對Pk-1/k-1進(jìn)行奇異值分解

(3)

(3)式中，Sk-1為對角矩陣，即

Sk-1=diag[S1,S2,S3,…,Sn]

計(jì)算容積點(diǎn)

i=1,2,3,…,2n

(4)

(4)式中：i為容積點(diǎn)序號；

通過系統(tǒng)函數(shù)傳播容積點(diǎn)

(5)

計(jì)算一步預(yù)測狀態(tài)值和對應(yīng)的協(xié)方差值分別為

(6)

(7)

步驟3量測更新。

對Pk/k-1進(jìn)行奇異值分解

(8)

計(jì)算新的容積點(diǎn)

(9)

通過量測函數(shù)傳播容積點(diǎn)

Zi,k/k-1=HkXi,k/k-1

(10)

k時(shí)刻的觀測預(yù)測值

(11)

殘差ηk和自相關(guān)協(xié)方差Pzz,k/k-1分別為

(12)

(13)

互相關(guān)協(xié)方差

(14)

步驟4狀態(tài)更新。

k時(shí)刻濾波增益

(15)

k時(shí)刻狀態(tài)估計(jì)值

(16)

k時(shí)刻狀態(tài)誤差協(xié)方差估計(jì)值

(17)

2 基于多重漸消因子的強(qiáng)跟蹤SVDCKF

相比于標(biāo)準(zhǔn)CKF，SVDCKF保證了誤差協(xié)方差矩陣的對稱性和正定性，提高了濾波迭代的穩(wěn)定性，但SVDCKF本身仍具有局限性，當(dāng)系統(tǒng)到達(dá)穩(wěn)態(tài)時(shí)，濾波增益Kk將趨于最小值，如果系統(tǒng)狀態(tài)發(fā)生突變，會引起殘差的增大，但是濾波增益Kk并不會隨著殘差的增大而增大，仍保持最小值，這會使SVDCKF喪失對突變狀態(tài)的跟蹤能力[17]。為此，本文引入強(qiáng)跟蹤濾波的思想來解決這個(gè)問題。

2.1 STF原理

強(qiáng)跟蹤濾波通過對預(yù)測協(xié)方差Pk/k-1引入次優(yōu)漸消因子λ，在線調(diào)整濾波增益Kk，強(qiáng)迫殘差序列ηk保持正交，使得濾波器在狀態(tài)突變時(shí)仍能保持對真實(shí)狀態(tài)的強(qiáng)跟蹤[18]。即有

(18)

(19)

2.2 漸消因子的計(jì)算

為了使不同的濾波器通道提供不同的漸消速率，用漸消因子矩陣代替單個(gè)的衰落因子。同時(shí)，為避免引起矩陣Pk/k-1不對稱的問題，改進(jìn)的預(yù)測協(xié)方差可表示為

(20)

當(dāng)濾波器穩(wěn)定時(shí)，殘差序列是高斯白噪聲，且有

(21)

(21)式中，Vk為濾波器實(shí)際輸出的殘差協(xié)方差陣，計(jì)算式為

(22)

(22)式中，0<ρ≤1為遺忘因子，通常取ρ=0.95。

定義參數(shù)為

(23)

(24)

在導(dǎo)航和定位應(yīng)用中，量測矩陣通?？梢员硎緸?/p>

Hk=[Sm×m0m×(n-m)]m×n

(25)

(25)式中：Sm×m=diag(S1,S2,…,Sm)。

(23)式可表示為

(26)

由(21)，(23)，(24)，(26)式可得

(27)

(27)式中，Nii，Jii為矩陣Nk，Jk中第i行，第i列對角線上的元素，由(27)式可得多重漸消因子的計(jì)算方法為

(28)

2.3 系統(tǒng)狀態(tài)評估

卡爾曼濾波的正交性原理[19]：在卡爾曼濾波理論中，當(dāng)系統(tǒng)不存在模型誤差且濾波穩(wěn)定時(shí)，新息殘差序列為零均值的高斯白噪聲，且殘差保持正交。

根據(jù)卡爾曼濾波的正交性原理，殘差馬氏距離平方服從卡方分布[20]，即

(29)

本文通過卡方檢驗(yàn)對系統(tǒng)進(jìn)行評估，如果存在系統(tǒng)狀態(tài)突變，殘差馬氏距離的平方將不再服從自由度為n的卡方分布，作出如下假設(shè)。

假設(shè)0γ0：系統(tǒng)狀態(tài)正常。

假設(shè)1γ1：系統(tǒng)存在狀態(tài)突變。

2.4 算法實(shí)現(xiàn)

考慮非線性離散系統(tǒng)(1)，MST-SVDCKF的主要計(jì)算過程如下。

步驟1根據(jù)(2)式初始化狀態(tài)估計(jì)值和協(xié)方差。

步驟2時(shí)間更新。根據(jù)(3)—(6)式計(jì)算狀態(tài)一步預(yù)測值。

步驟3利用卡方檢驗(yàn)對系統(tǒng)模型進(jìn)行評估，若存在狀態(tài)異常，則根據(jù)(20)式更新狀態(tài)協(xié)方差，否則根據(jù)(7)式計(jì)算。

步驟4量測更新。根據(jù)(8)—(14)式計(jì)算自相關(guān)協(xié)方差和互相管協(xié)方差。

步驟5狀態(tài)更新。根據(jù)(16)，(17)式計(jì)算狀態(tài)估計(jì)及其協(xié)方差陣。

步驟6令xk-1=xk/k，Pk-1=Pk/k，k=k+1，返回步驟2，進(jìn)行下一次循環(huán)迭代。

3 仿真分析

3.1 仿真設(shè)置

SINS/BDS組合導(dǎo)航系統(tǒng)采用SINS系統(tǒng)為主系統(tǒng)解算得到姿態(tài)、速度和位置信息，并通過BDS系統(tǒng)的信息定期對SINS進(jìn)行反饋修正。建立的狀態(tài)方程為

xk=f(xk-1)+ωk-1

(30)

(30)式中：f(·)為非線性函數(shù)，由SINS的力學(xué)編排和誤差方程得到；ωk-1為系統(tǒng)過程噪聲向量，xk為15維的狀態(tài)向量，表示為

xk=[δrx,δry,δrz,δvx,δvy,δvz,δψx,δψy,

δψz,εx,εy,εz,Δx,Δy,Δz]

(31)

(31)式中：δrx,δry,δrz為三軸位置誤差；δvx,δvy,δvz為三軸速度誤差；δψx,δψy,δψz為三軸姿態(tài)角誤差；εx,εy,εz為三軸陀螺儀零漂；Δx,Δy,Δz為三軸加速度計(jì)零偏。加速度計(jì)和陀螺儀的誤差建模為一階馬爾科夫過程。量測方程建立如下

Zk=[ZSINS-ZGPS]=Hxk+vk=

[I6×606×9]xk+vk

(32)

ZSINS=[rSINSx,rSINSy,rSINSz,vSINSx,vSINSy,vSINSz]

ZGPS=[rGPSx,rGPSy,rGPSz,vGPSx,vGPSy,vGPSz]

(32)式中：ZSINS為SINS解算得到的三軸速度和三軸位；ZGPS為BDS輸出的三軸速度和三軸位置。仿真中BDS采樣周期為1 s，SINS的采樣周期為0.02 s，濾波周期為1 s，仿真其他參數(shù)設(shè)置如表1。

載體在0～77 s進(jìn)行了滑跑、加速拉起、爬高、改平飛行；75～200 s一直為平直飛行；200～220 s進(jìn)行了一次大機(jī)動(dòng)轉(zhuǎn)彎；221～440 s一直為平直飛行；441～600 s進(jìn)行了連續(xù)機(jī)動(dòng)包括3次轉(zhuǎn)彎和改平飛行以及一次俯沖和改平飛行。飛行軌跡如圖1。

圖1 飛機(jī)飛行軌跡Fig.1 Trace of the airplane

表1 仿真參數(shù)

3.2 仿真結(jié)果及分析

分別采用CKF，STCKF和本文所提出MST-SVDCKF對INS/BDS組合導(dǎo)航系統(tǒng)進(jìn)行仿真，結(jié)果如圖3—圖5。3種算法在直線飛行時(shí)都具有較好的濾波精度。在210～220 s和450～600 s期間，載體機(jī)動(dòng)較大，CKF算法的濾波精度明顯下降，CKF在載體大機(jī)動(dòng)的情況下，三維速度誤差和位置誤差分別達(dá)到了0.62 m/s和11.25 m；STCKF在檢查出狀態(tài)突變后，可通過實(shí)時(shí)對狀態(tài)協(xié)方差矩陣的調(diào)節(jié)，得到優(yōu)于CKF的濾波精度，其三維速度誤差和位置誤差分別為0.30 m/s和5.86 m；相比于只能產(chǎn)生相同漸消速率的STCKF，由于MST-SVDCKF可以對不同的狀態(tài)產(chǎn)生不同的漸消速率，所以，本文所提出的算法具有更強(qiáng)的調(diào)節(jié)能力，在載體機(jī)動(dòng)較大時(shí)，三維速度誤差和位置誤差仍能保持在0.146 m/s和3.543 m。

圖2 采用標(biāo)準(zhǔn)CKF和MST-SVDCKF時(shí)的卡方檢驗(yàn)統(tǒng)計(jì)量Fig.2 Chi-square test value as standard CKF and MST-SVDCKF is used

表2 精度和解算時(shí)間比較(RMSE)

圖3 姿態(tài)誤差Fig.3 Estimation error of attitude

圖4 速度誤差Fig.4 Estimation error of velocity

圖5 位置誤差Fig.5 Estimation error of position

表2給出了不同算法在210～220 s，450～600 s的東、北、天的均方誤差?？梢钥闯觯疚乃岢龅乃惴ň哂凶罡叩臑V波精度，在噪聲不確定性增加時(shí)，仍能保持對真實(shí)狀態(tài)的強(qiáng)跟蹤。對比CKF，STCKF，MST-SVDCKF的算法運(yùn)行時(shí)間，雖然本算法的平均解算時(shí)間略長，但仍然保持在一個(gè)較低的水平，能夠滿足實(shí)際現(xiàn)場應(yīng)用的實(shí)時(shí)性要求。

4 總 結(jié)

本文提出了一種基于多重漸消因子的強(qiáng)跟蹤SVDCKF算法。該算法首先引入SVD分解代替原來的Cholesky分解，解決了協(xié)方差矩陣可能非正定的問題，提高了數(shù)值計(jì)算的穩(wěn)定性，然后通過卡方檢驗(yàn)對系統(tǒng)狀態(tài)進(jìn)行評估，當(dāng)系統(tǒng)出現(xiàn)狀態(tài)突變時(shí)，采用多重漸消因子對代替單一漸消因子對預(yù)測狀態(tài)協(xié)方差陣進(jìn)行調(diào)節(jié)，使得不同濾波通道具有不同的漸消能力，調(diào)節(jié)能力更強(qiáng)，可以更好實(shí)現(xiàn)對多變量系統(tǒng)狀態(tài)的強(qiáng)跟蹤。將此算法用于SINS/BDS組合導(dǎo)航系統(tǒng)的仿真實(shí)驗(yàn)中，結(jié)果表明，本文提出的MST-SVDCKF能對系統(tǒng)狀態(tài)進(jìn)行判斷，并在系統(tǒng)噪聲不確定性增加時(shí)，自適應(yīng)地調(diào)節(jié)預(yù)測狀態(tài)協(xié)方差陣。對比CKF和傳統(tǒng)的STCKF，本算法調(diào)節(jié)能力更強(qiáng)，濾波精度更高。