基于平方根UKF的偽衛(wèi)星動態(tài)跟蹤定位算法

段晨浩 王玨 鄧志鑫

摘 要： 為了解決傳統(tǒng)Kalman濾波在處理非線性系統(tǒng)時的局限性，以及擴展Kalman濾波（EKF）在處理強非線性系統(tǒng)時發(fā)散性和精度較差的問題，結(jié)合動態(tài)導(dǎo)航系統(tǒng)中的目標(biāo)跟蹤定位問題，在不敏Kalman濾波（UKF）算法的基礎(chǔ)上，提出了一種基于平方根UKF的動態(tài)跟蹤定位算法，在遞推運算過程中采用協(xié)方差矩陣的平方根代替?zhèn)鹘y(tǒng)算法計算過程中的協(xié)方差矩陣。MATLAB仿真結(jié)果表明，平方根UKF算法的精度比EKF提升了54.7%，比UKF提升了14.8%。所提出的算法解決了Kalman處理非線性系統(tǒng)的局限性以及傳統(tǒng)EKF和UKF算法精度不高的問題，為偽衛(wèi)星系統(tǒng)的高精度定位研究提供了有力支撐。

關(guān)鍵詞： 算法理論;動態(tài)定位跟蹤;偽衛(wèi)星;平方根濾波;卡爾曼濾波算法;非線性

中圖分類號：TN967.1文獻標(biāo)識碼： A

doi：10.7535/hbkd.2020yx06003

Pseudolite dynamic tracking and positioning

algorithm based on square root UKF

DUAN Chenhao1， WANG Jue1， DENG Zhixin1，2

（1.The 54th Research Institute of CETC，Shijiazhuang，Hebei 050081，China;2.State Key Laboratory of Satellite Navigation System and Equipment Technology，Shijiazhuang， Hebei 050081，China）

In order to solve the limitation of traditional Kalman filter in dealing with nonlinear system and the problem of divergence and poor accuracy of extended Kalman filter （EKF） in dealing with strong nonlinear system， a dynamic tracking and positioning algorithm based on square root UKF was proposed based on the unscented Kalman filter （UKF） algorithm in combination with the problem of target tracking and positioning in dynamic navigation system. In the process of recursive operation， the square root of covariance matrix was used to replace the covariance matrix in the calculation process of covariance algorithm. The MATLAB simulation results show that the accuracy of the square root UKF algorithm is 54.7% higher than that of the EKF algorithm， and 14.8% higher than that of the UKF algorithm. The proposed algorithm solves the limitation of Kalman processing nonlinear system and the problem of the low accuracy of traditional EKF and UKF algorithms， and provides a strong support for the high-precision positioning of pseudolite system.

algorithm theory; dynamic positioning and tracking; pseudolite; square root filtering; Kalman filter algorithm; nonlinearity

為了解決傳統(tǒng)衛(wèi)星星座在某些特定區(qū)域（如峽谷、隧道、密林等）受地形遮擋[1]定位授時精度難以達到要求以及信號功率相對較弱容易受到復(fù)雜環(huán)境影響干擾欺騙的問題，作為一種區(qū)域增強系統(tǒng)，偽衛(wèi)星能有效彌補衛(wèi)星信號受遮擋的問題，并提供較強的導(dǎo)航信號。近幾年，空基偽衛(wèi)星成為研究熱點。相比于陸基偽衛(wèi)星，空基偽衛(wèi)星雖有著更加靈活、應(yīng)用場合更為多樣、覆蓋區(qū)域更廣的優(yōu)勢，但也面臨著諸多問題[2-4]。

空基偽衛(wèi)星這一動態(tài)平臺的自身位置對整個系統(tǒng)的定位精度有著重要影響。針對動態(tài)系統(tǒng)的定位跟蹤，常采用Kalman濾波算法及擴展Kalman濾波算法。一般而言，Kalman濾波算法或者在其基礎(chǔ)上演變的相關(guān)算法，在處理線性或者弱非線性系統(tǒng)方面效果較為理想，但對于較為復(fù)雜的非線性系統(tǒng)，其處理結(jié)果對于較高精度的動態(tài)跟蹤定位來說很難達到要求[5-7]。針對上述問題，本文提出一種基于平方根UKF的動態(tài)目標(biāo)跟蹤定位算法，并與常見的EKF以及UKF算法進行仿真分析對比。

1 傳統(tǒng)跟蹤定位算法

空基偽衛(wèi)星動態(tài)平臺在運動過程中，偽衛(wèi)星飛行狀態(tài)之間、狀態(tài)與觀測量之間存在嚴(yán)重的非線性關(guān)系，因此需要采用非線性濾波技術(shù)得到其狀態(tài)變量的最優(yōu)估計值，將濾波算法運用到動目標(biāo)跟蹤定位中，通過引入濾波方法對動目標(biāo)的行進路線進行預(yù)測與估計。常見的用于目標(biāo)跟蹤定位算法的濾波方法有Kalman濾波算法、EKF算法和UKF算法。

1.1 Kalman濾波算法

Kalman濾波是根據(jù)一定的濾波準(zhǔn)則，采用最優(yōu)化估計方法對觀測系統(tǒng)的狀態(tài)進行估計與預(yù)測的一種最優(yōu)估計或最優(yōu)濾波。其思路是在估計過程中，用上一次的最優(yōu)狀態(tài)估計和最優(yōu)誤差估計計算這一次的先驗狀態(tài)估計和先驗誤差估計，再用得到的先驗誤差估計以及量測噪聲得到Kalman增益，結(jié)合前面所得到的先驗估計和Kalman增益得到本次的最優(yōu)估計，不斷重復(fù)這個過程，用本次的最優(yōu)估計來估算下一次的先驗估計[8-9]。

假設(shè)動態(tài)系統(tǒng)的狀態(tài)空間模型為

X（t+1）=ΦX（t）+ΓW（t），

Y（t）=HX（t）+V（t），

式中：X（t）為系統(tǒng)在時刻t的狀態(tài);Y（t）為對狀態(tài)的觀測值;W（t）為系統(tǒng)噪聲，其方差矩陣為Q;V（t）為觀測噪聲，其方差陣為R;Φ為狀態(tài)轉(zhuǎn)移矩陣;H為觀測矩陣;Γ為系統(tǒng)噪聲驅(qū)動矩陣。Kalman濾波的計算流程如下。

計算狀態(tài)一步預(yù)測：

[AKX^]（t+1|t）=Φ[AKX^]（t|t）;

計算新息：

ε（t+1）=Y（t+1）-H[AKX^]（t+1|t）;

計算狀態(tài)估計值：

[AKX^]（k+1|k+1）=[AKX^]（k+1|k）+K（k+1）ε（k+1）;

計算Kalman濾波增益：

K（t+1）=P（t+1|t）HT[HP（t+1|t）HT+R]-1; （1）

計算一步預(yù)測均方誤差：

P（t+1|t）=ΦP（t|t）ΦT+ΓQΓT; （2）

計算一步預(yù)測估計均方誤差：

P（t+1|t+1）=[In-K（t+1）H]P（t+1|t）。 （3）

為了更形象地說明Kalman濾波原理，圖1給出了Kalman濾波的系統(tǒng)模型框圖。

1.2 EKF以及UKF濾波算法

Kalman濾波在處理線性高斯模型以及弱非線性系統(tǒng)時有著比較理想的效果，誤差顯著減少，但是在處理較強非線性系統(tǒng)時其濾波效果大部分情況下很難滿足要求。而在實際應(yīng)用場景即空基偽衛(wèi)星在飛行過程中存在嚴(yán)重的非線性問題，此時常采用EKF以及UKF濾波方法對動目標(biāo)的行進路線進行預(yù)測與估計[7]。

EKF的基本思路是圍繞濾波值將非線性函數(shù)f（*）和h（*）展成Taylor級數(shù)，將一般的非線性系統(tǒng)處理為一個線性化系統(tǒng)的模型，再使用上述提到的傳統(tǒng)Kalman濾波進行濾波處理[10]。但是Kalman濾波存在數(shù)值穩(wěn)定性以及模型偏差等問題，同時又要求系統(tǒng)模型和系統(tǒng)噪聲的統(tǒng)計特性精確已知，因此當(dāng)系統(tǒng)具有較強的非線性或者初始誤差較大時，EKF的濾波精度就會明顯下降，甚至?xí)霈F(xiàn)發(fā)散[11]。

不敏Kalman濾波器（UKF）是針對非線性系統(tǒng)的一種改進型Kalman濾波器，采用Kalman線性濾波框架，對于一步預(yù)測方程使用不敏（UT）變換解決協(xié)方差以及均值的非線性處理問題。UKF實質(zhì)上不是對非線性函數(shù)進行近似，而是對非線性函數(shù)的概率密度分布進行近似，同時也不需要對Jacobian矩陣求導(dǎo)，沒有忽略高階項，這就使得UKF克服了EKF精度低、穩(wěn)定性差的缺點[11]。

UT變換與線性化方法的比較如圖2所示。

2 平方根UKF濾波算法

在傳統(tǒng)UKF濾波算法中，需要對每個采樣點進行非線性變換，計算量比較大，而且數(shù)值計算的誤差也比較明顯，估計誤差協(xié)方差矩陣的非負(fù)定性和對稱性會因此受到影響，從而影響濾波算法的收斂速度以及穩(wěn)定性[12]。為了提高濾波算法的濾波效率以及濾波精度，在遞推運算過程中采用協(xié)方差矩陣的平方根來代替?zhèn)鹘y(tǒng)算法計算過程中的協(xié)方差矩陣，這種方法稱為平方根不敏Kalman濾波算法[12]。

由式（1）—式（3）及初始值[AKX^]0=E[X0]，P0=E[（X0-[AKX^]0）（X0-[AKX^]0）T]，根據(jù)定義，在這里Pk及其預(yù)測Pk，k-1至少是非負(fù)定的，但是在舍去誤差的情況下，很難保證這一點。因此，在遞推過程中將Pk的遞推式改為Pk平方根Sk的遞推式，從而建立平方根濾波方程。

2.1 Pk的平方根遞推方程

根據(jù)定義，Pk-1具有對稱非負(fù)定性，設(shè)

Pk-1=Sk-1STk-1，

于是

Pk/k-1=Φk/k-1Pk-1ΦTk/k-1=Φk/k-1Sk-1STk-1ΦTk/k-1=Sk/k-1STk/k-1，

將式中的Sk/k-1=Φk/k-1Sk-1代入到式（3）中，

Pk=[I-KkHk]Pk/k-1=Sk/k-1{I-STk/k-1HTk[HkSk/k-1STk/k-1HTk+Rk]-1HkSk/k-1}STk/k-1， （4）

記Fk=STk/k-1HTk，式（4）可寫成

Pk=Sk/k-1{I-Fk[FTkFk+Rk]-1FTk}STk/k-1=Sk/k-1[I-akFkFTk]STk/k-1，

式中ak=[FTkFk+Rk]-1。

2.2 平方根濾波方程

平方根濾波方差可寫為

[AKX^]k=Φk/k-1[AKX^]k/k-1+Kk[Zk-HkΦk/k-1[AKX^]k-1]，

[AKX^]0=E[X0]，

Kk=akSk/k-1STk/k-1HTk=akSk/k-1Fk，

Fk=STk/k-1HTk，

Sk/k-1=Φk/k-1Sk-1， P0=S0ST0，

Sk=Sk/k-1[I-akrkFkFTk]， rk=11±akRk。

根據(jù)以上平方根濾波方程進行濾波，每一步得到的Pk以及Pk/k-1至少是非負(fù)定的。

根據(jù)平方根濾波的原理，可以得到平方根UKF的濾波過程如下。

1）初始化

[AKX^]0=E[X0]，SX0=sqrt（E[（X0-[AKX^]0）（X0-[AKX^]0）T]）。

系統(tǒng)的初始增廣狀態(tài)變量為

[AKX^]a0=E[Xa]=[[AKX^]T0 [AKW-]T [AKV-]T]T，

系統(tǒng)

Sa0=sqrt（E[（Xa0-[AKX^]a0）（Xa0-[AKX^]a0）T]）=SX0

SW

SV。 （5）

式中：W和V分別為系統(tǒng)過程噪聲以及觀測噪聲向量;SW=RW，RW為過程噪聲協(xié)方差矩陣;SV=RV，RV為觀測噪聲協(xié)方差矩陣。

2）對于k=1，2，3的實現(xiàn)步驟

計算Sigma點向量：

ξak-1=[[AKX^]ak-1 [AKX^]ak-1+γSaXk-1 [AKX^]ak-1-γSaXk-1]。

計算權(quán)值和參數(shù)：

γ=n+λ，

ω（m）0=λλ+n，

ω（c）0=ω（m）0+（1-α2+β），

ω（c）i=ω（m）i=12（n+λ）， i=1，2，…，2n，

式中：λ=α2（n+κ）-n為復(fù)合刻度參數(shù);n為增廣狀態(tài)向量的維數(shù);α為決定先驗均值周圍Sigma點分布廣度的主要刻度因數(shù);β為強調(diào)驗后協(xié)方差計算的零階Sigma點權(quán)值的第二刻度因數(shù);κ為第三刻度因數(shù)。

計算時間更新方程：

ξXk，k-1=f（ξak-1，ξWk-1， uk-1），

[AKX^][AKk-]=∑2ni=0ω（m）iξXi，（k，k-1），

S[AKX-]k=qr{[ω（c）1（ξX（1∶2n），（k，k-1）-[AKX^][AKk-]）]}，

S[AKX-]k=cholupdate{S[AKX-]k，ξX0，（k/k-1）-[AKX^][AKk-]，ω（c）c}，

χk，k-1=h（ξXi，（k/k-1）， ξVk-1），

[AKZ^][AKk-]=∑2ni=0ω（m）iχi，（k/k-1）。

計算觀測更新方程：

Sk=qr{[ω（c）1（χ（1∶2n），（k/k-1）-[AKZ^][AKk-]）]}，

Sk=cholupdate{Sk， χ0，（k/k-1）-[AKZ^][AKk-]，ω（c）c}，

PXkZk=∑2ni=0ω（c）i（ξXi，（k，k-1）-[AKX^][AKk-]）

（χi，（k，k-1）-[AKZ^][AKk-]）T，

Kk=（PXkZk/STk）/Sk，

[AKX^]k=[AKX^][AKk-]+Kk（Zk-[AKZ^][AKk-]），

U=KkSk，

SXk=cholupadate{S[AKX-]k，U，-1}。

式中：線性代數(shù)運算符“”表示利用下三角Cholesky分解得到的矩陣方根;qr（A）表示對矩陣A進行qr分解得到的下三角部分R矩陣;cholupdate{R，U，±V}表示矩陣（R±VUU）的Cholesky分解。

3 仿真分析

3.1 Kalman性能仿真分析

仿真條件設(shè)置如下：從初始位置X0（0，0）以X方向2 m/s，Y方向20 m/s的速度行進，時間總長為80 s，圖3、圖4分別為線性狀態(tài)及非線性狀態(tài)2種場景下Kalman濾波的跟蹤軌跡及誤差情況。

由圖3可以看出，Kalman濾波在處理線性及弱非線性時的效果較為理想，誤差顯著減小。由圖4可以看出，當(dāng)處理較強線性處理效果不太理想時，Kalman濾波幾乎沒有什么效果，噪聲也太大，隨機擾動性增大導(dǎo)致”無規(guī)律可循”。

3.2 EKF，UKF及平方根UKF濾波性能對比分析

圖5是3種算法的非線性定位跟蹤效果，圖6是3種算法的非線性定位跟蹤誤差，表1示出了估計誤差均方差及運行時間，圖7是95%置信區(qū)間下平方根UKF的跟蹤定位效果。

由圖5及圖6可以較為明顯地看出，在較強的非線性條件下，EKF的濾波精度較差且會出現(xiàn)明顯的發(fā)散情況;圖7在

95%的置信區(qū)間平方根UKF的大部分真實值都可以遍歷到。由表1 可以看出，EKF估計誤差均方差為9.640 3 m，雖然小于10 m但還是有明顯差距。UKF的估計誤差均方差為5.090 0 m，平方根UKF估計誤差均方差為4.366 2 m，相比于EKF精度提升了54.7%，相比于UKF精度提升了14.8%。

在濾波過程中，不計算協(xié)方差矩陣及其預(yù)測，而是計算其平方根，雖然提升了精度以及穩(wěn)定性，但會引起計算量的提升，EKF算法的運行時間為0.657 s;UKF算法的運行時間為0.723 s，平方根UKF算法的運行時間為 1.044 s。

4 結(jié) 語

1） 在濾波過程中采用協(xié)方差矩陣的平方根代替協(xié)方差矩陣參與遞推過程，保證了遞推過程中的穩(wěn)定性。

2） 與EKF以及UKF相比，利用本文提出的以平方根Kalman濾波算法的動態(tài)定位跟蹤算法，解決了發(fā)散問題，精度也有了較明顯的提升。

3）本研究解決了Kalman處理非線性系統(tǒng)的局限性以及傳統(tǒng)EKF和UKF算法精度不足的問題，為偽衛(wèi)星系統(tǒng)的高精度定位提供了有力的支撐。

4） 由于遞推過程中計算量的增加，使得算法運行時間有所增加，后續(xù)將在算法穩(wěn)定性及運算速度方面進行深入探討。

參考文獻/References：

[1]李占營. 基于偽衛(wèi)星技術(shù)的室內(nèi)定位系統(tǒng)硬件設(shè)計及實現(xiàn)[D].成都：電子科技大學(xué)，2018.

LI Zhanying. Hardware Design and Implementation of the Indoor Positioning System Based on Pseudo Satellite Technology[D].Chengdu： University of Electronic Science and Technology of China，2018.

[2] 李坤.北斗偽衛(wèi)星系統(tǒng)高精度定位關(guān)鍵技術(shù)研究[D].成都：電子科技大學(xué)，2018.

LI Kun. Key Techniques for High Accuracy Positioning of Beidou Pseudolite System[D].Chengdu： University of Electronic Science and Technology of China，2018.

[3]謝瑞煜，孫瑾.偽衛(wèi)星輔助復(fù)雜地形衛(wèi)星標(biāo)定方法研究[J].現(xiàn)代防御技術(shù)，2019，14（5）：22-28.

XIE Ruiyu，SUN Jin. Calibration method of pseudo-satellite assisted satellite incomplex terrain[J]. Modern Defence Technology，2019，14（5）：22-28.

[4]史海青.北斗偽衛(wèi)星空基增強網(wǎng)絡(luò)優(yōu)化與高精度動態(tài)時間同步[D].南京：南京航空航天大學(xué)，2014.

SHI Haiqing.Research on Enhancement Network Optimization and Precise Timing Synchronization for Air-borne Pseudo-satellites of Beidou[D]. Nanjing：Nanjing University of Aeronautics and Astronautics，2014.

[5]鄒俊成.GNSS接收機中卡爾曼濾波的研究[D].廣州：廣東工業(yè)大學(xué)，2015.

ZOU Juncheng. Research of Kalman Filtering in GNSS Receiver[D].Guangzhou：Guangdong University of Technology，2015.

[6]周成松.GNSS 導(dǎo)航中卡爾曼濾波優(yōu)化算法研究[D].長沙：國防科學(xué)技術(shù)大學(xué)，2016.

ZHOU Chengsong. Research on Kalman Filtering Optimized Algorithms in GNSS Navigation[D].Changsha： National University of Defense Technology，2015.

[7]毛秀華，吳健.卡爾曼濾波算法研究[J].艦船電子對抗，2017，40（3）：64-68.

MAO Xiuhua，WU Jian. Research into Kalman filter algorithm[J].Shipboard Electronic Countermeasure，2017，40（3）：64-68.

[8]黃小平，王巖.卡爾曼濾波原理及應(yīng)用[M].北京：電子工業(yè)出版社，2015.

[9]楊元喜.動態(tài)Kalman濾波模型誤差的影響[J].測繪科學(xué)，2006，31（1）： 17-18.

YANG Yuanxi. Influence of dynamic Kalman filter model error[J].Science of Surveying and Mapping，2006，31（1）： 17-18.

[10]馮亞麗.非線性卡爾曼濾波算法的改進及精度分析[D].重慶：西南大學(xué)，2017.

FENG Yali. Improvement and Accuracy Analysis of Nonlinear Kalman Filtering Algorithms[D].Chongqing：Southwest University，2017.

[11]葉松慶.非線性卡爾曼濾波算法研究[D].北京：中國科學(xué)院大學(xué)，2016.

YE Songqing. Research of Nonlinear Kalman Filtering Algorithm[D].Beijing： University of Chinese Academy of Sciences，2016.

[12]王海春，賈小林，李鼎. 北斗三號衛(wèi)星廣播星歷精度評估分析[J].導(dǎo)航定位學(xué)報，2019，7（4）：60-63.

WANG Haichun，JIA Xiaolin，LI Ding. Accuracy assessment and analysis of broadcast ephemeris of BDS-3 satellites[J]. Journal of Navigation and Positioning，2019，7（4）：60-63.

[13]KAZMIERSKI K，SO[KG-*4]S[DD（-*2]'NICA K， HADAS T. Quality assessment of multi-GNSS orbits and clocks for real-time precise point positioning[J].GPS Solutions，2018，22（11）：678-683.

[14]BRADLEY W. Near-optimal low-thrust earth-mars trajectory via a genetic algorithm[J].Journal of Guidance，Control and Dynamics，2015，28（5）：1027-1031.

[15]CHANDRA K P B，GU D W，POSTLETHWAITE I.Square root cuba-ture information filte[J].IEEE Sensors Journal，2013，13（2）：750-758.

[16]VO B T，VO B N，CANTONL A.The cardinality balanced multi-target multi-bernoulli filter and its implementation[J]. IEEE Transactions on Signal Processing，2009，57（2）：409-423.

[17]VO B N，VO B T.Labeled random finite sets and the multi-object conjugate priors[J]. IEEE Transactions on Signal Procession，2013，61（13）： 3460-3475.

[18]王海環(huán)，王俊. 一種改進的多伯努利多目標(biāo)跟蹤算法[J].西安電子科技大學(xué)學(xué)報，2016，43（6）： 176-182.

WANG Haihuan，WANG Jun.Multi-target tracking with the cubature Kalman multi-bernoulli filter[J].Journal of Xidian University，2016，43（6）：176-182.

[19]張棟，焦嵩鳴，劉延泉. 互補濾波和卡爾曼濾波的融合姿態(tài)解算方法[J]. 傳感器與微系統(tǒng)，2017，36（3）：62-65.

ZHANG Dong， JIAO Songming，LIU Yanquan. Fused attitude estimation algorithm based on complementary filtering and Kalman filtering[J]. Transducer and Microsystem Technologies，2017，36（3）： 62-65.

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

[21]王見，馬建林.EKF與互補融合濾波在姿態(tài)解算中的研究[J].傳感技術(shù)學(xué)報，2018，

31（8）： 1187-1191.

WANG Jian，MA Jianlin. Research on attitude algorithm of EKF and complementary filtering fusion[J]. Chinese Journal of Sensors and Actuators， 2018，31（8）： 1187-1191.

[22]張?zhí)K英，趙國花.基于改進的蟻群算法的移動機器人路徑規(guī)劃[J].河北工業(yè)科技，2019，36（6）：390-395.

ZHANG Suying，ZHAO Guohua. Path planning of mobile robot based on improved ant colony algorithm[J]. Hebei Journal of Industrial Science and Technology， 2019，36（6）：390-395.