基于高斯過程回歸的平方根UPF算法

孟 陽，高社生，王 維

（西北工業(yè)大學(xué)自動化學(xué)院，陜西西安710072）

基于高斯過程回歸的平方根UPF算法

孟 陽，高社生，王 維

（西北工業(yè)大學(xué)自動化學(xué)院，陜西西安710072）

針對系統(tǒng)動力學(xué)模型不準(zhǔn)確可能導(dǎo)致濾波精度下降，以及系統(tǒng)狀態(tài)協(xié)方差陣可能出現(xiàn)的負(fù)定性問題，提出一種新的高斯過程回歸平方根分解無跡粒子濾波（Gaussian process regression square-root decomposition unscented particle filter，GPSR-UPF）算法。在該算法中，采用高斯過程回歸求取UPF的重要性密度函數(shù)。當(dāng)系統(tǒng)模型不準(zhǔn)確時，通過高斯過程回歸學(xué)習(xí)訓(xùn)練數(shù)據(jù)，進而獲取系統(tǒng)的回歸模型及系統(tǒng)噪聲協(xié)方差，同時引入平方根變換抑制系統(tǒng)狀態(tài)協(xié)方差陣的負(fù)定性。將提出的GPSR-UPF算法應(yīng)用到捷聯(lián)慣導(dǎo)／全球定位系統(tǒng)（strapdown inertial navigation system／global positioning system，SINS／GPS）組合導(dǎo)航系統(tǒng)中進行仿真驗證。結(jié)果表明，所提出濾波算法的性能優(yōu)于基本的無跡粒子濾波算法，能提高組合導(dǎo)航系統(tǒng)的解算精度。

高斯過程回歸；平方根分解；無跡粒子濾波；組合導(dǎo)航系統(tǒng)

0 引 言

非線性濾波問題廣泛存在于導(dǎo)航制導(dǎo)、自動控制以及信息通信等領(lǐng)域。當(dāng)前，處理非線性問題常用的濾波方法包括擴展卡爾曼濾波（extended Kalman filtering，EKF）［1］，無跡卡爾曼濾波（unscented Kalman filtering，UKF），無跡粒子濾波（unscented particle filtering，UPF）［2］等。EKF將非線性模型進行Taylor級數(shù)展開，忽略高階項以解決非線性系統(tǒng)的濾波計算問題。UKF算法基于無跡變換（unscented transformation，UT），采用確定性采樣以近似非線性函數(shù)的概率分布，相對于EKF算法無需計算Jacobian矩陣。UPF算法采用UKF以獲取重要性密度函數(shù)，利用狀態(tài)空間中的加權(quán)隨機樣本集來近似實際后驗概率分布。上述濾波方法在處理擁有確定系統(tǒng)模型和先驗噪聲協(xié)方差的問題時，能夠獲得較好的濾波精度。然而在很多工程問題中，精確的系統(tǒng)模型往往難以獲取，傳統(tǒng)的濾波器在面對此類情況時濾波精度明顯下降。針對這一問題，本文將高斯過程回歸（Gaussian process regression，GPR）與UPF算法相結(jié)合來解決由于系統(tǒng)誤差導(dǎo)致的濾波精度下降問題，以獲取更好的濾波效果。

高斯過程回歸基于貝葉斯理論和統(tǒng)計學(xué)習(xí)理論，是一種全新的機器學(xué)習(xí)方法，適用于處理高維數(shù)、小樣本和非線性問題，其泛化能力強且與神經(jīng)網(wǎng)絡(luò)和支持向量機相比具有更易實現(xiàn)、超參數(shù)自適應(yīng)獲取以及輸出具有概率意義等優(yōu)點［3］。近年來高斯過程回歸被嘗試應(yīng)用于各個領(lǐng)域，并取得了一定的研究成果。文獻［4］采用十進制遺傳算法代替共軛梯度法搜尋高斯過程最優(yōu)超參數(shù)，提高了高斯過程回歸的泛化能力。文獻［5］將高斯過程回歸引入計量學(xué)領(lǐng)域，用于挖掘近紅外光譜與被測物組分之間的復(fù)雜關(guān)系，從而獲取了精度高、釋放能力強的分析模型。文獻［6］將GPR應(yīng)用于無線電強度估計的定位問題中，取得了較好的定位效果。文獻［4- 6］嘗試了GPR在不同領(lǐng)域的應(yīng)用，驗證了其在工程領(lǐng)域的有效性。文獻［7］將GPR與容積卡爾曼濾波相結(jié)合，采用GPR學(xué)習(xí)建立模型并通過容積卡爾曼濾波處理非線性問題，探討了將GPR與非線性濾波算法相結(jié)合的新思路。然而，如何改善GPR與非線性濾波的融合效果并進一步提高濾波精度，是本文研究的重點。

本文提出一種適用于非線性系統(tǒng)的高斯過程回歸平方根分解無跡粒子濾波（Gaussian process regression squareroot decomposition unscented particle filter，GPSR-UPF）算法。該算法將高斯過程回歸融入UPF算法中，通過高斯過程回歸求取UPF的重要性密度函數(shù)，當(dāng)系統(tǒng)模型不精確時，GPR通過高斯過程實時學(xué)習(xí)訓(xùn)練數(shù)據(jù)，獲取系統(tǒng)的回歸模型及系統(tǒng)噪聲協(xié)方差，提高模型精度；同時，采用平方根變換，利用協(xié)方差平方根代替協(xié)方差參加遞推運算來保證系統(tǒng)狀態(tài)協(xié)方差陣的正定性。將提出的GPSR-UPF算法應(yīng)用到SINS／GPS組合導(dǎo)航系統(tǒng)中進行仿真驗證。結(jié)果表明，該算法適用于非線性動態(tài)系統(tǒng)，能夠降低由于系統(tǒng)模型變化所導(dǎo)致的濾波誤差，提高導(dǎo)航解算精度。

1 高斯過程回歸平方根UPF算法設(shè)計

考慮如下非線性系統(tǒng)：

式中，xk，zk∈Rn分別為k時刻系統(tǒng)的狀態(tài)向量和量測向量；uk為控制輸入向量；εk∈Rn是系統(tǒng)噪聲，其方差為Qk；δk∈Rn是量測噪聲，其方差為Rk；f（·）和h（·）為非線性函數(shù)。

1.1 高斯過程回歸

高斯過程回歸是通過尋找訓(xùn)練數(shù)據(jù)之間的關(guān)系來對系統(tǒng)進行辨識的非參數(shù)黑箱模型［8］。其性質(zhì)由均值函數(shù)m（x）和協(xié)方差函數(shù)k（x，x′）確定［3］，即

式中，函數(shù)f（x）的高斯過程（Gaussian process，GP）數(shù)學(xué)表達式為f（x）～GP（m（x），k（x，x′）），x，x′∈Rd是任意的隨機變量。

對于回歸問題，給出如下模型：

同時，有觀測值y和預(yù)測值f*的聯(lián)合先驗分布為

式中，K（X，X）＝Kn＝（kij）為n×n階對稱正定協(xié)方差矩陣。矩陣元素kij＝k（xi，xj）用來度量xi和xj的相關(guān)性，x*為測試點，K＝K（K，x*）＝K（x*，X）T為測試點與訓(xùn)練集的輸入X之間的協(xié)方差陣，k（x*，x*）為測試點x*自身的協(xié)方差，In是n維單位矩陣。

預(yù)測值f*的后驗分布為

式中，均值和方差［9］分別為

通常采用平方指數(shù)協(xié)方差來求解協(xié)方差函數(shù)中的未知參數(shù)，有

采用極大似然法求取超參數(shù)θ。建立似然函數(shù)

并對超參數(shù)θ求偏導(dǎo)，采用共軛梯度法對偏導(dǎo)數(shù)進行最小化以求取最優(yōu)解［3］得

式中，C＝Kn＋In，α＝C－1y。獲得最優(yōu)超參數(shù)后，聯(lián)合式（7）和式（8）即可求得測試點xi所對應(yīng)的期望和方差。

1.2 平方根變換

在對粒子進行預(yù)測和更新時，要求狀態(tài)協(xié)方差陣為非負(fù)定矩陣。然而在計算過程中，由于誤差等因素可能使得濾波協(xié)方差負(fù)定或者不對稱，從而導(dǎo)致濾波器發(fā)散，本文采用平方根變換來解決該問題?？紤]L維隨機變量x經(jīng)過非線性變換后得到z＝h（x），假設(shè)x的均值和方差分別為x－和Px，且有x的Sigma采樣點（xi）和權(quán)值ωi。將采樣點進行非線性變換得到

計算得到均值和協(xié)方差，有

新算法在濾波過程中采用Sz替代Pz進行運算以抑制協(xié)方差陣的負(fù)定性。

1.3 高斯學(xué)習(xí)過程的狀態(tài)和觀測模型

高斯過程回歸學(xué)習(xí)獲取系統(tǒng)的狀態(tài)方程、觀測方程以及噪聲協(xié)方差陣Q和R，其狀態(tài)方程和觀測方程的訓(xùn)練數(shù)據(jù)集為

式中，X為狀態(tài)陣；X′＝［Δx1，Δx2，…，Δxk］為狀態(tài)轉(zhuǎn)換矩陣，Δxk＝xk＋1－xk；Z是觀測陣。高斯過程將狀態(tài)方程f和觀測方程h表示為GPF和GPH，則

式中，εk～N（0，GP［（xk－1，uk－1），DF］）；δk～N（0，GP（xk，DH））。

取式（16）與系統(tǒng)近似模型獲取值之差為數(shù)據(jù)集，即通過高斯過程回歸學(xué)習(xí)真實系統(tǒng)模型與近似系統(tǒng)模型的差［9］，有

聯(lián)合系統(tǒng)近似模型與式（18）學(xué)習(xí)獲取的系統(tǒng)模型，其狀態(tài)方程和觀測方程為

式中，εk～N（0，GP［（xk－1，uk－1），Df］）；δk～N（0，GP（xk，Dh））；為近似的系統(tǒng)狀態(tài)方程；^h為近似的觀測方程。由數(shù)據(jù)集式（18）學(xué)習(xí)獲得的狀態(tài)方程和觀測方程分別表示為GP和GP。

1.4 提出的GPSR-UPF算法

本文所提出的GPSR-UPF算法將高斯過程回歸融入UPF算法中，采用高斯過程回歸方法提高系統(tǒng)模型精度以求取UPF的重要性密度函數(shù)；同時引入平方根分解，使用誤差協(xié)方差陣的平方根代替協(xié)方差陣參與濾波以抑制系統(tǒng)狀態(tài)協(xié)方差陣的負(fù)定性。具體算法如下：

（1）初始化并從先驗分布中抽取粒子

式中，La表示向量的維數(shù)；λ表示尺度因子；chol表示cholesky分解；α和β為控制采樣點分散程度的參數(shù)。分別建立狀態(tài)方程與觀測方程的訓(xùn)練數(shù)據(jù)：Df，Dh。

（

2）計算Sigma點

通過高斯過程進行實時學(xué)習(xí)，時間更新：

采用平方根變換，利用協(xié)方差平方根代替協(xié)方差參加遞推運算以抑制系統(tǒng)狀態(tài)協(xié)方差陣的負(fù)定性。

通過GPR學(xué)習(xí)獲取觀測值：

量測更新：

歸一化權(quán)值得

（3）重采樣

計算Neff，判斷Neff是否小于閾值NT。如小于閾值，則進行重采樣。Neff越小表示退化越嚴(yán)重。

（4）狀態(tài)量的估計值

2 SINS／GPS組合導(dǎo)航系統(tǒng)模型

捷聯(lián)慣導(dǎo)（strapdown inertial navigation system，SINS）［10］與全球定位系統(tǒng)（global positioning system，GPS）兩種導(dǎo)航方法具備互補特性，將SINS與GPS相融合進行組合導(dǎo)航，可以獲取較高的導(dǎo)航精度。

2.1 組合導(dǎo)航系統(tǒng)的狀態(tài)方程

SINS的姿態(tài)誤差方程為

速度誤差方程為

位置誤差方程為

陀螺漂移誤差模型表示為

式中，εb為隨機常數(shù)；εr為一階馬爾科夫隨機過程噪聲；ωg為陀螺隨機白噪聲漂移。

加速度計誤差模型為

式中，Tα為相關(guān)時間；ωα為加速度計隨機白噪聲漂移。

在東北天坐標(biāo)系中，選取組合系統(tǒng)的狀態(tài)量為

噪聲隨機誤差矢量W為

2.2 組合導(dǎo)航系統(tǒng)的量測方程

采集SINS和GPS所輸出的位置和速度信息，SINS的位置信息為

速度信息為

GPS輸出的位置信息為

GPS輸出的速度信息為

式中，vE，vN，vU是載體沿地理坐標(biāo)系3個軸方向的速度真值；Lt，λt，ht是位置真值；ME，MNMU是GPS的測速誤差；NE，NN，NU是GPS東，北，天三向的位置誤差。

取SINS與GPS的量測量之差為觀測信息，定義位置量測方程［11］為

即

其中，Vp（t）＝［NE，NN，NU］是系統(tǒng)位置信息量測噪聲。

定義速度量測方程為

即

式中，Vv（t）＝［ME，MN，MU］是系統(tǒng)速度信息量測噪聲。由此得到SINS／GPS位置和速度的聯(lián)合測量方程為

3 仿真分析

仿真實驗重點驗證了本文提出的GPSR-UPF算法和基本UPF算法在系統(tǒng)模型發(fā)生誤差時的性能對比。為了驗證本文所提出算法的優(yōu)越性，分別將基本UPF算法以及改進后的GPSR-UPF算法應(yīng)用于SINS／GPS組合導(dǎo)航中進行仿真對比。設(shè)飛行器初始位置為東經(jīng)109°，北緯34°，高度1 000 m。陀螺常值漂移為0.01°／h，陀螺一階馬爾可夫過程漂移0.01°／，加速度計常值偏置10－4g，隨機偏置為10－5g／，GPS測量噪聲標(biāo)準(zhǔn)差為10 m的白噪聲，SINS初始速度誤差為0.5 m／s，初始位置誤差為10 m，仿真時間1 000 s。飛行器飛行過程中進行各種機動動作，航跡包括滑跑起飛、爬升、勻速平飛、加速、盤旋等狀態(tài)。其中，0～200 s飛行器做滑跑、起飛、爬升等動作，500～600 s時，飛行器做加速、盤旋等動作。具體仿真結(jié)果如圖1～圖6所示。

圖1 基于UPF的位置誤差估計

圖2 基于GPSR-UPF的位置誤差估計

圖1和圖2分別是基本UPF算法和本文提出的GPSR-UPF算法在組合導(dǎo)航系統(tǒng)中得到的位置誤差曲線。可以看出，在勻速飛行時段，采用UPF算法得到的東向、北向以及天向的位置誤差穩(wěn)定在（－7.5 m，＋7.9 m）；而采用GPSR-UPF算法得到的東向、北向以及天向的位置誤差穩(wěn)定在（－4.9 m，＋4.9 m）。飛行器在0～200 s以及500～600 s兩個時間段做變速機動運動，由仿真圖可以看出，0～200 s時間段，GPSR-UPF算法相對于UPF算法，其位置誤差估計受系統(tǒng)變化的影響要小于UPF算法；而在500～600 s時間段，GPSR的位置誤差估計穩(wěn)定在±10 m以內(nèi)，而UPF算法的位置誤差明顯高于前者。

圖3 基于UPF的速度誤差估計

圖4 基于GPSR-UPF的速度誤差估計

圖3和圖4分別是UPF算法和本文提出的GPSRUPF算法在組合導(dǎo)航系統(tǒng)中得到的速度誤差曲線。在勻速飛行時段，通過UPF算法計算得到的速度誤差范圍穩(wěn)定在（－1.8 m／s，＋1.5 m／s）；而由GPSR-UPF算法計算得到的速度誤差范圍穩(wěn)定在（－1.0 m／s，＋1.2 m／s）。飛行器在0～200 s以及500～600 s兩個時間段做機動運動，0～200 s時間段，GPSR-UPF算法相對于UPF算法在速度誤差估計上差別不大；在500～600 s時間段，GPSR的速度誤差估計穩(wěn)定在（－1.5 m／s，＋2.0 m／s）以內(nèi)，而UPF算法的速度誤差最高達到了3.3 m／s。

圖5和圖6分別是UPF算法和本文提出的GPSR-UPF算法在組合導(dǎo)航系統(tǒng)中得到的姿態(tài)誤差曲線。在勻速飛行時段，通過UPF算法計算得到的姿態(tài)誤差范圍穩(wěn)定在（－1.4′，＋1.6′）內(nèi)；采用GPSR-UPF算法計算得到的姿態(tài)誤差范圍穩(wěn)定在（－0.9′，＋1.0′）以內(nèi)。飛行器在0～200 s以及500～600 s兩個時間段做機動運動，在0～200 s時間段GPSR-UPF算法的位置誤差明顯小于UPF算法；在500～600 s時間段，GPSR的速度誤差估計穩(wěn)定在（－1.9′，＋1.6′）以內(nèi)，而UPF算法的速度誤差接近±3′，具體結(jié)果如表1所示。

圖6 基于GPSR-UPF的姿態(tài)誤差估計

表1 GPSR-UPF與基本UPF在組合導(dǎo)航中的仿真結(jié)果對比

通過表1的結(jié)果對比可以看出，飛行器在做勻速飛行時，GPSR-UPF算法的解算精度要高于UPF算法。而當(dāng)飛行器做滑跑、爬升、加速、盤旋等機動動作時，隨著飛行器軌跡的變化系統(tǒng)模型產(chǎn)生誤差，此時GPSR-UPF能夠降低系統(tǒng)模型誤差對濾波性能的影響，保持較好的導(dǎo)航精度。新算法對于位置和速度的解算誤差相比基本UPF算法能夠降低約33%左右，其對于姿態(tài)的跟蹤誤差能夠降低約40%。由此說明，改進的GPSR-UPF算法相對于基本UPF算法能夠有效降低系統(tǒng)模型誤差對濾波結(jié)果的干擾，獲得更高的解算精度。

4 結(jié) 論

本文提出的GPSR-UPF算法，首先將高斯過程回歸融入無跡粒子濾波算法以求取UPF的重要性密度函數(shù)，降低了由于系統(tǒng)模型誤差所導(dǎo)致的濾波誤差。同時，在算法中引入平方根變換，利用協(xié)方差平方根代替協(xié)方差參與遞推運算來解決系統(tǒng)狀態(tài)協(xié)方差陣可能出現(xiàn)的負(fù)定性問題。最后，將該算法應(yīng)用于SINS／GPS組合導(dǎo)航系統(tǒng)中進行仿真驗證。結(jié)果表明，通過對目標(biāo)系統(tǒng)模型的辨識學(xué)習(xí)，設(shè)計的GPSR-UPF算法能夠降低系統(tǒng)誤差對濾波精度的影響，在系統(tǒng)模型變化時保持濾波效率，其性能優(yōu)于基本UPF算法，提高了組合導(dǎo)航系統(tǒng)的解算精度。

［1］Giannitrapani A，Ceccarelli N，Scortecci F，et al.Comparison of EKF and UKF for spacecraft localization via angle measurements［J］.IEEE Trans.on Aerospace and Electronic Systems，2011，47（1）：75- 84.

［2］Gerasimos G R.Nonlinear Kalman filters and particle filters for integrated navigation of unmanned aerial vehicles［J］.Robotics and Autonomous Systems，2012，60（7）：978- 995.

［3］He Z K，Liu G B，Zhao X J，et al.Overview of Gaussian process regression［J］.Control and Decision，2013，28（8）：1121- 1129.（何志昆，劉光斌，趙曦晶，等.高斯過程回歸方法綜述［J］.控制與決策，2013，28（8）：1121- 1129.）

［4］Liu K Y，F(xiàn)ang Y，Liu B G，et al.Intelligent deformation prediction model of tunnel surrounding rock based on genetic-Gaussian process regression coupling algorithm［J］.Journal of the China Railway Society，2011，33（12）：101- 106.（劉開云，方昱，劉保國，等.隧道圍巖變形預(yù)測的進化高斯過程回歸模型［J］.鐵道學(xué)報，2011，33（12）：101- 106.）

［5］Feng A M，F(xiàn)ang L M，Lin M.Gaussian process regression and its application in near-infrared spectroscopy analysis［J］.Spectroscopy and Spectral Analysis，2011，31（6）：1514- 1517.（馮愛明，方利民，林敏.近紅外光譜分析中的高斯過程回歸方法［J］.光譜學(xué)與光譜分析，2011，31（6）：1514- 1517.）

［6］Ferris B，Haehnel D，F(xiàn)ox D.Gaussian processes for signal strength-based location estimation［C］∥Proc.of the International Conference on Robotics，Science and Systems，2006：303- 310.

［7］Wei X Q，Song S M.Model-free cubature Kalman filter and its application［J］.Control and Decision，2013，28（5）：769- 773.（魏喜慶，宋申民.無模型容積卡爾曼濾波及其應(yīng)用［J］.控制與決策，2013，28（5）：769- 773.）

［8］Taeryon C.Alternative posterior consistency results in nonparametric binaryregression using Gaussian process priors［J］.Journal of Statistical Planning and Inference，2007，137（9）：2975-2983.

［9］Jonathan K，Daniel J K，Dieter F，et al.GP-UKF：unseented Kalman filters with Gaussian process predietion and observation models［C］∥Proc.of the International Conference on Intelligent Robots and Systems，2007：1901- 1907.

［10］Gao S S，Wei W H，Zhong Y M，et al.Rapid alignment method based on local observability analysis for strapdown inertial navigation system［J］.Acta Astronautica，2014，94（2）：790- 798.

［11］Zhang K，Liu H P，Li H N，et al.SINS／GPS／CNS integrated navigation federal filtering algorithm［J］.Journal of Chinese Inertial Technology，2013，21（2）：226- 230.（張科，劉海鵬，李恒年，等.SINS／GPS／CNS組合導(dǎo)航聯(lián)邦濾波算法［J］.中國慣性技術(shù)學(xué)報，2013，21（2）：226- 230.）

Square-root unscented particle filter based on Gaussian process regression

MENG Yang，GAO She-sheng，WANG Wei
（School of Automation，Northwestern Polytechnical University，Xi’an 710072，China）

In view of the uncertainty of the system dynamic model may reduce the filtering effect and the system state covariance matrix is negative definiteness，a new unscented particle filter（UPF）based on Gaussian process regression and square-root decomposition（GPSR）is proposed.The importance density function of UPF is gotten by Gaussian process regression.When the system model and observation model are inaccurate，Gaussian process regression is used to learn the training data，the regression models and noise covariance of the dynamic system are gotten；square-root decomposition is used to restrain the negative definiteness of the system state covariance matrix.The proposed algorithm is applied to the integrated navigation system of strapdown inertial navigation system／global positioning system（SINS／GPS）.The simulation results show that the proposed algorithm is better than UPF，and also effectively improves the positioning precision of the navigation system.

Gaussian process regression；square-root decomposition；unscented particle filter（UPF）；integrated navigation

V 249.32

A

10.3969／j.issn.1001-506X.2015.12.23

孟 陽（198-5- ），男，博士研究生，主要研究方向為導(dǎo)航、制導(dǎo)與控制。

E-mail：27104683＠qq.com

高社生（1956- ），男，教授，博士研究生導(dǎo)師，博士，主要研究方向為導(dǎo)航、制導(dǎo)與控制。

E-mail：gshshnpu＠163.com

王 維（1984-- ），男，博士研究生，主要研究方向為導(dǎo)航、制導(dǎo)與控制。

E-mail：413773558＠qq.com

1001-506X（2015）12-2817-06

2015- 01- 18；

2015- 04- 12；網(wǎng)絡(luò)優(yōu)先出版日期：2015- 08- 31。

網(wǎng)絡(luò)優(yōu)先出版地址：http：∥www.cnki.net／kcms／detail／11.2422.TN.20150831.1931.012.html

國家自然科學(xué)基金（61174193）；航天科技創(chuàng)新基金（2014-HTXGD）資助課題