最小二乘Unscented卡爾曼濾波在偽衛(wèi)星定位中的應(yīng)用

趙 飛， 高社生 ， 吳佳鵬， 楊 一

（1.西北工業(yè)大學(xué) 自動(dòng)化學(xué)院，陜西 西安 710072；2.航天恒星科技有限公司（503所）北京 100086）

衛(wèi)星導(dǎo)航系統(tǒng)的精度和可靠性嚴(yán)重依靠于可見星的數(shù)量和幾何分布狀況，在機(jī)場(chǎng)、峽谷、室內(nèi)和地下等觀測(cè)環(huán)境較差的區(qū)域，因可視星的數(shù)量受限而無法滿足用戶導(dǎo)航定位的需要。偽衛(wèi)星作為一種簡(jiǎn)便易行、效果顯著的導(dǎo)航定位手段，可以克服衛(wèi)星系統(tǒng)的缺陷[1-3]?；谂R近空間飛艇平臺(tái)的偽衛(wèi)星不僅能夠輔助衛(wèi)星導(dǎo)航系統(tǒng)進(jìn)行定位，而且可以在導(dǎo)航衛(wèi)星全部不可用時(shí)，由多顆（至少四顆）偽衛(wèi)星在目標(biāo)區(qū)域上空獨(dú)立組網(wǎng)，向用戶提供導(dǎo)航定位服務(wù)。

在獨(dú)立組網(wǎng)偽衛(wèi)星系統(tǒng)中，以飛艇為運(yùn)載平臺(tái)的偽衛(wèi)星由于受到臨近空間各種干擾的影響使得其自身位置存在誤差，從而影響到用戶最終的定位精度。因此，需要對(duì)偽衛(wèi)星飛艇平臺(tái)的位置誤差進(jìn)行修正[5-6]。在偽衛(wèi)星定位解算中，偽距方程是非線性的，傳統(tǒng)的方法是利用一階泰勒展開式將偽距方程在狀態(tài)預(yù)測(cè)值處進(jìn)行線性化，成為擴(kuò)展卡爾曼濾波（EKF）。同時(shí)，由于偽衛(wèi)星到用戶的距離要比導(dǎo)航衛(wèi)星到用戶的距離近得多，因而偽衛(wèi)星偽距觀測(cè)方程的線性化誤差要比導(dǎo)航衛(wèi)星偽距觀測(cè)方程的線性化誤差大得多[6]，從而影響到用戶的定位精度。因此，需要減小偽衛(wèi)星偽距觀測(cè)方程的線性化誤差。

基于以上分析，本文在研究偽衛(wèi)星星歷誤差傳遞規(guī)律和偽距觀測(cè)方程線性化誤差的基礎(chǔ)上，提出了一種新的最小二乘Unscented卡爾曼濾波算法。該算法首先利用最小二乘法估計(jì)出偽衛(wèi)星的位置誤差，以減小偽衛(wèi)星位置誤差對(duì)導(dǎo)航解算精度的影響；然后利用無跡卡爾曼濾波算法（Unscented-Kalman filter，UKF）對(duì)用戶進(jìn)行定位解算。仿真結(jié)果表明，與傳統(tǒng)的擴(kuò)展卡爾曼濾波算法相比，提出的算法能夠有效減小偽衛(wèi)星位置誤差對(duì)用戶定位精度的影響，提高獨(dú)立組網(wǎng)偽衛(wèi)星系統(tǒng)的定位精度。

1 最小二乘Unscented卡爾曼濾波算法

假設(shè)系統(tǒng)的狀態(tài)方程和觀測(cè)方程為：

式 （1）中，Xk為n維系統(tǒng)狀態(tài)向量，Zk為維系統(tǒng)量測(cè)向量，f和h為非線性向量函數(shù)，Wk和Vk為不相關(guān)的零均值白噪聲序列，方差陣分別為Qk和Rk。

1.1 UKF算法

與EKF不同，UKF不需要對(duì)狀態(tài)方程或觀測(cè)方程進(jìn)行線性化，它利用非線性模型來近似逼近狀態(tài)的分布，避免了線性化誤差。其主要思想是選擇能夠代表系統(tǒng)狀態(tài)均值和方差的采樣點(diǎn)，再將采樣點(diǎn)經(jīng)過非線性變換，變換后采樣點(diǎn)的分布以二階以上精度接近于真實(shí)均值和方差。

UKF算法是基于UT（unscented transformation）變換的最小方差估計(jì)。 假設(shè)隨機(jī)變量 X∈Rn～N（X，PXX），Y=f（X），則 Y的均值和方差可通過UT變換求取，步驟如下

1）計(jì)算2n+1個(gè)樣本點(diǎn)

根據(jù)隨機(jī)變量X的均值X和方差陣PXX，構(gòu)造Sigma點(diǎn)集

式（2）中，k=α2（n+λ）-n 為尺度參數(shù)，調(diào)整它可以提高逼近精度；α確定周圍Sigma點(diǎn)的分布程度，通常設(shè)為一個(gè)較小的正數(shù)（1>α≥1e-4）；λ 通常設(shè)置為 0或 3-n；用這組采樣點(diǎn) χi可以近似表示狀態(tài)X的高斯分布。

2）通過非線性函數(shù)計(jì)算變換點(diǎn)

3）計(jì)算Y的均值和方差

W（m）i和W（c）i分別為計(jì)算Y的均值和方差所用權(quán)值

β為狀態(tài)分布參數(shù)，對(duì)于高斯分布β=2是最優(yōu)的；如果狀態(tài)變量是單變量，則β=0是最佳的選擇。

1.2 最小二乘Unscented卡爾曼濾波算法

在偽衛(wèi)星存在位置誤差的情況下，其位置坐標(biāo)如下

式（9）中，Si=（xi，yi，zi）表示由星歷計(jì)算得到的第顆偽衛(wèi)星的位置；（x0i，y0i，z0i）表示第顆偽衛(wèi)星的真實(shí)位置；（εxi，εyi，εzi）分別為第偽衛(wèi)星在 3個(gè)坐標(biāo)方向上的位置誤差，εxi，εyi和 εzi相互獨(dú)立且分別服從高斯分布 N（0，σ2xi），N（0，σ2yi）和 N（0，σ2zi）。

最小二乘Unscented卡爾曼濾波的步驟如下：

① 預(yù)測(cè)：根據(jù)系統(tǒng)狀態(tài)的均值Xk和誤差協(xié)方差Pk，計(jì)算狀態(tài)預(yù)測(cè)值的均值和預(yù)測(cè)誤差的協(xié)方差

②偽衛(wèi)星位置向量估計(jì)：利用最小二乘估計(jì)得到偽衛(wèi)星位置的估計(jì)值S^i。

③把第二步得到的估計(jì)值S^i代入量測(cè)方程中，然后計(jì)算量測(cè)量預(yù)測(cè)值的均值和誤差協(xié)方差以及狀態(tài)量和觀測(cè)量的互協(xié)方差

④更新：計(jì)算濾波增益、濾波值和濾波誤差矩陣

上述步驟中，通過最小二乘法對(duì)偽衛(wèi)星的位置誤差進(jìn)行實(shí)時(shí)估計(jì)，修正偽衛(wèi)星位置；并且利用無跡卡爾曼濾波算法對(duì)用戶進(jìn)行定位解算，減小星歷誤差和線性化誤差對(duì)導(dǎo)解的影響，提高偽衛(wèi)星的定位精度。

2 偽衛(wèi)星定位系統(tǒng)模型

根據(jù)文獻(xiàn)[7]，建立接收機(jī)用戶靜態(tài)下的離散系統(tǒng)狀態(tài)方程：

式（22）中，Xk（x，y，z，δtu，δfu）T，（x，y，z）表示接收機(jī)位置，δtu為接收機(jī)鐘差，δf為接收機(jī)鐘漂Wk為系統(tǒng)激勵(lì)噪聲序列，取為零均值高斯白噪聲，Qk為系統(tǒng)噪聲序列的方差陣，假設(shè)為非負(fù)定陣。

離散系統(tǒng)每一顆可見星的量測(cè)方程為：

其中，Vk為量測(cè)噪聲，且為零均值高斯白噪聲；Rk為量測(cè)噪聲序列的方差陣，假設(shè)為正定陣；Wk與Vk不相關(guān)。

3 仿真實(shí)驗(yàn)及分析

獨(dú)立組網(wǎng)偽衛(wèi)星系統(tǒng)的幾何布局方案為：一顆偽衛(wèi)星位于基準(zhǔn)或定位中心位置的正上方，高度為30 km，另外3顆偽衛(wèi)星均勻的分布于服務(wù)區(qū)的周邊上空，高度角取15°，彼此間的方位角之差為120°，高度為20 km；第五顆偽衛(wèi)星位于中心偽衛(wèi)星的正下方，高度為20 km。假定接收機(jī)測(cè)量得到的各偽距誤差相互獨(dú)立且服從高斯分布；各偽衛(wèi)星的位置誤差相互獨(dú)立且服從高斯分布；接收機(jī)時(shí)鐘偏差Δt=200 ns；σρ=10 m；σs從1 m增加到100 m，仿真結(jié)果如圖1和圖2所示。

圖1 GDOP值較小時(shí)定位性能比較Fig.1 Positioning performance comparison of small GDOP value

圖2 GDOP值較大時(shí)定位性能比較Fig.2 Positioning performance comparison of big GDOP value

圖 1和圖 2分別對(duì)應(yīng)接收機(jī)位于 u=（400 m，800 m）和u=（2 km，2.5 km）的情況，實(shí)際上也反映了偽衛(wèi)星的幾何布局（）對(duì)定位精度的影響。圖1和圖2中的上圖對(duì)應(yīng)于偽衛(wèi)星數(shù)量為4的情況，下圖對(duì)應(yīng)于偽衛(wèi)星數(shù)量為5的情況。通過對(duì)圖1和圖2中的上下圖進(jìn)行分析比較可以看出，隨著偽衛(wèi)星數(shù)量的增多，兩種算法的定位精度都得到了提高。

圖1和圖2中方形表示采用最小二乘Unscented卡爾曼濾波算法計(jì)算得到的位置誤差曲線，菱形表示采用擴(kuò)展卡爾曼濾波算法計(jì)算得到的位置誤差曲線。通過對(duì)圖1和圖2中誤差曲線的分析和比較可以看出，在偽衛(wèi)星存在位置誤差的情況下，新算法的定位精度明顯優(yōu)于擴(kuò)展卡爾曼濾波的定位精度，而且隨著偽衛(wèi)星位置誤差的增大，該優(yōu)勢(shì)越明顯。

4 結(jié) 論

為了提高獨(dú)立組網(wǎng)偽衛(wèi)星系統(tǒng)的定位精度，在研究偽衛(wèi)星星歷誤差傳遞規(guī)律和偽距觀測(cè)方程線性化誤差的基礎(chǔ)上，提出了一種新的偽衛(wèi)星定位算法。將該算法應(yīng)用到獨(dú)立組網(wǎng)偽衛(wèi)星系統(tǒng)中進(jìn)行仿真驗(yàn)證，并與擴(kuò)展卡爾曼濾波進(jìn)行比較。結(jié)果表明，提出的最小二乘Unscented卡爾曼濾波算法，濾波性能優(yōu)于擴(kuò)展卡爾曼濾波[8]，能夠顯著提高偽衛(wèi)星獨(dú)立組網(wǎng)系統(tǒng)的定位精度。

[1]王 宇，孫國良，鄒世開.機(jī)載偽衛(wèi)星差分定位精度分析[J].全球定位系統(tǒng)，2007，32（2）：4-7.WANG Yu，SUN Guo-liang，ZOU Shi kai.The analysis on the precision of differential UAV onboard pseudolite[J].GNSS World of China，2007，32（2）：4-7.

[2]Kaplan E D.Understanding GPS:principle and application，2nd ed[M].Boston:Artech House，2006.

[3]Zhang M，Zhang J.A fast satellite selection algorithm:Beyond four satellite[J].IEEE Journal of Selection Topics in Signal Processing，2009，3（5）:740-747.

[4]Rapinski J.Pseudoulite Location Error.Environmental engineering[C]//The 8th International Conference，2011.

[5]鄧強(qiáng)，黃順吉.星歷誤差在偽距定位中的傳遞 [J].導(dǎo)航，1995（3）：42-46.DENG Qiang，HUANG Shun-ji.Ephemeris error propagation law in point positioning with pseudolite[J].Navigation，1995（3）：42-46.

[6]楊榮華，花向紅.GPS偽衛(wèi)星的組合定位技術(shù)及其誤差分析[J].地理空間信息，2008，6（4）:48-50.YANG Rong-hua，HUA Xiang-hong.Pseudolite-augmented GPS technology and its error analysis[J].Geospatial Information，2008，6（4）:48-50.

[7]謝鋼.GPS原理與接收機(jī)設(shè)計(jì)[M].北京：電子工業(yè)出版社，2007.

[8]王京偉，董大偉，華春蓉.擴(kuò)展卡爾曼濾波的影響因素分析[J].電子科技，2013（8）:10-12.WANG Jing-wei，DONG Da-wei，HUA Chun-rong.Analysis of extended kalman filter’s influencing factors[J].Electronic Science and Technology，2013（8）:10-12.