基于Hermite插值改進(jìn)的軌跡修正濾波算法

丁志偉 陳澤宇 馬夢園 張正軍

摘? ?要：借助移動(dòng)物體軌跡上標(biāo)定的真實(shí)值點(diǎn)的位置信息和方向信息，構(gòu)造分段三次Hermite插值函數(shù)，對(duì)標(biāo)準(zhǔn)卡爾曼濾波和標(biāo)準(zhǔn)粒子濾波的狀態(tài)方程進(jìn)行修正，得到兩種改進(jìn)濾波模型，解決了標(biāo)準(zhǔn)濾波算法中狀態(tài)方程不能準(zhǔn)確刻畫物體實(shí)際運(yùn)動(dòng)軌跡的問題。在實(shí)驗(yàn)分析中對(duì)定常速曲線運(yùn)動(dòng)物體的位置坐標(biāo)進(jìn)行濾波修正，位置坐標(biāo)的均方誤差分析表明，卡爾曼濾波算法和粒子濾波算法的濾波效果均有改善，其中卡爾曼濾波算法在精度上明顯提高。此外，真實(shí)值點(diǎn)數(shù)量的靈敏度分析的結(jié)果表明，在目標(biāo)軌跡上取少量且關(guān)鍵的點(diǎn)作為真實(shí)值點(diǎn)即可達(dá)到明顯的修正效果。

關(guān)鍵詞：卡爾曼濾波;粒子濾波;真實(shí)值;Hermite插值

中圖分類號(hào)： TN713;P228.4? ? ? ? ? ? ? ? ? ? ? ? ? ? ? 文獻(xiàn)標(biāo)識(shí)碼：A

Implementing Filtering Algorithm Refined

by Hermite Interpolation in Track Revision

DING Zhi-wei?，CHEN Ze-yu，MA Meng-yuan，ZHANG Zheng-jun

（School of Science，Nanjing University of Science and Technology，Nanjing，Jiangsu 210094，China）

Abstract：Through the position and direction information of the real value points on object track，a piecewise cubic Hermite interpolation function is constructed to modify the state equation of standard Kalman filter and standard particle filter. The state equation in standard filter algorithm can describe the actual trajectory of specific object more accurately. In the experimental analysis，the track of the object with constant velocity in curvilinear motion is revised. The mean square error of the position coordinates demonstrates that the improvement of Kalman filter algorithm and particle filter algorithm is practical. Moreover，Kalman filter algorithm is improved more significantly in position accuracy. Besides，the sensitivity analysis about the number of real points shows that a proper number of key points on the target track can be regarded as real points to achieve expected accuracy.

Key words：Kalman filter;particle filter;real value;Hermite interpolation

隨著衛(wèi)星定位技術(shù)和計(jì)算機(jī)技術(shù)的發(fā)展，移動(dòng)物體軌跡的應(yīng)用也日益廣泛，如車輛異常狀態(tài)的分析、車輛的動(dòng)態(tài)軌跡預(yù)測等[1]。但由于 GPS 定位受到多種誤差的影響[2-3]，極大限制了高精度實(shí)時(shí)定位的應(yīng)用。

在動(dòng)態(tài)軌跡修正領(lǐng)域，常用的技術(shù)有標(biāo)準(zhǔn)卡爾曼濾波和標(biāo)準(zhǔn)粒子濾波。雖然它們的可行性已經(jīng)得到研究的驗(yàn)證[4-5]，但在實(shí)際應(yīng)用中濾波算法會(huì)產(chǎn)生精度不高、濾波發(fā)散等問題。當(dāng)前國內(nèi)外針對(duì)標(biāo)準(zhǔn)濾波算法的不足，已經(jīng)提出了一系列的改進(jìn)算法。

在提高卡爾曼濾波算法的精度方面，學(xué)者們從不同角度提出基于卡爾曼濾波的改進(jìn)算法，如結(jié)合最小二乘法的卡爾曼濾波算法[6]，運(yùn)用位置差分方法提出帶速度約束條件的卡爾曼濾波算法和擴(kuò)展卡爾曼濾波等[7-8]。還有學(xué)者通過修正狀態(tài)方程中的誤差項(xiàng)以改善濾波效果[9]。為了避免濾波發(fā)散，有學(xué)者提出一種漸消因子改進(jìn)算法，實(shí)現(xiàn)卡爾曼濾波的自適應(yīng)[10]。作為粒子濾波算法的主要缺陷，粒子退化問題受到學(xué)者的廣泛討論與研究，解決該問題最有效的方法是選擇好重要性概率密度函數(shù)或采用重采樣方法。在提高粒子濾波實(shí)時(shí)性的方面，有學(xué)者提出自適應(yīng)粒子濾波（APF）和實(shí)時(shí)粒子濾波（RTPF）等改進(jìn)算法[11]。

標(biāo)準(zhǔn)卡爾曼濾波方法中使用的線性模型不能刻畫物體的某些復(fù)雜曲線運(yùn)動(dòng)，雖然一些基于非線性模型的改進(jìn)卡爾曼濾波算法和部分改進(jìn)的粒子濾波算法能夠解決該問題，但這些方法較為復(fù)雜，且無法達(dá)到某些應(yīng)用領(lǐng)域的要求。因此，需要一種較為簡便的濾波方法，繼續(xù)使用線性模型來描述動(dòng)態(tài)運(yùn)動(dòng)，并盡可能縮小誤差范圍，保證濾波的效果。

提出了一種基于真實(shí)值點(diǎn)構(gòu)造分段三次Hermite插值函數(shù)的改進(jìn)思路，將其運(yùn)用于標(biāo)準(zhǔn)卡爾曼濾波和標(biāo)準(zhǔn)粒子濾波，分別得到兩種改進(jìn)的濾波算法。該類算法通過Hermite插值函數(shù)近似移動(dòng)物體的真實(shí)軌跡，達(dá)到濾波預(yù)測值更接近真實(shí)情況的目的。實(shí)驗(yàn)結(jié)果表明，卡爾曼濾波算法和粒子濾波算法的濾波效果均有改善，其中卡爾曼濾波的改進(jìn)效果尤為顯著。與先前的研究相比，上述方法簡便易行，對(duì)于移動(dòng)物體的軌跡修正精度有顯著提高。

1? ?移動(dòng)物體的運(yùn)動(dòng)學(xué)模型

在BJ-54坐標(biāo)系下，考慮物體的定常速曲線運(yùn)動(dòng)模型，以物體的位置和速度作為狀態(tài)變量，并假定系統(tǒng)噪聲和測量噪聲均為高斯白噪聲。設(shè)物體的狀態(tài)向量為Xt = [xt? ?yt? ?vx，t? ?vy，t]T。其中，xt、yt分別為t時(shí)刻物體所在位置的BJ-54橫坐標(biāo)、縱坐標(biāo);vx，t、vy，t分別為t時(shí)刻物體在橫坐標(biāo)軸、縱坐標(biāo)軸方向上的速度。

此時(shí)，狀態(tài)方程為：

式中，At|t-1為從t-1時(shí)刻到t時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣。

狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)矩陣分別為[12]：

式中，ωt為t時(shí)刻目標(biāo)物體的角速度;dT為GPS采樣的時(shí)間間隔。

觀測方程為：

Zt = HXt + νt（4）

式中，Zt為觀測向量;H為測量矩陣，H = I4 為4階單位矩陣。

2? ?標(biāo)準(zhǔn)濾波模型及算法

2.1? ?標(biāo)準(zhǔn)卡爾曼濾波算法

標(biāo)準(zhǔn)卡爾曼濾波算法是基于狀態(tài)空間的一套遞推濾波算法，它主要包括狀態(tài)方程和觀測方程。標(biāo)準(zhǔn)卡爾曼濾波過程主要由以下公式組成：

式中，At|t-1為狀態(tài)轉(zhuǎn)移矩陣;[^][X]t為t時(shí)刻狀態(tài)向量的濾波估計(jì)值;[^][P]t為t時(shí)刻誤差的協(xié)方差矩陣;Q = σ2

uBBT為系統(tǒng)噪聲的協(xié)方差矩陣;R為測量噪聲的協(xié)方差矩陣;Kt為t時(shí)刻的卡爾曼濾波增益值。

2.2? ?標(biāo)準(zhǔn)粒子濾波算法

標(biāo)準(zhǔn)粒子濾波算法是一種基于貝葉斯理論和蒙特卡羅方法的統(tǒng)計(jì)濾波算法。其基本思想是先在狀態(tài)空間中產(chǎn)生一組加權(quán)隨機(jī)樣本（粒子），然后在測量數(shù)據(jù)的基礎(chǔ)上通過調(diào)節(jié)粒子的權(quán)重大小來獲得服從實(shí)際分布的樣本，并以樣本均值作為系統(tǒng)狀態(tài)的估計(jì)值。

粒子濾波的具體實(shí)現(xiàn)步驟如下[11，13]：

Step1：初始化k = 0，采樣xi

0 ～ p（x0），即根據(jù)分布p（x0）采樣得到xi

Step2：重要性權(quán)值計(jì)算。設(shè)定k ：= k + 1，采樣xi

重要性權(quán)值計(jì)算如下：

Step5：判斷是否結(jié)束，若是則退出本算法，若否則返回Step2。

3? ?改進(jìn)的濾波算法

3.1? ?改進(jìn)的思路

上述兩種濾波算法均利用狀態(tài)轉(zhuǎn)移矩陣At|t-1刻畫移動(dòng)物體從t-1時(shí)刻到t時(shí)刻的狀態(tài)變化，并結(jié)合實(shí)際觀測值Zt，求得t時(shí)刻濾波預(yù)測值[^][X]t，再讓 [^][X]t參與下一步的迭代。但在實(shí)際應(yīng)用中路況復(fù)雜，運(yùn)動(dòng)學(xué)公式無法準(zhǔn)確描述移動(dòng)物體的實(shí)際運(yùn)動(dòng)軌跡，從而經(jīng)濾波后得到的軌跡與實(shí)際軌跡有較大偏移?？紤]到應(yīng)用中存在上述不足，本文提出一種改進(jìn)思路應(yīng)用于濾波算法，用作移動(dòng)物體的軌跡修正。事實(shí)上，物體的移動(dòng)軌跡上存在一些關(guān)鍵點(diǎn)，如連續(xù)轉(zhuǎn)向點(diǎn)。而這些轉(zhuǎn)向點(diǎn)一般存在于特殊路段如十字路口、高速的匝道等，該處經(jīng)緯度可以事先精確測量。

基于上述考慮，給定移動(dòng)物體軌跡上的n個(gè)已知經(jīng)緯度坐標(biāo)的真實(shí)值點(diǎn)列S = {Pi|i = 1，2，…，n}，利用分段插值方法做出一段曲線，再將由運(yùn)動(dòng)學(xué)公式計(jì)算得到的t時(shí)刻位置的橫坐標(biāo)xi代入插值曲線方程，可以計(jì)算出預(yù)測的真實(shí)值點(diǎn)的縱坐標(biāo)yi*。然后，對(duì)（xi，yi）和（xi，yi*）經(jīng)過加權(quán)修正得到（xi，[y][～]i）以減少運(yùn)動(dòng)學(xué)公式利用帶來的誤差。最后，將（xi，

[y][～]i）作為修正后的理論值中間變量代入濾波過程，可求得修正后的濾波預(yù)測值（[x][^]i，[y][^]i）。

改進(jìn)濾波算法的具體思路見圖1。

3.2? ?真實(shí)值點(diǎn)列的選擇

針對(duì)定常速曲線運(yùn)動(dòng)情形，主要選取在方向發(fā)生較大變化的拐點(diǎn)、周圍存在較多的隨機(jī)因素的路口等位置。Hermite插值方法需要區(qū)間端點(diǎn)的位置和導(dǎo)數(shù)值，本文用待求點(diǎn)的一階差商作為該點(diǎn)導(dǎo)數(shù)值的近似替代，在實(shí)際算法中，只需再一個(gè)構(gòu)造輔助的真實(shí)點(diǎn)列[S][～] = {[P][～]i|i = 1，2，…，n}。

對(duì)Pi和[P][～]i的坐標(biāo)作差商，將得到的結(jié)果作為真實(shí)值點(diǎn)Pi的導(dǎo)數(shù)值，用于插值函數(shù)的構(gòu)造。實(shí)際應(yīng)用中可以借助于速度傳感器等設(shè)備搜集一階導(dǎo)數(shù)信息，不必依賴真實(shí)值點(diǎn)。只是在本研究中受限于儀器設(shè)備，故作該處理，因此后文在統(tǒng)計(jì)所需真實(shí)值點(diǎn)的數(shù)目時(shí)，未將輔助點(diǎn)數(shù)目納入其中。引入真實(shí)值點(diǎn)列具有重要意義：根據(jù)點(diǎn)的選取原則，真實(shí)值點(diǎn)列為軌跡上關(guān)鍵的，或者測量中容易發(fā)生偏移的位置。那么，運(yùn)用點(diǎn)列信息后就可以減少點(diǎn)列附近測量數(shù)據(jù)的偏移。在上述方法中，還需指出，僅利用插值函數(shù)修正理論值中間變量的位置信息，對(duì)于速度和方向沒有改動(dòng)。

3.3? ?分段三次Hermite插值

設(shè)真實(shí)軌跡函數(shù)為fi（x），i = 1，2，…，n，相鄰兩個(gè)真實(shí)值點(diǎn)Pi和Pi+1的BJ-54坐標(biāo)為（xi

式中，α1和α2為權(quán)重因子，分別衡量對(duì)狀態(tài)方程和真實(shí)值點(diǎn)的依賴程度。當(dāng)α1 = 1，α2 = 0時(shí)，退化為標(biāo)準(zhǔn)濾波算法。一般地，α1和α2在（0，1）內(nèi)取值。

3.4? ?Hermite-KF算法（HKF）

HKF算法過程主要由以下公式組成：

式中，Xt*為修正后的理論值，其它符號(hào)的含義同前。

HKF的主要步驟如下：

Step1：參數(shù)值的初始化：給定初始狀態(tài)X0，誤差協(xié)方差矩陣初值P0 = I4，其中I4為四階單位矩陣;系統(tǒng)噪聲的協(xié)方差矩陣Q = 10-3 × BBT;測量噪聲的協(xié)方差矩陣R為對(duì)角矩陣，可由單點(diǎn)定位誤差的方差和速度的測量方差得到;

Step2：狀態(tài)更新：根據(jù)t - 1時(shí)刻的預(yù)測狀態(tài)[^][X]t 結(jié)合狀態(tài)方程計(jì)算第t時(shí)刻的狀態(tài)Xt;

Step3：加權(quán)修正：根據(jù)第二步中Xt確定位置區(qū)間和運(yùn)動(dòng)方向，尋找對(duì)應(yīng)的插值區(qū)間，并運(yùn)用Hermit插值法計(jì)算插值函數(shù)的函數(shù)值H（Xt），用算術(shù)平均修正位置信息，得到修正后的狀態(tài)量Xt*;

Step4：誤差更新：根據(jù)第t - 1時(shí)刻的過程預(yù)測誤差[^][P]t估計(jì)t時(shí)刻的過程預(yù)測誤差[P][～]t;

Step5：增益計(jì)算：計(jì)算卡爾曼增益Kt;

Step6：濾波預(yù)測：結(jié)合實(shí)際觀測值計(jì)算系統(tǒng)最優(yōu)估算值[^][X]t;

Step7：誤差預(yù)測：計(jì)算系統(tǒng)當(dāng)前時(shí)刻的系統(tǒng)預(yù)測誤差[^][P]t。

3.5? ?Hermite-PF算法（HPF）

HPF算法的狀態(tài)方程和觀測方程分別為：

Xt* = F（At|t-1 [^][X]t-1 ） + But - 1

Zt = HXt + νt（10）

其余迭代步驟同前。

4? ?實(shí)驗(yàn)與分析

4.1? ?實(shí)驗(yàn)設(shè)計(jì)

實(shí)驗(yàn)選取ATGM332D雙模接收機(jī)作為GPS數(shù)據(jù)接收工具，結(jié)合自行編寫的BD-GPS數(shù)據(jù)采集系統(tǒng)對(duì)衛(wèi)星數(shù)據(jù)進(jìn)行解碼，獲取經(jīng)緯度、海拔高度及速度等信息。在南京奧體中心進(jìn)行實(shí)地實(shí)驗(yàn)，對(duì)定位數(shù)據(jù)分別采用KF、HKF、PF、HPF進(jìn)行濾波。實(shí)驗(yàn)結(jié)果以均方根誤差作為評(píng)價(jià)標(biāo)準(zhǔn)，衡量濾波算法的改善效果，并對(duì)真實(shí)值點(diǎn)列個(gè)數(shù)進(jìn)行靈敏度分析。

改進(jìn)的濾波算法中真實(shí)值點(diǎn)列經(jīng)緯度坐標(biāo)的確定：在軌跡上取12個(gè)關(guān)鍵點(diǎn)，每個(gè)點(diǎn)用GPS接收機(jī)采集1000個(gè)經(jīng)緯度定位數(shù)據(jù)，后進(jìn)行單點(diǎn)濾波[15]，將濾波的平均值作為真實(shí)值對(duì)應(yīng)的經(jīng)緯度。

4.2? ?數(shù)據(jù)預(yù)處理

首先剔除野點(diǎn)。由于運(yùn)動(dòng)模型中采用XY坐標(biāo)而非經(jīng)緯度，因此需要進(jìn)行坐標(biāo)變換[16-17]。圖2是南京奧體中心經(jīng)緯度數(shù)據(jù)轉(zhuǎn)換后的物體運(yùn)動(dòng)軌跡。

4.3? ?實(shí)驗(yàn)結(jié)果及分析

4.3.1? ?濾波算法的效果比較

在定常速曲線運(yùn)動(dòng)情形下，分別利用KF、HKF、PF、HPF對(duì)定位數(shù)據(jù)進(jìn)行修正，這里借助的真實(shí)值點(diǎn)的數(shù)目為7。

圖3中的a，b，c，d分別為KF、HKF、PF、HPF的濾波效果圖。圖中theoretical表示理論公式計(jì)算得到的坐標(biāo);metrical表示實(shí)際測量值;estimated表示濾波估計(jì)的位置坐標(biāo)。不難看出，相比于標(biāo)準(zhǔn)算法，兩種改進(jìn)濾波算法的理論值與實(shí)際值更加接近，這是引入真實(shí)值點(diǎn)陣修正后的效果，更加準(zhǔn)確地刻畫實(shí)際測量中的道路軌跡，進(jìn)而可以達(dá)到讓濾波更為準(zhǔn)確的目的。

進(jìn)一步地，將濾波結(jié)果同真實(shí)值相比，分別做出兩種濾波下x坐標(biāo)和y坐標(biāo)的誤差圖，見圖3中e，f，g，h?？梢钥吹?，HKF的誤差較KF有顯著減小;HPF在y方向上誤差較PF有顯著減小，在x方向上有些許增大?？傮w來說，兩種算法的誤差均有減小。這一點(diǎn)也可以從兩種濾波方法下位置坐標(biāo)的均方根誤差中看出，根據(jù)均方根誤差計(jì)算式：

RMSEx = ，（11）

RMSEy = .（12）

得在四種濾波下的x、y方向上的均方誤差，見表1。

由表1知，卡爾曼濾波算法和粒子濾波算法均有改進(jìn)。

4.3.2? ?靈敏度分析

對(duì)于改進(jìn)的濾波方法，真實(shí)值點(diǎn)的數(shù)量與誤差減小之間需要進(jìn)行權(quán)衡。如果所需要的點(diǎn)數(shù)過多，那么前期采集真實(shí)值的工作量就變得龐大，改進(jìn)濾波方法的實(shí)用價(jià)值自然也就不大。因此，以下針對(duì)真實(shí)值點(diǎn)的個(gè)數(shù)進(jìn)行靈敏度分析。

圖4是HKF和HPF在x、y方向上偏移量示意圖，橫坐標(biāo)表示時(shí)間，縱坐標(biāo)表示對(duì)應(yīng)時(shí)刻真實(shí)點(diǎn)和濾波預(yù)測點(diǎn)坐標(biāo)的偏移量。對(duì)于HKF，不同的真實(shí)值點(diǎn)數(shù)對(duì)于x、y方向的誤差影響較小;對(duì)于HPF，除了在5個(gè)真實(shí)值點(diǎn)時(shí)由于真實(shí)值點(diǎn)數(shù)量太少導(dǎo)致y坐標(biāo)誤差較大外，借助多于8個(gè)真實(shí)值點(diǎn)時(shí)x、y方向上誤差均在（-5，15）內(nèi)波動(dòng)，這說明真實(shí)值點(diǎn)的數(shù)量對(duì)于誤差的減小效果并不敏感。

計(jì)算在不同真實(shí)值點(diǎn)數(shù)下兩種改進(jìn)濾波的均方根誤差，見表2和表3。結(jié)果表明，隨著真實(shí)值點(diǎn)數(shù)目的增加，均方根誤差總體上呈減小趨勢，但效果不明顯，這表明在運(yùn)用中，只需要依賴較少的真實(shí)點(diǎn)，就可以達(dá)到減小定位誤差的目的。

研究還表明，當(dāng)真實(shí)值點(diǎn)取在道路上的關(guān)鍵點(diǎn)，比如軌跡的起始點(diǎn)、終點(diǎn)和拐點(diǎn)的時(shí)候，濾波效果更好，均方根誤差更小。針對(duì)選用7個(gè)真實(shí)值點(diǎn)

研究還表明，當(dāng)真實(shí)值點(diǎn)取在道路上的關(guān)鍵點(diǎn)，比如軌跡的起始點(diǎn)、終點(diǎn)和拐點(diǎn)的時(shí)候，濾波效果更好，均方根誤差更小。針對(duì)選用7個(gè)真實(shí)值點(diǎn)時(shí)，在對(duì)所取點(diǎn)的位置進(jìn)行分析后得出，選取關(guān)鍵點(diǎn)可以顯著降低誤差，效果甚至優(yōu)于借助更多的真實(shí)值點(diǎn)，改進(jìn)卡爾曼濾波的效果見表4。

表4? ?不同取點(diǎn)位置下HKF的誤差比較

[點(diǎn)列數(shù) 7點(diǎn) 7點(diǎn)關(guān)鍵點(diǎn) RMSEx /m 4.488 8 4.487 5 RMSEy /m 3.498 2 3.480 2 ]

5? ?結(jié)? ?論

對(duì)標(biāo)準(zhǔn)濾波算法進(jìn)行優(yōu)化，在狀態(tài)方程中融入由真實(shí)值點(diǎn)列構(gòu)成的插值多項(xiàng)式，根據(jù)物體的運(yùn)動(dòng)方式，對(duì)插值多項(xiàng)式和運(yùn)動(dòng)學(xué)公式選取不同的權(quán)值，改進(jìn)狀態(tài)方程，使得對(duì)下一時(shí)刻狀態(tài)的估計(jì)更加精確。實(shí)驗(yàn)結(jié)果表明，借助于移動(dòng)物體軌跡上較少的關(guān)鍵位置的真實(shí)點(diǎn)，改進(jìn)卡爾曼濾波算法和改進(jìn)粒子濾波算法可以實(shí)現(xiàn)對(duì)一段軌跡的經(jīng)緯度的修正，且在精度上相比標(biāo)準(zhǔn)算法得到了提高。在實(shí)際應(yīng)用中，只需事先在計(jì)劃軌跡上標(biāo)定真實(shí)點(diǎn)，以后無需標(biāo)定。該方法成本低、效果好。但是受限于時(shí)間和經(jīng)費(fèi)，沒有進(jìn)行大量的實(shí)驗(yàn)，僅僅對(duì)定常速曲線運(yùn)動(dòng)情況進(jìn)行了分析，對(duì)于更一般的運(yùn)動(dòng)形式有待進(jìn)一步分析。

參考文獻(xiàn)

[1]? ? 陳志軍，吳超仲，呂能超，等. 基于改進(jìn)三次Hermite插值的車輛時(shí)空軌跡重構(gòu)研究[J]. 交通信息與安全，2013，31（6）：43-46.

[2]? ? ELLIOT D K，CHRISTOPHER J H. GPS原理與應(yīng)用[M]. 寇艷紅，譯. 北京：電子工業(yè)出版社，2007.

[3]? ? 劉基余，李征航，王躍虎，等. 全球定位系統(tǒng)原理及應(yīng)用[M]. 北京：測繪出版社，1993.

[4]? ? 劉嬋媛，陳國光. 基于GPS的卡爾曼濾波技術(shù)研究[J]. 彈箭與制導(dǎo)學(xué)報(bào)，2006，26（4）：110-112.

[5]? ? 張塹，高天德. 基于GPS的動(dòng)態(tài)目標(biāo)定位濾波方法[J]. 計(jì)算機(jī)仿真，2012，29（5）：319-321，337.

[6]? ? 陳鵬，錢徽，朱淼良.基于加權(quán)最小二乘的卡爾曼濾波算法[J]. 計(jì)算機(jī)科學(xué)，2009，36（11）：230-231，257.

[7]? ? 陳寶遠(yuǎn)，孫宇思，陳光毅，等. 一種基于卡爾曼濾波的動(dòng)態(tài)目標(biāo)GPS定位算法[J]. 哈爾濱理工大學(xué)學(xué)報(bào)，2016，21（4）：1-6.

[8]? ? 林雪原，駱卉子，范文強(qiáng). 一種改進(jìn)的GPS動(dòng)態(tài)定位濾波方法[J]. 海軍航空工程學(xué)院學(xué)報(bào)，2010，25（4）：361-364.

[9]? ? 宋迎春. 動(dòng)態(tài)定位中的卡爾曼濾波研究[D]. 長沙：中南大學(xué)，2008.

[10]? 蔡艷輝，程鵬飛，李夕銀.用卡爾曼濾波進(jìn)行GPS動(dòng)態(tài)定位[J]. 測繪通報(bào)，2006（7）：6-8.

[11]? 朱志宇. 粒子濾波算法及其應(yīng)用[M]. 北京：科學(xué)出版社，2010.

[12] ACHUTEGUI K，RRODAS J，ESCUDERO C J，et al. Bayesian filtering methods for target tracking in mixed indoor/outdoor environments[C]. International Conference on Mobile Lightweight Wireless Systems，2012：169-185.

[13]? 胡洪濤，敬忠良，李安平，等. 非高斯條件下基于粒子濾波的目標(biāo)跟蹤[J]. 上海交通大學(xué)學(xué)報(bào)，2004，38（12）：1996-1999.

[14]? 關(guān)冶，陸金甫.數(shù)值分析基礎(chǔ)（第二版）[M]. 北京：高等教育出版社，2010.

[15]? 孫罡，王昌明，張愛軍. GPS靜態(tài)單點(diǎn)定位的濾波算法比較[J]. 南京理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2011，35（1）：80-85.

[16]? 王解先，王軍，陸彩萍. GPS測定坐標(biāo)轉(zhuǎn)換至地方坐標(biāo)[J]. 全球定位系統(tǒng)，2003，28（4）：7-10.

[17]? 蔣小軍，宋占峰，吳清華. 地方獨(dú)立坐標(biāo)系與WGS-84坐標(biāo)系轉(zhuǎn)換方法及應(yīng)用[J]. 鐵道勘察，2010，36（4）：8-10，13.