粒子群優(yōu)化噪聲參數(shù)的行人導(dǎo)航零速修正算法*

李 磊，蘇 中，吳學(xué)佳，雷 明，王一靜

(北京信息科技大學(xué)高動(dòng)態(tài)導(dǎo)航技術(shù)北京市重點(diǎn)實(shí)驗(yàn)室，北京 100192)

隨著導(dǎo)航定位技術(shù)的不斷發(fā)展，各行各業(yè)對(duì)于位置服務(wù)的需求日益增加。行人導(dǎo)航是導(dǎo)航定位領(lǐng)域的重要課題之一，行人導(dǎo)航系統(tǒng)(Pedestrian Navigation System，PNS)在軍用和民用領(lǐng)域都具有重要作用[1]。目前，成熟的行人導(dǎo)航系統(tǒng)大部分仍基于基礎(chǔ)設(shè)施，如全球?qū)Ш叫l(wèi)星系統(tǒng)(Global Navigation Satellite System，GNSS)、超寬帶和無(wú)線局域網(wǎng)等，但對(duì)于地下遮蔽空間、封閉建筑物內(nèi)部等環(huán)境，易受到信號(hào)遮擋、基礎(chǔ)設(shè)施損毀或設(shè)備需預(yù)先布置等條件約束，上述行人導(dǎo)航系統(tǒng)通常難以實(shí)現(xiàn)[2]。此類復(fù)雜環(huán)境下，主要應(yīng)用慣性測(cè)量單元(Inertial Measurement Unit，IMU)的自主定位定向技術(shù)成為行人導(dǎo)航的研究熱點(diǎn)。隨著微機(jī)電系統(tǒng)(Micro-Electro-Mechanical System，MEMS)的迅速發(fā)展，MEMS-IMU 成為復(fù)雜環(huán)境下行人自主定位定向的主要設(shè)備[3]。由于慣性器件本身存在漂移，定位誤差隨時(shí)間迅速累積[4]。行人慣性導(dǎo)航系統(tǒng)通常使用零速修正(Zero Velocity Update，ZUPT)方法修正定位誤差，ZUPT 要求將IMU 固定于行人足部，檢測(cè)足部靜止階段，再通過(guò)擴(kuò)展卡爾曼濾波(Extended Kalman Filter，EKF)利用零速區(qū)間的速度誤差作為量測(cè)來(lái)修正捷聯(lián)解算的結(jié)果[5]。田曉春等[6]構(gòu)造了一種小波系數(shù)介于軟硬閾值之間的連續(xù)小波閾值函數(shù)，對(duì)MEMS 陀螺儀信號(hào)進(jìn)行降噪后應(yīng)用于行人導(dǎo)航系統(tǒng)，通過(guò)實(shí)驗(yàn)對(duì)比了不同閾值法去噪后的陀螺儀信號(hào)，但未進(jìn)行行人導(dǎo)航實(shí)驗(yàn)。余志鵬等[7]采用秩卡爾曼濾波來(lái)適應(yīng)非高斯噪聲對(duì)系統(tǒng)的影響，在時(shí)間更新前進(jìn)行秩采樣，再使用采樣點(diǎn)集進(jìn)行卡爾曼濾波，在室內(nèi)環(huán)境下實(shí)驗(yàn)證明行人導(dǎo)航系統(tǒng)定位精度有所提高。

對(duì)于使用EKF 實(shí)現(xiàn)行人ZUPT 方法，系統(tǒng)建模誤差和測(cè)量環(huán)境的不確定性對(duì)濾波器性能有重要影響，上述方法考慮了慣性器件在使用中產(chǎn)生的噪聲信號(hào)，而濾波器中合適的噪聲參數(shù)也能夠有效提高行人慣性導(dǎo)航定位精度，但噪聲參數(shù)通常未知或給定的參數(shù)不適用，將導(dǎo)致定位誤差較大。劉韜等[8]利用Sage-Husa 濾波和Huber 函數(shù)建立自適應(yīng)擴(kuò)展卡爾曼濾波(Adaptive Extended Kalman Filter，AEKF)，分別對(duì)系統(tǒng)過(guò)程噪聲協(xié)方差Q和量測(cè)噪聲協(xié)方差R進(jìn)行在線修正并應(yīng)用于室內(nèi)無(wú)人車定位，實(shí)驗(yàn)結(jié)果表明僅在線修正Q時(shí)平均誤差反而會(huì)增大。胡高歌等[9]針對(duì)過(guò)程噪聲協(xié)方差的不確定問(wèn)題提出了一種基于極大似然估計(jì)的無(wú)味卡爾曼濾波，在濾波過(guò)程中對(duì)Q進(jìn)行極大似然估計(jì)并將問(wèn)題轉(zhuǎn)化為對(duì)Q的對(duì)角元素的估計(jì)。Yan 等[10]提出了利用遞歸神經(jīng)網(wǎng)絡(luò)對(duì)IMU 輸出序列進(jìn)行某些運(yùn)動(dòng)模式的識(shí)別，并通過(guò)知識(shí)庫(kù)獲取Q值，還提出了一種逐步增加數(shù)據(jù)子集的神經(jīng)網(wǎng)絡(luò)訓(xùn)練方法以提高網(wǎng)絡(luò)的訓(xùn)練效率。該方法在無(wú)人機(jī)組合導(dǎo)航系統(tǒng)上取得了良好的定位精度，但行人足部相對(duì)于無(wú)人機(jī)的慣性信息更加多變，此類方法可能仍需建立足夠龐大的行人慣性數(shù)據(jù)集。

本文在僅使用由三軸陀螺儀和三軸加速度計(jì)構(gòu)成的MEMS-IMU 的情況下，提出了一種基于粒子群優(yōu)化(Particle Swarm Optimization，PSO)的ZUPT 算法，該算法通過(guò)零速修正時(shí)最小化濾波器新息序列來(lái)優(yōu)化EKF 的噪聲參數(shù)，能夠在零速區(qū)間自主調(diào)整過(guò)程噪聲參數(shù)和量測(cè)噪聲參數(shù)，得到更加精確、平滑的行人運(yùn)動(dòng)軌跡。本文分別使用ZUPT 算法和提出的PSO-ZUPT 算法對(duì)行人實(shí)測(cè)足部慣性數(shù)據(jù)進(jìn)行解算，實(shí)現(xiàn)了行人自主定位定向，并對(duì)兩種算法解算的運(yùn)動(dòng)軌跡及定位誤差進(jìn)行了比較分析，充分驗(yàn)證了所提出算法的有效性。

1 粒子群優(yōu)化的零速修正算法

將IMU 捆綁于行人足部，獲取慣性數(shù)據(jù)后進(jìn)行捷聯(lián)解算，通過(guò)零速檢測(cè)識(shí)別人員足部的靜止階段后得到零速區(qū)間的速度誤差量測(cè)，利用EKF 對(duì)解算結(jié)果進(jìn)行零速修正[11]。噪聲參數(shù)對(duì)EKF 的性能有重要影響，本文利用PSO 算法自主優(yōu)化EKF 的過(guò)程噪聲和量測(cè)噪聲參數(shù)，實(shí)現(xiàn)行人慣性導(dǎo)航，算法流程如圖1 所示。

圖1 算法流程圖

1.1 捷聯(lián)解算

捷聯(lián)解算過(guò)程主要包括姿態(tài)更新、速度更新和位置更新。IMU 捆綁位置及坐標(biāo)系如圖2 所示，選取“北-東-地”作為導(dǎo)航坐標(biāo)系，與地理坐標(biāo)系一致。選取人員足部的“前-右-下”作為載體坐標(biāo)系，IMU 固定在人員左足跟部，獲取行人行走時(shí)足部的加速度和角速度信息，行人運(yùn)動(dòng)軌跡為長(zhǎng)40.3 m、寬16.5 m 的矩形，IMU 輸出的原始數(shù)據(jù)如圖3 所示。

圖2 IMU 捆綁位置及坐標(biāo)系

圖3 IMU 輸出原始數(shù)據(jù)

利用如下導(dǎo)航方程對(duì)行人的姿態(tài)、速度、位置進(jìn)行計(jì)算:

1.2 零速檢測(cè)

零速檢測(cè)的主要任務(wù)是識(shí)別行人足部的靜止階段，并確定步態(tài)周期內(nèi)的零速區(qū)間。使用廣義似然比檢測(cè)法(Generalized Likelihood Ratio Test，GLRT)進(jìn)行零速檢測(cè)[12]，第n組IMU 輸出序列的檢驗(yàn)統(tǒng)計(jì)量為:

式中:T(un)為檢驗(yàn)統(tǒng)計(jì)量，W為滑動(dòng)窗口大小。un為窗口內(nèi)傳感器輸出分別為加速度計(jì)和陀螺儀的輸出值，為滑動(dòng)窗口內(nèi)加速度計(jì)輸出值的平均值，g為當(dāng)?shù)刂亓铀俣龋謩e為加速度計(jì)和陀螺儀的噪聲方差。Ck為零速檢測(cè)輸出標(biāo)志位，γ為零速檢測(cè)閾值。

1.3 零速修正

在行人行走過(guò)程中，足部周期性地與地面接觸，ZUPT 算法將足部靜止階段的速度輸出作為速度誤差的量測(cè)值，使用EKF 對(duì)導(dǎo)航狀態(tài)誤差進(jìn)行濾波估計(jì)，在量測(cè)更新過(guò)程中進(jìn)行零速修正。

行人慣性導(dǎo)航系統(tǒng)的十五維向量Xk為:

式中:pk表示k時(shí)刻位置、vk表示k時(shí)刻速度、φk表示k時(shí)刻姿態(tài)、ak表示k時(shí)刻加速度計(jì)零偏誤差，ωk表示k時(shí)刻陀螺儀零偏誤差。

取導(dǎo)航系統(tǒng)狀態(tài)向量的誤差項(xiàng)δXk建立卡爾曼濾波方程:

以上15 維狀態(tài)誤差向量分別為k時(shí)刻的三軸位置誤差、速度誤差、姿態(tài)誤差、加速度計(jì)零偏誤差和陀螺儀零偏誤差。

慣性導(dǎo)航系統(tǒng)的狀態(tài)誤差方程如下:

式中:Δt為采樣時(shí)間，ft為加速度計(jì)測(cè)量值的反對(duì)稱矩陣，b1和b2分別為加速度計(jì)和陀螺儀零偏誤差的比例系數(shù)，Wk·a和Wk·ω分別為k時(shí)刻的加速度計(jì)和陀螺儀誤差的隨機(jī)系統(tǒng)動(dòng)態(tài)噪聲。

當(dāng)行人足部處于靜止階段時(shí)，足部的理論速度應(yīng)該為零，但由于存在測(cè)量誤差等原因，靜止階段計(jì)算得到的速度并不為零。因此選擇靜止階段的速度誤差作為量測(cè)向量Zk:

由此可以得到離散時(shí)間的線性系統(tǒng)狀態(tài)空間模型:

Wk-1和Vk分別表示過(guò)程噪聲和量測(cè)噪聲，其噪聲統(tǒng)計(jì)為:

式中:Qk表示k時(shí)刻過(guò)程噪聲協(xié)方差矩陣，Rk表示k時(shí)刻量測(cè)噪聲協(xié)方差矩陣，δkg表示Kronecker 函數(shù)。在標(biāo)準(zhǔn)EKF 中，需設(shè)定合適的Qk和Rk。

EKF 濾波過(guò)程主要分為時(shí)間更新和量測(cè)更新，時(shí)間更新過(guò)程中，EKF 將誤差狀態(tài)轉(zhuǎn)移到捷聯(lián)解算系統(tǒng)中，在測(cè)量后立即將誤差狀態(tài)置零，并計(jì)算先驗(yàn)估計(jì)誤差協(xié)方差矩陣。當(dāng)Ck＝0 時(shí)，不進(jìn)行量測(cè)更新；當(dāng)Ck＝1 時(shí)，進(jìn)入量測(cè)更新過(guò)程，執(zhí)行ZUPT 算法。

1.4 粒子群優(yōu)化的零速修正

粒子群優(yōu)化算法主要思想是通過(guò)粒子之間的信息交流在候選解空間內(nèi)尋找最優(yōu)解。PSO 算法作為一種經(jīng)典的群智能優(yōu)化算法被廣泛應(yīng)用于求最優(yōu)解與參數(shù)優(yōu)化問(wèn)題[13]。

本文提出的PSO-ZUPT 算法在行人足部靜止時(shí)，利用PSO 優(yōu)化零速修正階段的濾波器噪聲協(xié)方差矩陣參數(shù)，使濾波器能夠根據(jù)行人的運(yùn)動(dòng)狀態(tài)來(lái)自主調(diào)整參數(shù)。當(dāng)零速檢測(cè)判斷當(dāng)前狀態(tài)處于零速區(qū)間時(shí)，對(duì)系統(tǒng)狀態(tài)誤差進(jìn)行濾波，并執(zhí)行PSO 算法，將當(dāng)前的噪聲協(xié)方差矩陣Qk和Rk的對(duì)角線元素作為粒子的位置輸入到PSO 算法中，并利用當(dāng)前量測(cè)值和估計(jì)值計(jì)算新息序列作為PSO 算法的適應(yīng)度函數(shù)。通過(guò)PSO 算法來(lái)最小化量測(cè)值與預(yù)測(cè)值之間的差，從而自主更新噪聲矩陣Qk和Rk[14]。根據(jù)更新后的協(xié)方差矩陣參數(shù)，分別生成協(xié)方差為Qσi和Rσi的高斯噪聲Wpi和Vpi，并加入到濾波估計(jì)值和量測(cè)值中，以計(jì)算更新后的新息序列，得到優(yōu)化后的最優(yōu)個(gè)體。

EKF 中的過(guò)程噪聲協(xié)方差矩陣Qk和量測(cè)噪聲協(xié)方差矩陣Rk都為對(duì)角矩陣，Qk和Rk的優(yōu)化問(wèn)題可轉(zhuǎn)化為其對(duì)角元素的優(yōu)化問(wèn)題。選取對(duì)角元素值作為PSO 算法中粒子的位置，使用PSO 算法對(duì)加速度計(jì)噪聲、陀螺儀噪聲及速度測(cè)量噪聲的參數(shù)向量進(jìn)行優(yōu)化，第i個(gè)粒子的位置用σi表示:

式中:σai和σωi分別表示加速度計(jì)噪聲和陀螺儀噪聲參數(shù)，σvi為速度測(cè)量噪聲參數(shù)。第i個(gè)粒子的速度vσi和位置σi的更新公式如下:

式中:t表示PSO 算法的迭代次數(shù)，i∈[1，M]，M表示種群個(gè)數(shù)，均為正整數(shù)。w表示慣性權(quán)重，c1和c2表示加速因子，r1和r2為[0，1]之間的隨機(jī)數(shù)。bσi代表個(gè)體最優(yōu)解，gb 代表全局最優(yōu)解。

優(yōu)化后的噪聲協(xié)方差Qσi和Rσi可以通過(guò)粒子的位置σi得到:

式中:σab和σωb分別表示加速度計(jì)和陀螺儀的零位穩(wěn)定性噪聲，采用固定值不參與優(yōu)化。

PSO 算法中適應(yīng)度函數(shù)的設(shè)計(jì)決定了粒子的優(yōu)化目標(biāo)，計(jì)算卡爾曼濾波器新息序列的平方均值作為適應(yīng)度函數(shù)并向新息序列減小的方向優(yōu)化粒子，適應(yīng)度函數(shù)定義為:

式中:zpij為量測(cè)值z(mì)pi的第j維數(shù)值，xpij為濾波估計(jì)值xpi的第j維數(shù)值。zpi和xpi分別引入了參數(shù)為σvi和[σaiσωiσabσωb]T的高斯噪聲，其數(shù)值可以用如下表達(dá)式計(jì)算:

式中:Wpi和Vpi分別為協(xié)方差為Qσi和Rσi的高斯噪聲，即:

由于對(duì)零速區(qū)間的所有時(shí)刻進(jìn)行PSO 參數(shù)優(yōu)化會(huì)導(dǎo)致軌跡解算時(shí)間過(guò)長(zhǎng)，需設(shè)置一個(gè)連續(xù)迭代次數(shù)h，當(dāng)每連續(xù)執(zhí)行h次零速修正后執(zhí)行一次PSO 算法。實(shí)驗(yàn)中，為xpid設(shè)置了不同的搜索邊界來(lái)提高算法的穩(wěn)定性。為提高PSO 算法的效率，還應(yīng)該設(shè)置合適的最大迭代次數(shù)tmax和適應(yīng)度閾值ε。圖4 展示了一次PSO 參數(shù)優(yōu)化過(guò)程中的最優(yōu)個(gè)體適應(yīng)度變化曲線，粒子群在545 次到1 000 次迭代中適應(yīng)度已不再下降，根據(jù)實(shí)驗(yàn)經(jīng)驗(yàn)，將tmax設(shè)為800。在算法1 和算法2 處分別給出PSO 優(yōu)化噪聲協(xié)方差參數(shù)算法流程和PSO-ZUPT 算法流程。

圖4 PSO 最優(yōu)個(gè)體適應(yīng)度曲線

算法1 PSO 優(yōu)化噪聲協(xié)方差參數(shù)算法

算法2 PSO-ZUPT 算法

2 實(shí)驗(yàn)驗(yàn)證及分析

2.1 實(shí)驗(yàn)條件

實(shí)驗(yàn)使用圖5 中實(shí)驗(yàn)室自研的MEMS-IMU 采集行人行走時(shí)的足部慣性數(shù)據(jù)，IMU 僅包含三軸加速度計(jì)和三軸陀螺儀，其相關(guān)技術(shù)指標(biāo)如表1 所示。使用MATLAB R2018b 軟件進(jìn)行慣性數(shù)據(jù)解算，對(duì)算法進(jìn)行驗(yàn)證，計(jì)算機(jī)處理器為Intel Core i7-10700K，32GB運(yùn)行內(nèi)存。

表1 IMU 技術(shù)指標(biāo)

圖5 MEMS-IMU 實(shí)物圖

實(shí)驗(yàn)分別使用標(biāo)準(zhǔn)ZUPT 算法和PSO-ZUPT 算法對(duì)1.1 節(jié)中采集的慣性數(shù)據(jù)進(jìn)行解算，此外，還進(jìn)行了其他軌跡的行人行走實(shí)驗(yàn)。行人運(yùn)動(dòng)軌跡為沿足球場(chǎng)中圈連續(xù)行走3 圈回到起點(diǎn)，如圖6 中實(shí)線圓圈所示，球場(chǎng)中圈半徑為9.15 m，3 圈的路徑總長(zhǎng)度約為172.5 m，共采集5 組足部慣性數(shù)據(jù)。分別使用標(biāo)準(zhǔn)ZUPT 和PSO-ZUPT 兩種算法進(jìn)行解算，對(duì)定位結(jié)果進(jìn)行分析。

圖6 沿足球場(chǎng)中圈的運(yùn)動(dòng)軌跡

PSO 算法中，將M設(shè)為30，c1和c2均為2.05，零速修正連續(xù)迭代次數(shù)h設(shè)為1 000，最大迭代次數(shù)tmax為800。慣性器件噪聲σai和σωi的搜索空間選為[0.01，0.5]，σvi的搜索空間選為[0.001，0.5]。PSOZUPT 算法的噪聲協(xié)方差初值與ZUPT 算法的Q和R相等:

慣性權(quán)重進(jìn)行線性遞減，利用如下公式計(jì)算:

式中:wmax和wmin分別為慣性權(quán)重的最大值和最小值，在實(shí)驗(yàn)中分別設(shè)為1 和0.8。

2.2 實(shí)驗(yàn)結(jié)果

使用兩種算法對(duì)1.1 節(jié)中慣性數(shù)據(jù)解算的結(jié)果如圖7 所示。ZUPT 算法的解算軌跡與真實(shí)軌跡形狀相似，但仍存在較大誤差，軌跡在y 軸方向上的最大誤差約為3.75 m，終點(diǎn)位置誤差約為1.42 m。PSO-ZUPT 算法的解算軌跡更接近真實(shí)軌跡且更加平滑。經(jīng)計(jì)算，最大位置誤差約為1.85 m，終點(diǎn)位置誤差約為0.64 m，與ZUPT 算法結(jié)果相比，定位誤差分別減小了50.67%和54.93%。

圖7 兩種算法解算軌跡對(duì)比圖

對(duì)5 組足球場(chǎng)中圈數(shù)據(jù)的解算位置誤差進(jìn)行計(jì)算，并給出了在MATLAB 軟件下的運(yùn)行時(shí)間，算法A 表示標(biāo)準(zhǔn)ZUPT 算法，算法B 表示PSO-ZUPT 算法，結(jié)果如表2 所示。平均誤差為所有采樣點(diǎn)解算位置到中圈圓心的距離與中圈半徑之差的平均值，終點(diǎn)誤差為解算軌跡終點(diǎn)到起點(diǎn)之間的距離，最大誤差為所有采樣點(diǎn)解算位置到中圈圓心的距離和中圈半徑之差的最大值。

表2 中圈軌跡解算結(jié)果誤差表

由表2 可知，PSO-ZUPT 算法相對(duì)于ZUPT 算法在上述三個(gè)指標(biāo)上均有提高。ZUPT 算法的平均誤差為0.796 m，PSO-ZUPT 算法的平均誤差為0.092 m，平均誤差減小了88.31%，終點(diǎn)誤差減小了34.13%，最大誤差減小了88.66%。5 組數(shù)據(jù)的行人行走時(shí)間在145 s 至151 s 之間，PSO-ZUPT 算法運(yùn)行時(shí)間在86 s 至91 s 之間，相對(duì)于ZUPT 運(yùn)行時(shí)間提高4.3 倍左右，但仍可以支持算法在線運(yùn)行。

第1 組數(shù)據(jù)解算的運(yùn)動(dòng)軌跡如圖8 和圖9 所示，圖8 為ZUPT 解算軌跡，圖9 為PSO-ZUPT 解算軌跡，實(shí)線是作為參考的真實(shí)軌跡，點(diǎn)畫線為解算軌跡。由于PSO-ZUPT 濾波過(guò)程中Qk和Rk是變化的，給出每次PSO 算法中全局最優(yōu)解的平均值:

圖8 ZUPT 解算軌跡

圖9 PSO-ZUPT 解算軌跡

使用標(biāo)準(zhǔn)EKF 需要根據(jù)不同測(cè)試人員的生物特性、不同慣性器件指標(biāo)及不同測(cè)試環(huán)境來(lái)確定合適的Qk和Rk，由圖8 可見(jiàn)，解算軌跡會(huì)出現(xiàn)明顯的修正跳變點(diǎn)，且誤差較大，軌跡逐漸漂移。PSO 算法可以在EKF 量測(cè)過(guò)程中對(duì)Qk和Rk進(jìn)行自主尋優(yōu)，解算軌跡更加平滑，定位精度提高。

為驗(yàn)證算法在長(zhǎng)距離行走下的有效性，實(shí)驗(yàn)中人員沿400 m 跑道行走并形成閉環(huán)路線，采集了行人足部運(yùn)動(dòng)的慣導(dǎo)數(shù)據(jù)，行走軌跡如圖10 所示，圖中跑道處實(shí)線為實(shí)際行走軌跡，圓點(diǎn)為起點(diǎn)位置，行人沿實(shí)際軌跡行走一圈后回到起點(diǎn)位置。

圖10 沿操場(chǎng)跑道的運(yùn)動(dòng)軌跡

分別利用ZUPT 和PSO-ZUPT 對(duì)跑道數(shù)據(jù)進(jìn)行解算，得到的行人運(yùn)動(dòng)軌跡如圖11 所示。在行人長(zhǎng)距離運(yùn)動(dòng)的情況下，ZUPT 解算的軌跡與真實(shí)軌跡之間的誤差明顯，本文提出的算法解算的軌跡更接近真實(shí)軌跡。經(jīng)計(jì)算，ZUPT 算法解算的軌跡終點(diǎn)位置誤差為24.39 m，PSO-ZUPT 算法解算的軌跡終點(diǎn)位置誤差為3.02 m，誤差減小了87.62%，定位精度有明顯提升。

圖11 ZUPT 和PSO-ZUPT 解算軌跡對(duì)比圖

3 總結(jié)

本文在研究ZUPT 進(jìn)行行人自主定位定向的基礎(chǔ)上，提出了一種PSO 優(yōu)化的ZUPT 算法，設(shè)計(jì)新息序列作為適應(yīng)度函數(shù)，可以調(diào)整濾波器的噪聲矩陣參數(shù)。經(jīng)多組實(shí)驗(yàn)驗(yàn)證，該算法能夠有效提高行人零速修正算法解算軌跡的平滑程度，并減小定位誤差。在足球場(chǎng)中圈和400 m 跑道的運(yùn)動(dòng)軌跡下檢驗(yàn)了算法有效性，在足球場(chǎng)中圈行走軌跡的終點(diǎn)位置相對(duì)誤差為0.25%，在400 m 跑道軌跡的終點(diǎn)位置相對(duì)誤差為0.76%，結(jié)果均優(yōu)于標(biāo)準(zhǔn)ZUPT 算法，對(duì)于行人自主定位定向技術(shù)的應(yīng)用具有一定意義。