gpsins組合導(dǎo)航中自適應(yīng)卡爾曼濾波算法

gpsins組合導(dǎo)航中自適應(yīng)卡爾曼濾波算法

由于gps組合導(dǎo)航的互補(bǔ)性和定位的精度，gps聯(lián)合定位受到了廣泛的關(guān)注。這種信息整合通常采用卡爾曼濾波算法。標(biāo)準(zhǔn)的卡爾曼濾波算法(SKF)是已知噪聲統(tǒng)計(jì)特性時(shí)的最優(yōu)估計(jì),但在實(shí)際情況中,可見(jiàn)衛(wèi)星數(shù)目、多路徑效應(yīng)以及儀器內(nèi)部測(cè)量噪聲等多種因素會(huì)造成GPS測(cè)量噪聲的變化,SKF由于無(wú)法對(duì)此進(jìn)行檢測(cè)調(diào)節(jié)將導(dǎo)致精度下降甚至發(fā)散。為此人們提出了多種自適應(yīng)濾波算法(AKF),目前其研究方向主要集中在基于信息的自適應(yīng)估計(jì)(IAE)和基于多模型的自適應(yīng)估計(jì)(MMAE)。IAE是利用信息序列估計(jì)噪聲協(xié)方差陣使其自適應(yīng)于觀測(cè)信息,典型代表為sage-husa算法;MMAE是通過(guò)并行運(yùn)算各濾波模型的狀態(tài)估計(jì),自適應(yīng)加權(quán)得系統(tǒng)狀態(tài)的總體估計(jì)。此外,將上述方法與模糊邏輯、漸消濾波、抗差估計(jì)、神經(jīng)網(wǎng)絡(luò)等相結(jié)合,又發(fā)展了一些自適應(yīng)方法。上述AKF算法,其調(diào)節(jié)思想均是從卡爾曼濾波方程結(jié)構(gòu)關(guān)系出發(fā),獲得觀測(cè)量與濾波參數(shù)或與被估狀態(tài)之間的自適應(yīng)調(diào)節(jié)關(guān)系,并未充分考慮各子系統(tǒng)之間的測(cè)量關(guān)系。文獻(xiàn)提出一種改進(jìn)的GPS/DR組合位置自適應(yīng)濾波算法,考慮了GPS、DR定位誤差性質(zhì),獲得對(duì)GPS測(cè)量誤差的有效估計(jì)。本論文給出了該思想下GPS觀測(cè)噪聲統(tǒng)計(jì)性質(zhì)的理論證明,通過(guò)深入分析GPS、INS的測(cè)量性質(zhì),提出了基于測(cè)量特性的自適應(yīng)濾波算法(MAKF),針對(duì)GPS測(cè)量噪聲進(jìn)行實(shí)時(shí)有效的估計(jì),提高了定位精度和濾波穩(wěn)定性。1makf算法的理論分析1.1gps的傳播特性慣導(dǎo)系統(tǒng)是利用陀螺儀和加速度計(jì)等慣性器件的實(shí)時(shí)量測(cè),依據(jù)力學(xué)、運(yùn)動(dòng)學(xué)規(guī)律,遞推解算得載體的導(dǎo)航信息,其定位精度隨系統(tǒng)誤差的累計(jì)而逐漸降低。GPS是由接收的衛(wèi)星信號(hào)解算載體的導(dǎo)航信息,具有長(zhǎng)期穩(wěn)定性,但其信號(hào)的接收易受到天氣、周圍環(huán)境以及機(jī)動(dòng)等因素的影響?？梢?jiàn)慣導(dǎo)系統(tǒng)和GPS具有互補(bǔ)性質(zhì)的誤差傳播性能,前者長(zhǎng)期穩(wěn)定性差、短期精度高,后者反之。因此,可利用慣導(dǎo)系統(tǒng)的短期高精度性,研究GPS測(cè)量噪聲的統(tǒng)計(jì)特性。1.1誤差趨勢(shì)項(xiàng)設(shè)k時(shí)刻,INS、GPS對(duì)同一導(dǎo)航信息量X(k)的輸出值分別為XINS(k)、XGPS(k),有:其中,fINS(k)為誤差趨勢(shì)項(xiàng),WINS(k)、VGPS(k)為零均值白噪聲。分別求INS、GPS單系統(tǒng)差分序列得:求雙系統(tǒng)互差分序列,消去X(k)的變化量:由于慣導(dǎo)系統(tǒng)短期定位精度高于GPS,則由于慣導(dǎo)系統(tǒng)1s內(nèi)誤差累計(jì)增量很小,則:忽略小項(xiàng),則式(7)為:則有GPS觀測(cè)噪聲方差R的估計(jì):1.2慣導(dǎo)系統(tǒng)誤差增量的分析以慣導(dǎo)系統(tǒng)在1s內(nèi)定位誤差的增量進(jìn)行分析,假設(shè)載體運(yùn)動(dòng)速度v=1000m/s,速度方向誤差增量Δθ=1′,則Δt=1s時(shí)位置誤差增量為:可見(jiàn),即使在2.9倍音速和大誤差角增量的假設(shè)下,慣導(dǎo)系統(tǒng)1s內(nèi)的位置誤差增量只有0.3m,而GPS定位誤差一般為10m數(shù)量級(jí),因此,1s內(nèi)慣導(dǎo)誤差增量的平方值遠(yuǎn)小于GPS定位誤差的方差值,故滿足式(9)。2噪聲統(tǒng)計(jì)分析(1)求GPS、INS單系統(tǒng)差分序列其中,k為GPS測(cè)量時(shí)刻,Δ為單系統(tǒng)差分序列,XGPS(k)、XINS(k)分別為GPS、INS在k時(shí)刻的輸出。因GPS、INS測(cè)量頻率不同,故GPS差分序列為每秒測(cè)量之差,INS差分序列則需進(jìn)行1s慣性遞推運(yùn)算,然后獲得測(cè)量之差。(2)求GPS、INS雙系統(tǒng)互差分序列將式(12)中單系統(tǒng)差分序列依次做差,得相應(yīng)時(shí)刻的雙系統(tǒng)互差分序列,記為:(3)雙系統(tǒng)互差分序列統(tǒng)計(jì)由式(11)可知,雙系統(tǒng)互差分序列含GPS測(cè)量噪聲信息,因此有效統(tǒng)計(jì)式(13)序列可獲得GPS測(cè)量噪聲。當(dāng)噪聲為同分布時(shí),則樣本容量越大,統(tǒng)計(jì)結(jié)果的可信度越高;當(dāng)噪聲分布特性發(fā)生變化時(shí),則統(tǒng)計(jì)樣本容量越大,動(dòng)態(tài)信息的表征越不明顯,即當(dāng)前時(shí)刻的動(dòng)態(tài)信息淹沒(méi)在大量的歷史信息中,導(dǎo)致統(tǒng)計(jì)結(jié)果不能及時(shí)跟蹤變化的噪聲信息,不滿足實(shí)時(shí)性要求。故本論文設(shè)計(jì)了窗口為M的連續(xù)滑動(dòng)窗口法,進(jìn)行方差統(tǒng)計(jì),統(tǒng)計(jì)公式為:其中,R為GPS測(cè)量噪聲方差,i為當(dāng)前時(shí)刻,Ck為雙系統(tǒng)互差分序列,M為窗口寬度,一般取30~50。(4)野值剔除當(dāng)進(jìn)行小樣本統(tǒng)計(jì)時(shí),野值點(diǎn)的存在將導(dǎo)致統(tǒng)計(jì)結(jié)果產(chǎn)生較大誤差。為此,本論文設(shè)計(jì)如下野值點(diǎn)剔除方法:當(dāng)樣本點(diǎn)與樣本均值的差值大于α倍的標(biāo)準(zhǔn)差時(shí),為野值點(diǎn)序列,將其置為樣本均值,重新進(jìn)行統(tǒng)計(jì)。野值點(diǎn)判別條件為:其中,Ck為雙系統(tǒng)互差分序列,i為當(dāng)前時(shí)刻,M為窗口寬度;α為剔除因子,一般取2.5~5。(5)觀測(cè)噪聲協(xié)方差陣R的估計(jì)由式(14)所得可表征GPS的測(cè)量噪聲信息,但由于是小樣本統(tǒng)計(jì)結(jié)果的計(jì)算值變化比較劇烈。而卡爾曼濾波理論是建立在噪聲統(tǒng)計(jì)特性基礎(chǔ)上的遞推估計(jì),濾波方程中觀測(cè)噪聲協(xié)方差陣R不會(huì)劇烈變化,故本論文采用指數(shù)加權(quán)法更新R,見(jiàn)式(16)。所謂指數(shù)加權(quán),是指對(duì)于歷史測(cè)量數(shù)據(jù)和當(dāng)前測(cè)量數(shù)據(jù)給予不同的權(quán)重系數(shù),使得當(dāng)前數(shù)據(jù)在估計(jì)中發(fā)揮主要作用,而使歷史數(shù)據(jù)逐漸遺忘:其中,b為遺忘因子,0<b<1,一般取0.95~0.995。3makf算法用于gps組合導(dǎo)航3.1載體系的姿態(tài)轉(zhuǎn)移矩陣采用GPS/INS速度位置組合方式,構(gòu)建GPS/INS組合導(dǎo)航系統(tǒng)濾波模型。以平臺(tái)誤差角、速度誤差、位置誤差以及慣性器件誤差為狀態(tài)建立15維狀態(tài)方程:上述各式中,下標(biāo)e、n、u為導(dǎo)航系的東、北、天軸向,下標(biāo)x、y、z為載體系的右、前、上軸向,φeφ、nφ、u為平臺(tái)誤差角,δve、δvn、δvu為速度誤差,δLδ、λδ、h分別為緯度誤差、經(jīng)度誤差、高度誤差,εbxε、byε、bz為陀螺的隨機(jī)常值漂移,wgxw、gyw、gz為陀螺的隨機(jī)漂移,?x?、y?、z為加速度計(jì)的隨機(jī)常值漂移,waxw、ayw、az為加速度計(jì)的隨機(jī)漂移,Cbn為載體系到導(dǎo)航系的姿態(tài)轉(zhuǎn)移矩陣。以慣導(dǎo)系統(tǒng)與GPS的位置差、速度差作為觀測(cè)量,建立6維觀測(cè)方程:上述式中,下標(biāo)INS、GPS分別為INS、GPS相應(yīng)的測(cè)量值,下標(biāo)e、n、u為導(dǎo)航系的東、北、天軸向,Ne、Nn、Nu為GPS接收機(jī)的位置誤差,Me、Mn、Mu為GPS接收機(jī)的速度誤差,Rm、Rn分別為子午圈、卯酉圈上各點(diǎn)的曲率半徑。3.2基于模型的組合導(dǎo)航濾波仿真初始位置為東經(jīng)116°,北緯39.2°,高度1000m,速度300m/s,航向角35°,飛行7200s。陀螺隨機(jī)常值漂移0.01(°)/h,隨機(jī)漂移0.005(°)/h,加速度計(jì)隨機(jī)常值漂移100μg,隨機(jī)漂移50μg。GPS速度誤差0.1m/s,高度誤差120m,經(jīng)緯度誤差為15m。為了考察系統(tǒng)對(duì)GPS測(cè)量抗干擾的效果,GPS經(jīng)緯度誤差在1000~3000s時(shí)段增為40m,如圖1(a)(c)所示。(2)對(duì)比算法為了深入分析MAKF算法的濾波效果,本文采用改進(jìn)的sage-husa自適應(yīng)算法進(jìn)行同條件濾波解算。該算法是針對(duì)sage-husa算法計(jì)算復(fù)雜、高階不穩(wěn)定等缺陷進(jìn)行的工程化改進(jìn),在高精度組合導(dǎo)航系統(tǒng)中具有優(yōu)于SKF算法的濾波效果,其濾波公式為:3)仿真結(jié)果分析比較在條件(1)下,分別采用SKF、改進(jìn)的sage-husa算法、MAKF算法進(jìn)行GPS/INS組合導(dǎo)航濾波解算,仿真結(jié)果如圖1所示。由圖1(a)(c)可看出,相同初始條件下,SKF算法的經(jīng)緯度測(cè)量噪聲方差R始終為預(yù)設(shè)經(jīng)驗(yàn)值(100m2),而兩種AKF算法的R可動(dòng)態(tài)跟蹤實(shí)際的GPS測(cè)量噪聲。由圖1(b)(d)可看出:(1)SKF算法的經(jīng)緯度誤差抖動(dòng)較大,尤其在1000~3000s時(shí)段,而兩自適應(yīng)算法的誤差值抖動(dòng)較小;(2)在4000~6000s時(shí)段,改進(jìn)sage-husa算法經(jīng)緯度值有較大偏差,而MAKF算法的經(jīng)緯度誤差值在零值附近抖動(dòng)。由圖1可見(jiàn),Kalman濾波算法狀態(tài)估計(jì)的準(zhǔn)確程度取決于一步預(yù)測(cè)信息和測(cè)量新息的使用權(quán)重,而權(quán)重陣K受R陣的影響。對(duì)于SKF算法,由于其R不能跟蹤GPS測(cè)量噪聲變化導(dǎo)致K陣未合理調(diào)節(jié),因此濾波結(jié)果抖動(dòng)較大且在測(cè)量噪聲增大時(shí)段(1000~3000s)抖動(dòng)更嚴(yán)重。兩種AKF算法,R陣可跟隨GPS測(cè)量噪聲變化而變化,且MAKF算法對(duì)R具有更高的估計(jì)精度,因此濾波效果更好。另一方面,權(quán)重陣K還受到估計(jì)誤差方差陣P的影響。由式(19)可知,改進(jìn)sage-husa算法R陣和P陣之間存在迭代遞推關(guān)系,而在濾波過(guò)程中P陣并不能反映估計(jì)誤差的真實(shí)情況,因此,P陣的誤差會(huì)耦合到R陣中,R陣的誤差也會(huì)影響到P陣的更新,而R陣、P陣都會(huì)影響到K陣的調(diào)節(jié),所以導(dǎo)致了改進(jìn)sage-husa算法的定位誤差產(chǎn)生較大偏差。由式(14)(16)可知,MAKF算法是統(tǒng)計(jì)差值序列獲得R,無(wú)需P陣信息,所以在濾波過(guò)程中不會(huì)發(fā)生誤差耦合現(xiàn)象。當(dāng)將條件(1)中陀螺隨機(jī)常值漂移增為0.1(°)/h,隨機(jī)漂移增為0.05(°)/h,其他條件保持不變時(shí),濾波結(jié)果如圖2所示。由圖2可以看出,當(dāng)慣導(dǎo)系統(tǒng)精度降低時(shí),改進(jìn)sage-husa算法中誤差耦合現(xiàn)象更為嚴(yán)重,經(jīng)度方向R的估計(jì)已不可信,導(dǎo)致濾波誤差很大,而MAKF算法仍能保持較高精度和良好的濾波穩(wěn)定性。4自適應(yīng)估計(jì)實(shí)驗(yàn)本文在深入分析GPS、INS測(cè)量性質(zhì)的基礎(chǔ)上,提出了基于測(cè)量特性的自適應(yīng)濾波算法(MAKF),利用慣導(dǎo)