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

段晨浩 王玨 鄧志鑫

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

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

中圖分類(lèi)號(hào)：TN967.1文獻(xiàn)標(biāo)識(shí)碼： 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]定位授時(shí)精度難以達(dá)到要求以及信號(hào)功率相對(duì)較弱容易受到復(fù)雜環(huán)境影響干擾欺騙的問(wèn)題，作為一種區(qū)域增強(qiáng)系統(tǒng)，偽衛(wèi)星能有效彌補(bǔ)衛(wèi)星信號(hào)受遮擋的問(wèn)題，并提供較強(qiáng)的導(dǎo)航信號(hào)。近幾年，空基偽衛(wèi)星成為研究熱點(diǎn)。相比于陸基偽衛(wèi)星，空基偽衛(wèi)星雖有著更加靈活、應(yīng)用場(chǎng)合更為多樣、覆蓋區(qū)域更廣的優(yōu)勢(shì)，但也面臨著諸多問(wèn)題[2-4]。

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

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

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

1.1 Kalman濾波算法

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

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

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

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

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

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

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

計(jì)算新息：

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

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

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

計(jì)算Kalman濾波增益：

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

計(jì)算一步預(yù)測(cè)均方誤差：

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

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

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

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

1.2 EKF以及UKF濾波算法

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

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

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

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

2 平方根UKF濾波算法

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

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

2.1 Pk的平方根遞推方程

根據(jù)定義，Pk-1具有對(duì)稱非負(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）可寫(xiě)成

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 平方根濾波方程

平方根濾波方差可寫(xiě)為

[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ù)以上平方根濾波方程進(jìn)行濾波，每一步得到的Pk以及Pk/k-1至少是非負(fù)定的。

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

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)過(guò)程噪聲以及觀測(cè)噪聲向量;SW=RW，RW為過(guò)程噪聲協(xié)方差矩陣;SV=RV，RV為觀測(cè)噪聲協(xié)方差矩陣。

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

計(jì)算Sigma點(diǎn)向量：

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

計(jì)算權(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ù);α為決定先驗(yàn)均值周?chē)鶶igma點(diǎn)分布廣度的主要刻度因數(shù);β為強(qiáng)調(diào)驗(yàn)后協(xié)方差計(jì)算的零階Sigma點(diǎn)權(quán)值的第二刻度因數(shù);κ為第三刻度因數(shù)。

計(jì)算時(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）。

計(jì)算觀測(cè)更新方程：

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ù)運(yùn)算符“”表示利用下三角Cholesky分解得到的矩陣方根;qr（A）表示對(duì)矩陣A進(jìn)行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的速度行進(jìn)，時(shí)間總長(zhǎng)為80 s，圖3、圖4分別為線性狀態(tài)及非線性狀態(tài)2種場(chǎng)景下Kalman濾波的跟蹤軌跡及誤差情況。

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

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

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

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

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

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

4 結(jié) 語(yǔ)

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

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

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

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

參考文獻(xiàn)/References：

[1]李占營(yíng). 基于偽衛(wèi)星技術(shù)的室內(nèi)定位系統(tǒng)硬件設(shè)計(jì)及實(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)星空基增強(qiáng)網(wǎng)絡(luò)優(yōu)化與高精度動(dòng)態(tài)時(shí)間同步[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接收機(jī)中卡爾曼濾波的研究[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].長(zhǎng)沙：國(guó)防科學(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].艦船電子對(duì)抗，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]楊元喜.動(dòng)態(tài)Kalman濾波模型誤差的影響[J].測(cè)繪科學(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]馮亞麗.非線性卡爾曼濾波算法的改進(jìn)及精度分析[D].重慶：西南大學(xué)，2017.

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

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

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

[12]王海春，賈小林，李鼎. 北斗三號(hào)衛(wèi)星廣播星歷精度評(píng)估分析[J].導(dǎo)航定位學(xué)報(bào)，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)，王俊. 一種改進(jìn)的多伯努利多目標(biāo)跟蹤算法[J].西安電子科技大學(xué)學(xué)報(bào)，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]張棟，焦嵩鳴，劉延泉. 互補(bǔ)濾波和卡爾曼濾波的融合姿態(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原理與接收機(jī)設(shè)計(jì)[M].北京：電子工業(yè)出版社，2012.

[21]王見(jiàn)，馬建林.EKF與互補(bǔ)融合濾波在姿態(tài)解算中的研究[J].傳感技術(shù)學(xué)報(bào)，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ìn)的蟻群算法的移動(dòng)機(jī)器人路徑規(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.