基于UKF的UWB和GPS融合定位算法①

應(yīng)保勝,周曉帥,方海龍,吳偉偉

(武漢科技大學(xué) 汽車與交通工程學(xué)院,武漢 430065)

隨著智能交通和智能車輛的發(fā)展,汽車駕駛輔助系統(tǒng)、車輛安全等領(lǐng)域?qū)Ω呔榷ㄎ坏男枨笕找婕哟?由高精度定位傳感器與GPS 融合定位導(dǎo)航技術(shù)來改善定位性能成為重要的發(fā)展方向.GPS和IMU 組合定位系統(tǒng)采用卡爾曼濾波進行融合定位,IMU 模塊對加速度和角加速度進行積分的同時,時間積累誤差也逐漸增大[1],卡爾曼濾波器將產(chǎn)生濾波發(fā)散,進而導(dǎo)致定位漂移.超聲波、RFID、WLAN和藍牙等技術(shù)可以通過測距算法解算目標位置坐標,由于信號帶寬和定位誤差成負相關(guān),定位技術(shù)的定位精度受信號帶寬的限制,不適合與GPS 組成精確組合定位系統(tǒng).mmWave雷達的分辨率特性,一般只能分辨目標的距離、角度和速度等信息,在高度上分辨能力差,多采用多輸入多輸出技術(shù)實現(xiàn)目標的定位.文獻[2]考慮到雷達點稀疏性,提出基于mmWave 雷達和視覺傳感器的空間注意力融合障礙物檢測,有效對視覺特征進行融合;文獻[3]同樣結(jié)合攝像頭采集車道特征實現(xiàn)車道級定位.上述方法都采用多輸入方式將mmWave和視覺信號進行融合以提高感知性能,但在汽車復(fù)雜行駛工況下容錯性差.

UWB 定位利用納秒級非正弦窄脈沖載波傳輸數(shù)據(jù)的通訊技術(shù)進行定位,其頻譜范圍極寬,載波信號頻率能達到GHz 級別,定位精度能達到厘米級[4,5],UWB可提供高精度位置感知網(wǎng)絡(luò)中的優(yōu)良解決方案.GPS定位系統(tǒng)雖然無法直接對目標載體的測姿方位角進行估算[6-8],但對載體的速度和運動方位角的估算切實可行,可與UWB 系統(tǒng)輸出共同構(gòu)成組合定位系統(tǒng).

結(jié)合UWB的精度、成本優(yōu)勢和GPS 定位特點,可采用UWB 模塊可與GPS 系統(tǒng)組成多傳感器融合定位系統(tǒng).對此類非線性多源數(shù)據(jù)融合系統(tǒng)[9-11],擴展卡爾曼濾波(extend Kalman filter,EKF)是常用的濾波手段,將非線性系統(tǒng)函數(shù)進行Taylor 級數(shù)展開進而進行卡爾曼濾波,缺點是產(chǎn)生高階誤差,降低系統(tǒng)精度,系統(tǒng)函數(shù)線性化過程中將產(chǎn)生更復(fù)雜的雅可比矩陣和海塞矩陣,增加系統(tǒng)的計算復(fù)雜度,濾波收斂精度收益較小,系統(tǒng)非線性較為嚴重時誤差更大.針對卡爾曼濾波器的濾波發(fā)散問題,文獻[12]中提出了一種基于衰減記憶的最優(yōu)漸消濾波算法,但該算法利用一個漸消因子對高維系統(tǒng)進行自適應(yīng)濾波,系統(tǒng)狀態(tài)的變化不能被漸消因子準確描述,濾波器沒有得到明顯優(yōu)化.在漸消因子濾波的基礎(chǔ)上,文獻[13]又提出次優(yōu)漸消濾波,文獻[14]提出多因子濾波等方法.上述漸消濾波雖然能減小狀態(tài)模型誤差,但漸消濾波對有粗差向量的魯棒性不高,仍可能產(chǎn)生濾波發(fā)散.

為實現(xiàn)汽車定位系統(tǒng)的精確和可靠性,避免多源數(shù)據(jù)融合解算結(jié)果發(fā)散[15,16],減小濾波算法復(fù)雜度,本文提出基于無跡卡爾曼濾波(unscented Kalman filter,UKF)的UWB和GPS 融合定位算法,計算復(fù)雜度降低,能實現(xiàn)定位系統(tǒng)的高階近似.在UWB 數(shù)據(jù)解算方面,優(yōu)化多基站最小二乘定位算法,采用加權(quán)最小二乘法降低遠距離節(jié)點造成測距系統(tǒng)噪聲的影響.研究軌跡糾偏算法方面,基于UWB和GPS 多傳感器物理模型,構(gòu)建非線性系統(tǒng)方程,對構(gòu)建的濾波模型進行系統(tǒng)噪聲誤差分析,優(yōu)化濾波算法;UWB 定位系統(tǒng)獲取的載體的位置坐標和GPS 獲取到運動載體實時的速度和方位角組成觀測向量,構(gòu)建定位系統(tǒng)狀態(tài)方程和觀測方程,采用無跡卡爾曼濾波算法將融合的數(shù)據(jù)進行濾波,達到精確定位的目的.

1 系統(tǒng)架構(gòu)

基于UWB和GPS的高精度組合定位系統(tǒng)物理層由UWB 定位模塊和GPS 模塊兩個子系統(tǒng)組成.UWB模塊通過串口輸出測距數(shù)據(jù)由模塊總成芯片程序通過最小二乘法解算目標載體的位置坐標;GPS 模塊串口輸出的NMEA 報文,經(jīng)模塊總成芯片程序預(yù)先燒錄的解算程序解算目標載體的速度和方位角信息.如圖1所示,基于UWB和GPS的融合定位系統(tǒng)架構(gòu)包括:硬件物理層、數(shù)據(jù)解析層和濾波算法層.其中數(shù)據(jù)解析層是軟硬件中間層,為算法層提供量測向量的數(shù)據(jù)來源;算法層將計算采樣點經(jīng)過非線性系統(tǒng)方程進行無跡變換,利用卡爾曼濾波對量測預(yù)測和均方差預(yù)測進行更新.

圖1 UWB和GPS的融合定位算法架構(gòu)

UWB 模塊依據(jù)TOF 測距算法模型解算被測目標到各個基站間的距離,在最小二乘法的基礎(chǔ)上進行優(yōu)化,利用加權(quán)最小二乘法將UWB 基站測距值解算出待測坐標;GSP 報文信息解析出目標載體的速度v和方位角θ.UWB和GPS 組合定位系統(tǒng)是非線性系統(tǒng).系統(tǒng)模型在擴展卡爾曼濾波算法的基礎(chǔ)上進行優(yōu)化,采用無跡卡爾曼濾波算法對目標軌跡進行高階近似,通過一定規(guī)律的采樣點計算和無跡變換,近似獲得目標載體狀態(tài)向量的均值和方差,對狀態(tài)向量進行循環(huán)更新,實現(xiàn)對定位噪聲的濾波,達到精確定位目的.

2 數(shù)據(jù)解析

定位坐標解算的條件是獲得UWB 節(jié)點間的距離,UWB 節(jié)點由UWB 基站和標簽?zāi)K組成,節(jié)點模塊解算節(jié)點間距離.UWB 坐標解算采用最小二乘(least squares,LS)定位算法,如圖2所示.設(shè)待測標簽坐標為(x,y),標簽到參考基站i的測量距離為d′i,其中,1≤i≤n,n為基站最大序號,標簽到參考基站i的測量距離為di,如式(1)所示.

圖2 UWB 坐標解算算法模型

測量距離和基站與標簽間的真實距離差值記為ρi=d′i-di,利用最小二乘法最小化的值解算標簽的坐標(x,y),利用式(2)解待定坐標X.

其中,

則最小方差解X如式(3)所示:

由于采用傳統(tǒng)最小二乘定位算法計算標簽坐標時將標簽距離所有基站視為相同權(quán)重,實際情況是距離越大誤差越大,本文將加權(quán)最小二乘(weighted least squares,WLS)定位算法用于UWB 坐標解算優(yōu)化,具體解算公式如式(4)所示:

其中,ω是加權(quán)矩陣,其中距離觀測噪聲是均值為0,方差為σ2的高斯白噪聲,其中加權(quán)矩陣ω 如式(5)所示:

為對比傳統(tǒng)LS 定位算法和提出的WLS 定位算法的定位性能,在面積為5 m×5 m的正方形區(qū)域內(nèi)分別對待測點(2,2)進行100 次定位仿真,假設(shè)LS 定位算法和WLS 定位算法都服從方差為σ2=0.01 m2,均值為0且相互獨立分布的高斯白噪聲.LS 定位算法和WLS定位算法仿真結(jié)果如圖3所示.

圖3 LS與WLS 定位算法性能

由仿真數(shù)據(jù)可知,WLS 定位算法定位誤差為12.8 cm,而LS 定位算法誤差為9.1 cm,WLS 定位算法和LS 定位算法精度提高28.9%.

3 構(gòu)建和優(yōu)化濾波器

由于UWB和GPS 融合定位系統(tǒng)為非線性系統(tǒng),非線性系統(tǒng)可采用擴展卡爾曼濾波或無跡卡爾曼濾波算法構(gòu)建融合定位系統(tǒng)濾波器.本文是在基于擴展卡爾曼濾波的UWB和GPS 融合定位算法基礎(chǔ)上進行優(yōu)化,采用無跡卡爾曼濾波的UWB和GPS 融合定位算法對此非線性系統(tǒng)進行濾波.

3.1 濾波器建模

構(gòu)建卡爾曼濾波定位算法前構(gòu)建系統(tǒng)和狀態(tài)方程.在目標載體所在的平面建立坐標系,設(shè)向東是x的正方向,向北是y的正方向.目標的坐標和速度作為狀態(tài)向量,假設(shè)系統(tǒng)過程噪聲和量測噪聲都服從均值為0的高斯分布,則設(shè)狀態(tài)向量為X=[xyx′y′]T,其中,x、y分別是目標載體的分坐標;x′、y′分別是目標載體的分速度.對采集到的數(shù)據(jù)進行離散化建立狀態(tài)方程如式(6)所示:

其中,F為狀態(tài)轉(zhuǎn)移矩陣,Γ為噪聲驅(qū)動矩陣,W(k)為k時刻的過程噪聲,其相應(yīng)的協(xié)方差矩陣為Q=diag[σ2wx′,σ2wy′].X(k)和X(k+1) 分別為k和k+1時刻的狀態(tài)向量,F、Γ、W(k)具體值如式(7)所示:

其中,T為采樣周期,wx(k)和wy(k)分別為x、y軸的噪聲分量.

以UWB 采集的目標載體坐標(x,y)和GPS 報文解析得的速度v和方位角θ 作為量測值,離散化數(shù)據(jù)建立量測方程如式(8)所示:

觀測方程的各項參數(shù)為如式(9)所示.

其中,Z(k)為k時刻的觀測向量,f[X(k)]為狀態(tài)向量與觀測向量的關(guān)系函數(shù),V(k)為觀測噪聲,其相應(yīng)的協(xié)方差矩陣為R.觀測向量V(k)中,坐標量測噪聲vx和vy分別服從均值為0,方差為σ2x和σ2y的高斯白噪聲;速度和方位角量測噪聲vv和vθ分別服從均值為0,方差為σ2v和σ2θ的高斯白噪聲,V(k) 對應(yīng)的協(xié)方差矩陣R=diag[σ2x,σ2y,σ2v,σ2θ].

為和觀測向量建立聯(lián)系,使用狀態(tài)向量x′(k)、y′(k) 非線性表示速度v(k)和方位角θ (k),其中的x′(k)、y′(k) 分別為狀態(tài)向量X(k)的速度分量.由于量測方程為非線性方程,使用擴展卡爾曼濾波需將f([X(k)])關(guān)于濾波值(k)進行一階Taylor 級數(shù)展開,如式(10).

系統(tǒng)狀態(tài)方程和量測方程建立后進行卡爾曼濾波,先對狀態(tài)向量的均值和協(xié)方差矩陣進行初始化.取狀態(tài)向量的初值為X(0)=[x(0)y(0)x′(0)y′(0)]T;協(xié)方差矩陣為4×4的單位矩陣I4;采樣周期T為1 s,按照卡爾曼濾波核心公式進行預(yù)測和更新.

狀態(tài)預(yù)測和狀態(tài)均方差預(yù)測分別如式(11)所示:

其中,根據(jù)k時刻的狀態(tài)向量(k|k) 預(yù)測k+1時刻的狀態(tài)向量(k+1|k),根據(jù)k時刻的均方差矩陣P(k|k)預(yù)測k+1時刻的均方差矩陣P(k+1|k),通過狀態(tài)轉(zhuǎn)移矩陣F、過程噪聲協(xié)方差矩陣Q和噪聲驅(qū)動矩陣Γ 等信息進行狀態(tài)一步預(yù)測和狀態(tài)一步均方差預(yù)測.

在量測更新階段,卡爾曼增益為:

狀態(tài)向量更新為:

均方差矩陣更新為:

式(1 2) 中,通過k時刻的預(yù)測均方差矩陣P(k+1|k)、量測矩陣H(k+1)和量測噪聲協(xié)方差矩陣R預(yù) 測k+1時刻卡爾曼增益K(k+1),K(k+1)表示預(yù)測均方差占量測誤差和預(yù)測均方差之和的比重.式(13)和式(14)表示利用k+1時刻卡爾曼增益K(k+1)和量測值Z(k+1),對均方差矩陣P(k+1|k+1)和濾波值(k+1|k+1) 進行更新.式(13)中f((k+1|k))表示狀態(tài)向量和量測向量的非線性函數(shù)關(guān)系.

3.2 優(yōu)化濾波器

由于定位算法運行于嵌入式芯片,算法復(fù)雜度過大將增大硬件資源開銷,算法復(fù)雜度將決定定位實時性等性能指標.擴展卡爾曼濾波將非線性系統(tǒng)函數(shù)進行一階Taylor 級數(shù)展開,線性化過程中拋棄高階項從而給系統(tǒng)帶來非線性誤差,且高階EKF 算法復(fù)雜度大大提高.若利用UKF 直接對樣本的后驗概率密度分布進行近似,算法復(fù)雜度得以優(yōu)化.針對提出的基于UKF的UWB和GPS 融合定位算法進行復(fù)雜度分析,并對比EKF 算法復(fù)雜度,得出算法復(fù)雜度優(yōu)化的依據(jù).

對算法浮點操作數(shù)(flops)的統(tǒng)計是分析算法復(fù)雜度的有效手段.一次flops 定義為兩個浮點數(shù)的加減乘除運算,Cholesky 分解、指數(shù)運算和開方等效為相同運行時間的flops.本文中基本的運算flops 次數(shù)為:若A∈Rn×m,B∈Rn×m,計算A±B需要進行nm次flops;若A∈Rn×m,B∈Rm×l,計算AB需要進行2mnl-nl次flops;若A∈Rn×n,計算A-1需要進行n3次flops;若A∈Rn×n,計算chol(A)需要進行次flops.下文中n為狀態(tài)維數(shù),l為量測維數(shù).

UKF 將UWB 模塊解算的載體坐標和GPS 報文解算的速度和方位角信息作為UWB和GPS的組合定位算法數(shù)據(jù)源,基于UKF的UWB和GPS 融合定位算法架構(gòu)如圖4所示.

圖4 基于UKF的UWB和GPS 融合定位算法架構(gòu)

假設(shè)目標載體在平面坐標系做勻速直線運動,定義在k時刻狀態(tài)向量為X(k)=[x(k)y(k)x′(k)y′(k)x′′(k)y′′(k)]T,其中包含目標在坐標系內(nèi)的坐標、速度和加速度,設(shè)定位系統(tǒng)運動過程受外界因素導(dǎo)致的誤差,即過程噪聲為W(k),采樣周期為T,Tw(k)為加速度引起的過程噪聲,為加速度積分引起的過程噪聲,為加速度二次積分引起的過程噪聲,無跡卡爾曼濾波定位算法系統(tǒng)方程如式(15)所示.

設(shè)定位系統(tǒng)狀態(tài)方程為X(k+1)=FX(k)+ΓW(k),則狀態(tài)轉(zhuǎn)移矩陣F和噪聲驅(qū)動矩陣Γ 如式(16) 所示,W(k)=[wx(k) wy(k)]T表示均值為0、協(xié)方差為的過程噪聲.

在基于無跡卡爾曼濾波的UWB和GPS 融合定位算法系統(tǒng)中,假設(shè)定位系統(tǒng)在k時刻量測值為Z(k),其中包含標簽相對于基站的真實坐標 (x,y)和GPS 定位系統(tǒng)確定的速度v和方位角θ,則離散化的系統(tǒng)的量測方程如式(17)所示:

其中,Z(k)為k時刻狀態(tài)向量的量測值,Z(k)=[x(k)y(k)v(k) θ(k)]T,V(k)為k時刻 狀態(tài)向量的量測值,V(k)=[vx(k)vy(k)vv(k)vθ(k)]T,坐標量測噪聲vx和vy分別服從(0,σ2x)和(0,σ2y)的高斯白噪聲;速度和方位角向量測噪聲vv和vθ分別服從(0,σ2v)和(0,σ2θ)的高斯白噪聲,量測噪聲V(k) 對應(yīng)的協(xié)方差矩陣R=diag[σ2x,σ2y,σ2v,σ2θ].

用上述確定的系統(tǒng)狀態(tài)方程和量測方程進行無跡卡爾曼濾波,選取目標載體的初始坐標和濾波初值如式(18)所示,濾波框架如式(19)所示.

計算增益K(k) 進行l(wèi)3+(2nl2-nl)次flops,計算狀態(tài)向量估計(k) 進行2nl+l次flops,計算協(xié)方差矩陣PX(k) 進行2nl2-nl+2n2l次flops.

設(shè)已知3 個UWB 基站BS.A、BS.B和BS.C 分別位于 (0,10)、(10,0)和(10,10)處,目標載 體的初始狀 態(tài)向量為X(0)=[0 0 0.2 0.2 0.05 0.05]T,狀態(tài)協(xié)方差矩陣P(0)=I6.非線性量測方程無跡變換,分布系數(shù)α=1,比例因子κ=0,均方差系數(shù)γ=α2(n+κ)-n,狀態(tài)向量維數(shù)n=6.狀態(tài)預(yù)測過程中,計算采樣點如式(20),狀態(tài)一步預(yù)測和均方差矩陣一步預(yù)測如式(21).

計算采樣點 χ (k-1) 進行次flops,計算狀態(tài)一步預(yù)測(k/k-1)和均方差矩陣一步預(yù)測PX(k/k-1) 分別進行2n2+2n次 flops和4n3+5n2+2n次flops.

量測預(yù)測過程中,采樣點更新如式(22)所示,量測均值更新、量測均方差矩陣更新和狀態(tài)量測協(xié)方差矩陣更新如式(23)所示.ηi為非線性變換后的狀態(tài)向量,Wim和Wic分別為采樣后均值和協(xié)方差的加權(quán)系數(shù).

計算量測樣本點 χ(k/k-1) 進行次flops,采樣點預(yù)測ηi(k/k-1) 進行4n2l-l次flops,量測均值更新(k/k-1)進行2nl+2l次flops、量測均方差矩陣更新PZ(k/k-1) 進行4nl2+2nl+3l2+2l次flops,狀態(tài)量測協(xié)方差矩陣更新PXZ(k/k-1) 進行4n2l+2n2+2nl+2n次flops.

4 實驗分析

為評估優(yōu)化后的UKF 算法復(fù)雜度帶來系統(tǒng)濾波實時性的提升,實驗將UKF 定位算法和EKF 算法分別寫入到實驗平臺的控制器,將濾波結(jié)果以日志文件的形式從串口輸出,根據(jù)日志中每幀濾波結(jié)果的時間戳,計算兩種算法濾波時間 Δt,驗證UKF 有比EKF 更好的實時性.如表1所示為UKF和EKF 濾波時間.由表2可知,EKF 平均濾波時間為63.3 ms,UKF 平均濾波時間為37.9 ms,表明基于UKF的UWB和GPS 融合定位算法由較高的實時性,效率更高.

表1 各組實驗數(shù)據(jù)和性能

表2 EKF和UKF 濾波時間 (ms)

為評估提出的基于無跡卡爾曼濾波的UWB和GPS 融合定位算法定位精度,實驗平臺車輛由UWB定位標簽、GPS 定位模塊以及GPS/IMU 組合定位模組構(gòu)成.采用GNW-MKS-4051-A 模塊作為GPS 傳感器單元,串口輸出數(shù)據(jù)經(jīng)STM32F103 芯片進行解析;DW1000 模塊作為UWB 傳感器單元,STM32F103 芯片對UWB 采集數(shù)據(jù)由上位機軟件解算速度和方位角并導(dǎo)出為Excel 格式.Matlab 構(gòu)建基于無跡卡爾曼濾波的UWB和GPS 融合定位算法,對解算進行無跡卡爾曼濾波,最終用圖像形式將濾波定位結(jié)果進行展示.

在10 m×10 m的正方形區(qū)域內(nèi),三基站BS.A、BS.B和BS.C 坐標分別為(0,10)、(10,0)和(10,10),小車分別沿y=x軸線和y=5 m軸線以進行勻速直線運動做兩組實驗,對兩組實驗的實驗采集數(shù)據(jù)進行分析.每組實驗分別包括基于EKF的UWB和GPS 融合定位系統(tǒng)、基于UKF的UWB和GPS 融合定位系統(tǒng)和GPS/IMU 組合定位系統(tǒng),圖5和圖6分別為兩組實驗.

圖5(a)表示第A 組實驗,目標載體沿y=5 m進行0.2 m/s的直線運動,定位采樣周期T=1 s,GPS/IMU組合定位系統(tǒng)采集的定位坐標振動明顯,平均誤差為84.46 cm;基于EKF的UWB和GPS 融合定位算法的定位坐標與真實值平均誤差為13.89 cm;基于UKF的UWB和GPS 融合定位算法平均誤差僅為10.11 cm,且無濾波發(fā)散現(xiàn)象.圖5(b)為實驗數(shù)據(jù)的局部細節(jié)圖.

圖5 A 組實驗各種定位算法軌跡實驗

為驗證在更大范圍的定位性能進行B 組實驗,在100 m×100 m的區(qū)域內(nèi)進行濾波定位實驗,如圖6所示,三基站BS.A、BS.B和BS.C 坐標分別為(0,100)、(100,0)和(100,100),目標載體沿y=x進行直線運動,定位采樣周期T=10 s.

圖6 實驗場地及布點

圖7(a)表示第B 組實驗數(shù)據(jù),GPS/IMU 組合定位系統(tǒng)定位坐標平均誤差在79.29 cm;基于EKF的UWB和GPS 融合定位算法的定位坐標與真實值平均誤差在21.86 cm;而基于UKF的UWB和GPS 融合定位算法平均誤差僅為18.39 cm,其定位精度仍能達到cm 級.圖7(b)為實驗數(shù)據(jù)的局部細節(jié)圖.

圖7 B 組實驗各種定位算法軌跡實驗

表2列出了A、B 兩組實驗的實驗數(shù)據(jù)和性能.

實驗結(jié)果表明,基于無跡卡爾曼濾波的UWB和GPS 融合定位算法相比傳統(tǒng)GPS/IMU 組合定位系統(tǒng),精度提高83.54%,定位誤差不隨時間變化;相較于基于擴展卡爾曼濾波的UWB和GPS 融合定位算法優(yōu)化效果明顯,精度提高23.05%,定位坐標與真實值擬合度更高,且無濾波發(fā)散現(xiàn)象;本算法在100 m 范圍平均誤差有所增大,但仍能達到cm 級精度.在保持cm 級精度下,極限定位距離約為150 m.

5 結(jié)論

本文針對當今汽車廣泛使用的GPS/IMU 組合定位系統(tǒng)存在時間積累誤差問題,提出了一種基于無跡卡爾曼濾波的UWB和GPS 融合定位算法.為降低系統(tǒng)噪聲的影響,使用加權(quán)最小二乘法對UWB 定位算法進行優(yōu)化;濾波過程中使用無跡卡爾曼濾波器對數(shù)據(jù)融合算法進行優(yōu)化.實驗將基于UKF的UWB和GPS 融合算法寫入到實驗平臺控制器,對濾波實時性和定位精度等性能指標進行分析,同時該算法與基于擴展卡爾曼濾波的UWB和GPS 融合定位算法以及傳統(tǒng)GPS/IMU 組合定位系統(tǒng)做性能對比.得出以下結(jié)論:

1)基于無跡卡爾曼濾波的UWB和GPS 融合定位算法相較于傳統(tǒng)的GPS/IMU 組合定位系統(tǒng)定位精度大幅提高,定位誤差不隨時間變化,系統(tǒng)在特定工況下容錯性佳,可實現(xiàn)cm 級精度定位;

2)基于無跡卡爾曼濾波的UWB和GPS 融合定位算法避免雅可比矩陣和海塞矩陣參與運算,算法復(fù)雜度大幅降低,可在嵌入式平臺進行實時性濾波,同時比EKF 算法定位精度提高23.05%,無濾波發(fā)散現(xiàn)象;

3)此外,加權(quán)最小二乘定位算法用于UWB 定位系統(tǒng),優(yōu)化定位系統(tǒng)的過程噪聲進而提高定位精度,同時削弱粗差數(shù)據(jù)影響、提高定位方程抗差性.