據(jù)的四種定位算法對比實現(xiàn)與解析)
手機上跑GNSS原始數(shù)據(jù)做定位這個事放在五年前還是件挺折騰的事現(xiàn)在門檻低了不少但真正把鏈路跑通、再把幾種算法放在一起對比出說服力強的結(jié)論依然有不少坑。這個項目做的就是典型的一條龍拿手機GNSS日志解析衛(wèi)星觀測值結(jié)合星歷自己算定位結(jié)果然后對比WLS、EKF、MHE、RTS四種算法在相同數(shù)據(jù)下的表現(xiàn)。我最初接這個活的時候心里想的是不就解個方程嘛真正動手才發(fā)現(xiàn)從日志格式的坑到濾波器發(fā)散的坑每一個都能讓人卡上小半天。這篇文章把我整個實現(xiàn)過程、參數(shù)整定思路、踩過的坑全部整理出來適合正在做GNSS數(shù)據(jù)處理課程設計、畢業(yè)設計或者想用低成本設備入門定位算法研究的朋友。你不需要買幾十萬的接收機一臺支持原始GNSS輸出的Android手機就夠了。1. 項目概覽從手機GNSS日志到定位結(jié)果的一整條鏈路1.1 這個項目到底在做什么展開說這個項目分四層第一層是數(shù)據(jù)獲取用Android手機上的GNSS Logger類應用記錄原始測量值包括偽距、載波相位、多普勒頻移、信號強度以及接收機時間戳。第二層是數(shù)據(jù)解析把日志文件喂給MATLAB腳本提取出每個歷元、每顆可見衛(wèi)星的觀測數(shù)據(jù)同時讀入廣播星歷計算衛(wèi)星在信號發(fā)射時刻的位置和鐘差。第三層是定位解算核心部分是四套算法WLS加權(quán)最小二乘、EKF擴展卡爾曼濾波、MHE模型預測估計、RTS平滑。它們各自輸出一條定位軌跡。第四層是對比評估把四條軌跡與參考真值比較統(tǒng)計水平誤差、垂直誤差、收斂時間、計算耗時等指標。這個項目最有價值的地方在于它把接收機-數(shù)據(jù)-算法-評估的完整鏈條都打通了不是那種只調(diào)現(xiàn)成庫函數(shù)的演示而是每個環(huán)節(jié)都能看到、能改、能復現(xiàn)的完整實現(xiàn)。1.2 為什么用MATLAB而不是Python或C我承認Python在GNSS領域現(xiàn)在很流行尤其是配合georinex、gnsspy這類庫解析RINEX確實方便。但這個項目我堅持用MATLAB有幾個現(xiàn)實原因第一MATLAB的矩陣運算和濾波器設計工具箱非常成熟。EKF、卡爾曼平滑這類遞推算法用矩陣表達幾乎是翻譯而非實現(xiàn)代碼可讀性極高后期調(diào)參非常順手。第二MATLAB的繪圖能力在論文和報告場景下無可替代。定位軌跡對比圖、誤差累積分布圖、衛(wèi)星天空圖幾行代碼就能輸出出版級質(zhì)量的圖。做算法對比研究圖就是臉面。第三MATLAB自帶一些GNSS相關的工具箱比如Navigation Toolbox里有衛(wèi)星位置計算、定位解算的參考函數(shù)。雖然我沒直接用但在驗證自己寫的星歷解析代碼是否正確時這些工具箱是不錯的對照基準。當然MATLAB的缺點也明顯最典型的就是循環(huán)效率。但在GNSS數(shù)據(jù)處理里歷元數(shù)一般也就幾千個MATLAB的向量化寫法足夠應付完全不會成為瓶頸。2. 數(shù)據(jù)鏈路構(gòu)建日志讀取與觀測值解析2.1 手機GNSS原始數(shù)據(jù)的獲取與格式要拿到原始觀測值手機必須支持Android 7.0及以上版本的原始GNSS測量接口Android 10以后支持得更完整。市面上大多數(shù)中高端手機都支持但有些廠商的固件會限制輸出。我在測試中主要用了兩臺設備一臺是Google Pixel系列一臺是某國產(chǎn)旗艦前者日志格式非常規(guī)范后者偶爾會出現(xiàn)偽距跳變建議有條件的話優(yōu)先用Pixel系或者原生Android系統(tǒng)的手機。采集軟件我用的是Google官方的GNSS Logger它可以直接輸出一個TXT格式的日志文件。這個文件里面有幾種類型的行#開頭的是注釋行記錄接收機型號、固件版本、AGPS輔助數(shù)據(jù)等信息。Raw開頭的行是原始測量值字段很多包括時間戳、衛(wèi)星號、載波頻率、偽距、偽距rate、載波相位、CN0信噪比等。Nav開頭的行是導航電文每行對應一顆衛(wèi)星的星歷參數(shù)。Meas開頭的行是GNSS測量匯總信息。值得說明的是GNSS Logger輸出的是偽距和載波相位已經(jīng)經(jīng)過Android框架處理的測量值不是I/Q采樣級別的原始中頻數(shù)據(jù)所以不需要自己處理信號捕獲跟蹤的問題。這對做定位算法研究反而是好事能把精力集中在更高層的處理上。如果你需要把這些日志轉(zhuǎn)成標準RINEX 3.03格式可以用GNSS Logger配套的轉(zhuǎn)換腳本也可以直接用我的MATLAB解析腳本一步到位。我選擇自己解析而不是轉(zhuǎn)RINEX原因是跳過一步轉(zhuǎn)換少一個可能出錯的環(huán)節(jié)而且MATLAB腳本可以直接對接后續(xù)處理省去文件格式來回折騰。2.2 關鍵解析細節(jié)偽距與載波相位的提取偽距觀測值的核心方程是ρ r c·δt_u - c·δt_s I T ε其中r是接收機到衛(wèi)星的幾何距離δt_u是接收機鐘差δt_s是衛(wèi)星鐘差I是電離層延遲T是對流層延遲。在單頻場景下電離層延遲只能用Klobuchar模型粗略改正如果手機支持雙頻L1L5就可以用無電離層組合消除一階電離層項。解析時最容易踩的坑是時間同步偽距對應的測量時刻是接收機接收到信號的時刻即接收機時間戳。計算衛(wèi)星位置時必須用信號的發(fā)射時刻也就是接收時刻減去偽距傳播時間。傳播時間約為偽距除以光速大約0.07秒左右。如果直接用接收時刻去查星歷算衛(wèi)星位置會產(chǎn)生幾十米的誤差這個在實現(xiàn)時必須嚴格處理。載波相位的使用門檻更高一些。手機上輸出的載波相位通常有整數(shù)周模糊度而且手機的天線相位中心和時鐘穩(wěn)定性遠不如測繪級接收機直接拿來做精密單點定位不現(xiàn)實我在項目里主要用它來做周跳探測和觀測值質(zhì)量評估輔助篩選數(shù)據(jù)不直接進定位方程。2.3 星歷文件與衛(wèi)星位置計算廣播星歷包含開普勒軌道參數(shù)和攝動改正項。每個參數(shù)的含義如下sqrtA軌道長半軸的平方根e軌道偏心率i0軌道傾角omega0升交點赤經(jīng)omega近地點幅角M0平近點角DeltaN平均運動角速度改正iDot、omegaDot傾角和升交點赤經(jīng)的變率Cuc, Cus, Crc, Crs, Cic, Cis諧波攝動改正系數(shù)toe星歷參考時刻衛(wèi)星位置計算的流程是標準的解Kepler方程得到偏近點角E M e·sin(E)迭代至收斂然后計算真近點角、升交角距加上攝動改正最后旋轉(zhuǎn)到地心地固坐標系。MATLAB里實現(xiàn)這段邏輯大概100多行核心就是查表寫方程。如果你手頭有Navigation Toolbox可以用gnssconstellation和ephemeris相關函數(shù)做交叉驗證。我第一次實現(xiàn)后隨機抽了幾顆GPS衛(wèi)星與在線SP3精密星歷對比位置誤差在10米量級這個量級對單點定位完全夠用。2.4 觀測值質(zhì)量篩查與預處理數(shù)據(jù)解析出來后不能直接拿去定位必須先做質(zhì)量篩查。我按以下順序處理一是高度角篩選。設置10度或15度的截止高度角。手機在城市峽谷環(huán)境下載波相位多徑嚴重低高度角衛(wèi)星觀測噪聲大且多徑誤差明顯篩掉是劃算的。二是信噪比篩選。CN0低于20dB-Hz的衛(wèi)星直接剔除30dB-Hz以上算健康。手機上信噪比數(shù)據(jù)經(jīng)常波動做滑動平均更穩(wěn)。三是偽距合理性檢查。計算偽距殘差如果某顆星的殘差超過100米大概率是周跳或者粗差剔除。四是衛(wèi)星幾何檢查。保留的衛(wèi)星少于4顆時直接放棄該歷元另外還計算DOP值HDOP大于5的歷元寧可丟點也不硬算。這一套預處理做完定位結(jié)果的穩(wěn)定性會有質(zhì)的提升。很多人算法寫了半天效果不好問題往往不在算法本身而在數(shù)據(jù)質(zhì)量。3. 四種定位算法原理拆解3.1 WLS加權(quán)最小二乘最直接的批處理定位WLS是GNSS單點定位的經(jīng)典解法。核心思想是在每個歷元用所有可見衛(wèi)星的偽距觀測值最小化加權(quán)殘差平方和。對于n顆衛(wèi)星、4個未知數(shù)三維位置加接收機鐘差的情況觀測方程線性化后可以寫成Δρ H·Δx其中H矩陣的每一行是接收機到衛(wèi)星方向的單位向量轉(zhuǎn)置最后一列為1。WLS解為Δx (H^T·W·H)^{-1}·H^T·W·Δρ權(quán)重矩陣W通常取偽距噪聲方差的倒數(shù)。我用的是基于高度角的權(quán)重模型σ2 a2 b2 / sin2(el)高度角el越低sin(el)越小噪聲方差越大權(quán)重越低。這是一種經(jīng)典的仰角加權(quán)模型簡單且實用。MATLAB實現(xiàn)時要注意迭代收斂判斷。因為觀測方程是線化后的第一次解算用的初始位置可能是地心或者上一次的粗略解需要迭代直到Δx的范數(shù)小于某個閾值一般1毫米或者位置變化小于0.001米就收斂了。我實測下來冷啟動時通常3到5次迭代就能收斂。WLS的優(yōu)點是簡單、穩(wěn)定、沒有狀態(tài)模型假設每歷元獨立解算誤差不累積。缺點也很明顯沒有利用載體運動的物理規(guī)律單歷元解在衛(wèi)星幾何差或者多徑嚴重時跳變很厲害。3.2 EKF擴展卡爾曼濾波遞推式動態(tài)定位EKF的思路是把定位問題建模為標準的狀態(tài)估計問題。狀態(tài)向量取x [X, Y, Z, Vx, Vy, Vz, cdt_u, cdt_dot]^T也就是三維位置、三維速度、接收機鐘差、鐘差變化率。有的實現(xiàn)不帶速度狀態(tài)位置直接用隨機游走模型也可以但帶上速度狀態(tài)在中低速場景下會更平滑。狀態(tài)方程是最簡單的常速模型CV過程噪聲主要來源于載體機動加速度。測量方程是偽距的完整觀測模型和WLS里的觀測方程一致。EKF的遞推分兩步時間更新用狀態(tài)轉(zhuǎn)移矩陣預測狀態(tài)和協(xié)方差。預測協(xié)方差的公式是P_pred F·P·F^T QQ是過程噪聲協(xié)方差矩陣反映你對運動模型不確定性的估計。測量更新計算卡爾曼增益K P_pred·H^T·(H·P_pred·H^T R)^{-1}然后更新狀態(tài)和協(xié)方差。MATLAB里實現(xiàn)EKF最怕的是協(xié)方差矩陣不正定。原因通常是數(shù)值精度問題或者Q、R矩陣設置不當。我實際測試的結(jié)果是在手機這種觀測噪聲較大的場景下EKF的精度相比WLS提升有限但軌跡平滑度明顯更好速度輸出也很穩(wěn)適合運動狀態(tài)分析。還有一個關鍵點在濾波初值。EKF對初始位置偏差非常敏感。我的做法是先跑2秒WLS用WLS的結(jié)果作為EKF的初始狀態(tài)這樣能避免濾波初期發(fā)散。3.3 MHE模型預測估計有限時域優(yōu)化MHE全稱Moving Horizon Estimation它的核心思想是把狀態(tài)估計表述為固定窗口內(nèi)的優(yōu)化問題。每個時刻只使用最近N個歷元的觀測數(shù)據(jù)估計窗口內(nèi)的初始狀態(tài)和噪聲序列取窗口末尾的狀態(tài)作為當前估計。數(shù)學上MHE在每步需要求解一個帶約束的最小二乘問題minimize Σ_{k0}^{N-1} ||v_k||2_{R^{-1}} ||w_k||2_{Q^{-1}} ||x? - x??||2_{P??^{-1}}其中v_k是測量噪聲w_k是過程噪聲x??和P??是先驗信息。在GNSS定位里約束條件可以包括載體速度的上下限等物理約束這是MHE相比EKF的優(yōu)勢——它能顯式處理狀態(tài)約束。但MHE的計算開銷很可觀。窗口長度N越大優(yōu)化問題的規(guī)模越大。我在實現(xiàn)時窗口取10個歷元用MATLAB的fmincon求解單步耗時約50到100毫秒實時性勉強能達到1Hz的實時處理要求但明顯比EKF慢得多。MHE的位置精度與窗口大小正相關窗口長一些、數(shù)據(jù)質(zhì)量好的時候精度比EKF略好尤其是在發(fā)生短時信號遮擋時MHE因為有歷史窗口約束不容易像EKF那樣被單個壞歷元拉偏。但如果窗口內(nèi)存在較多粗差MHE同樣會受影響。3.4 RTS平滑離線后處理精度上限RTS全稱Rauch-Tung-Striebel平滑器。它的思路是先用標準卡爾曼濾波正向跑一遍記錄每一時刻的狀態(tài)估計和協(xié)方差然后從末尾往前做反向平滑。反向平滑的核心公式是x_s,k x_k K_s,k·(x_s,k1 - F_k·x_k)其中K_s,k是平滑增益是協(xié)方差矩陣的函數(shù)。RTS的意義在于它同時利用了t時刻之前和之后的所有觀測信息理論上在所有線性高斯估計器中精度是最優(yōu)的。在GNSS場景下RTS把WLS/EKF那種只用過去數(shù)據(jù)的限制打破了。城市環(huán)境下衛(wèi)星信號經(jīng)常被遮擋幾分鐘遮擋期間的定位誤差很大但RTS在信號恢復后能回頭修正遮擋時段的位置估計。在靜態(tài)或低速場景RTS的精度可以比EKF提升30%以上。這個方案的代價是只能在事后處理且需要保存整個時段的濾波器狀態(tài)存儲量不小。手機上采集一小時數(shù)據(jù)EKF正向和RTS反向各跑一遍總的處理時間大約幾秒鐘完全可接受。3.5 四種算法特性橫向?qū)Ρ任野阉奶姿惴ǖ暮诵奶匦宰隽艘粡垖Ρ缺硭惴ㄓ嬎銖碗s度實時性相對精度抗粗差能力利用未來數(shù)據(jù)適用場景WLS低高基準弱否實時單點定位EKF低高略優(yōu)于WLS中否實時動態(tài)定位MHE高中優(yōu)于EKF中強部分(窗口內(nèi))有約束的實時估計RTS中低(事后)最優(yōu)中是全部高精度后處理這里的相對精度結(jié)論是基于我的實測數(shù)據(jù)不是絕對結(jié)論。不同數(shù)據(jù)質(zhì)量、不同場景排序可能會有變化但RTS在最末位平滑后精度最高這個結(jié)論在絕大多數(shù)場景都成立。4. 核心實現(xiàn)細節(jié)與參數(shù)調(diào)優(yōu)4.1 坐標系統(tǒng)轉(zhuǎn)換ECEF、LLA與ENU定位解算輸出的坐標是地心地固系ECEF下的XYZ但要評估誤差或者畫軌跡圖最好轉(zhuǎn)成經(jīng)緯高LLA再投影到以參考點為原點的東北天坐標系ENU。我最常用的轉(zhuǎn)換流程是ECEF轉(zhuǎn)LLA用標準的迭代公式或者MATLAB的ecef2lla函數(shù)DDM格式的經(jīng)緯度注意不要混結(jié)算符ENU坐標系以真值軌跡的起點或者某個參考站的坐標為原點這樣誤差分析能直觀看出水平方向和垂直方向的偏差。這里有個容易搞混的點手機GNSS輸出的是WGS84坐標系的坐標而地圖應用通常用的也是WGS84但如果你做的是國內(nèi)的高精度定位評估可能需要了解CGCS2000與WGS84的差異。前者和后者在大多數(shù)民用場景下差異在厘米級對單點定位精度評估來說可以忽略但如果做高程方向的精密分析還是要注意參考框架。4.2 權(quán)重矩陣的構(gòu)造策略WLS和EKF的測量噪聲矩陣R的構(gòu)造直接影響定位精度這是我調(diào)參過程中感受最深的地方。第一版腳本我用了等權(quán)模型所有衛(wèi)星的偽距噪聲方差都取1米2結(jié)果在城市環(huán)境下定位誤差很大尤其是低高度角衛(wèi)星的觀測噪聲被低估了導致結(jié)果被帶偏。第二版改成基于高度角的模型效果好了很多但依然存在個別衛(wèi)星異常拉偏的情況。第三版在高度角基礎上加入信噪比校正CN0越高權(quán)重越大。模型形式是σ2 a2 b2/sin2(el) × (CN0_ref/CN0)^2。這個經(jīng)驗模型的物理含義是信噪比直接反映接收信號的質(zhì)量低信噪比觀測值往往伴隨顯著的多徑誤差壓低權(quán)重是非常合理的處理。對于手機上經(jīng)常出現(xiàn)的異常偽距跳變我還在測量更新前加了一步殘差卡方檢驗。計算每個歷元的標準化殘差如果某顆星的殘差超過3倍標準差就把該衛(wèi)星的測量噪聲方差乘以一個大因子等效于降低權(quán)重而不是直接粗暴剔除。這樣處理保留了衛(wèi)星的幾何貢獻同時抑制了粗差影響。4.3 EKF的過程噪聲和測量噪聲整定EKF里Q矩陣和R矩陣的比值決定了濾波器的響應速度和穩(wěn)定性這是整定過程中最核心的平衡。Q矩陣物理含義是你對運動模型的信任程度Q越大代表你越相信測量值、越不信運動模型濾波器響應越快但越不平滑容易受噪聲影響Q越小則相反軌跡越平滑但對真實機動響應越慢。我的初始設置參考了經(jīng)驗值位置過程噪聲取0.1 m/√s量級對應的方差速度過程噪聲取0.5 (m/s)/√s鐘差過程噪聲取10 m/√s對應的方差鐘差變化率取1 (m/s)/√s。然后用前2分鐘的靜態(tài)數(shù)據(jù)做靈敏度分析把位置過程噪聲從小到大掃一遍找到水平誤差最小的量級。如果車載運動三軸加速度突變大位置過程噪聲需要上調(diào)一個數(shù)量級否則轉(zhuǎn)彎時濾波會明顯滯后于真實軌跡。手機步行場景則相對溫和可以維持較小值。還有一個容易被忽視的細節(jié)是R矩陣的時間一致性。偽距噪聲方差和接收機跟蹤環(huán)路帶寬有關手機與專業(yè)接收機不同室內(nèi)外切換時偽距噪聲特性會突然變化建議在R矩陣里加一個基于CN0的時變因子。4.4 性能評估方法算法對比不能只看軌跡看起來對不對要有量化指標。我從三個維度評估水平誤差2D位置誤差的均方根RMS和95%分位誤差。定義是ENU坐標系下北向和東向分量的平面距離誤差。這是民用導航最重要的指標。垂直誤差的RMS。手機單頻偽距定位的垂直誤差普遍比水平誤差大1.5到2倍這主要是衛(wèi)星幾何特性決定的。我實測WLS的垂直誤差RMS約8到12米水平誤差RMS約5到8米參考真值是手機自帶的加速度計行人航位推算結(jié)果。計算耗時。用MATLAB的tic/toc統(tǒng)計每個歷元的平均處理時間在需要強調(diào)實時性的場景這個指標很重要。另外我還畫了CDF誤差累積分布圖和逐歷元時間序列誤差曲線。CDF圖能直觀看出90%的時間誤差落在什么范圍內(nèi)比單看RMS更有說服力。關于參考真值這是個關鍵問題。手機定位實驗很難獲得厘米級的真值我用了三種參考來源靜態(tài)場景直接用已知坐標點動態(tài)場景用手機自帶的GNSS融合定位輸出做參考雖然有誤差但量級遠小于單頻偽距定位作為對比基準可以接受如果有條件可以在開闊場地用RTK接收機同步觀測得到真正的高精度參考軌跡。5. 實測結(jié)果與避坑實錄5.1 靜態(tài)場景下四種算法的表現(xiàn)在樓頂開闊環(huán)境做了15分鐘的靜態(tài)測試天線位置固定手機平放視野內(nèi)可見衛(wèi)星數(shù)平均10顆左右GPS北斗Galileo聯(lián)合定位。從這個場景拿到的數(shù)據(jù)顯示W(wǎng)LS水平誤差RMS約4.6米95%誤差9.2米。由于每歷元獨立解算位置序列呈點狀分布能明顯看到多徑帶來的隨機跳變。EKF水平誤差RMS約4.1米95%誤差7.8米。軌跡平滑不少但靜態(tài)下仍存在低頻漂移這主要來自偽距多徑的慢變分量。MHE窗口N10時水平誤差RMS約3.8米95%誤差6.5米略微優(yōu)于EKF。RTS水平誤差RMS約2.9米95%誤差5.4米。平滑后靜態(tài)精度提升最明顯低頻漂移被有效抑制。需要提醒的是這些數(shù)據(jù)只是單一場景的一次測試不代表算法在所有場景下的絕對水平。做對比研究時至少要在開闊地、半遮擋環(huán)境、城市峽谷各做一組綜合看結(jié)論才靠譜。5.2 動態(tài)步行場景的對比拿著手機沿校園道路正常步行約600米途經(jīng)一段兩側(cè)有建筑的半遮擋區(qū)域。速度約1.2m/s。動態(tài)場景下WLS單點解跳得非常厲害半遮擋區(qū)域甚至出現(xiàn)了連續(xù)多個歷元定位結(jié)果偏離真實路徑超過30米的情況。EKF因為有速度狀態(tài)約束偏離幅度被壓到10米內(nèi)軌跡連續(xù)但出現(xiàn)過沖。MHE在遮擋區(qū)域的軌跡最穩(wěn)定因為窗口內(nèi)的歷史信息形成了約束。RTS是整個時段處理完后輸出在遮擋段落的軌跡被后續(xù)信號恢復后的信息拉回視覺效果明顯更好實際誤差也最小。動態(tài)和靜態(tài)的結(jié)論一致RTS MHE ≥ EKF WLS精確的排序依數(shù)據(jù)質(zhì)量略有浮動。5.3 常見問題與排查技巧速查表我把這個項目里實際遇到的高頻問題整理成了一張速查表現(xiàn)象可能原因排查與解決解析日志后衛(wèi)星數(shù)為0日志時間戳格式不匹配檢查GNSS Logger輸出的是GPS周秒還是Unix時間先統(tǒng)一時間基準偽距全部為負值或異常大未按發(fā)射時刻計算衛(wèi)星位置衛(wèi)星鐘差未修正核實偽距對應的時間標簽補上衛(wèi)星鐘差改正項定位結(jié)果偏向某個方向幾十米電離層/對流層改正未加或符號錯誤單頻場景至少加Klobuchar電離層改正檢查對流層改正項符號WLS迭代不收斂初始位置距離真值太遠或衛(wèi)星幾何退化先用粗略位置如手機網(wǎng)絡定位做初值少于4顆星時放棄該歷元EKF軌跡在信號遮擋后偏移很大過程噪聲Q設置過小濾波器太相信運動模型適當增大Q或在遮擋期間提高位置先驗噪聲濾波協(xié)方差矩陣非正定數(shù)值精度問題或Q/R設置不合理改用平方根濾波或約瑟夫形式檢查矩陣對稱性同一歷元雙重定位結(jié)果差很多衛(wèi)星觀測值粗差未剔除加強殘差卡門檢驗對超3倍標準差的觀測值降權(quán)RTS結(jié)果比EKF還差平滑實現(xiàn)中矩陣求逆數(shù)值問題或狀態(tài)轉(zhuǎn)移矩陣時間步長不匹配檢查平滑增益公式確認狀態(tài)轉(zhuǎn)移矩陣用的是均勻時間間隔必要時使用穩(wěn)定化的矩陣求逆方法5.4 幾個值得一說的實現(xiàn)技巧MATLAB循環(huán)效率不高解析日志文件時逐行處理幾萬行數(shù)據(jù)會有點慢。我一開始用的是fgetl循環(huán)300MB的日志要跑好幾秒。后來改成文本擦操作一次讀入后用strsplit和cellfun做向量化處理速度提升了一個量級。解析GNSS日志這類文本處理工作盡量用MATLAB的文本批處理函數(shù)別一行行讀。另一個技巧是繪圖。EKF和MHE的位置輸出有協(xié)方差信息可以畫誤差橢圓。我在軌跡圖上每隔固定歷元畫一個95%誤差橢圓能直觀看出定位置信度變化。衛(wèi)星信號遮擋時誤差橢圓明顯變大這種可視化效果在報告里非常加分。還有一個容易被忽略的問題手機GNSS日志中的GNSS接收機時鐘受鐘差和漂移影響明顯幾十分鐘內(nèi)可能漂移數(shù)十毫秒。當間隔較長時間再處理時務必要考慮鐘差的狀態(tài)轉(zhuǎn)移模型是否能捕捉這種漂移否則濾波會出現(xiàn)系統(tǒng)性偏差。EKF必須把鐘差變化率納入狀態(tài)向量WLS則不需要擔心因為每歷元重新估計鐘差。6. 擴展方向與個人經(jīng)驗做到這一步四種算法的對比已經(jīng)完整了但這個項目的可擴展性很強給你幾個我覺得很有價值的方向增加RTK或者PPP處理模塊。雖然手機上行RTK的整數(shù)模糊度固定很難但利用雙頻觀測值做PPP精度可以大幅提升。加入慣導數(shù)據(jù)。手機日志里通常同時有加速度計和陀螺儀數(shù)據(jù)和GNSS做松組合在城市峽谷場景下的連續(xù)性和精度都會上一個臺階。數(shù)據(jù)驅(qū)動的方式優(yōu)化噪聲參數(shù)。我在調(diào)參時做了靈敏度分析能不能做個自動化的網(wǎng)格搜索或Bayesian優(yōu)化把Q/R參數(shù)自動整定出來效果大概率優(yōu)于手工調(diào)參。把算法封裝成可配置的框架。目前四套算法各自獨立你可以設計一個統(tǒng)一的接口讓新算法比如粒子濾波、無跡卡爾曼濾波插進來就能跑對比實驗這樣后續(xù)做研究寫論文非常方便。最后分享一點個人體會。這個項目做完我最大的感觸不是哪個算法精度最高而是數(shù)據(jù)質(zhì)量決定上限算法決定逼近上限的程度。我在實驗里用同一套數(shù)據(jù)反復測試結(jié)果差異最大的不是WLS和RTS之間的算法差距而是數(shù)據(jù)預處理做得好不好的差距。把觀測值質(zhì)量篩查、時間對齊、衛(wèi)星位置計算這些基礎細節(jié)做到位哪怕只用WLS也能有不錯的效果反過來再先進的濾波算法喂進去一堆粗差數(shù)據(jù)輸出的也只能是看起來很平滑的錯誤軌跡。做GNSS定位研究別急著上復雜算法先把數(shù)據(jù)鏈路搞扎實再談算法創(chuàng)新。這個項目的價值也正在于此——它逼著你走完整個鏈路每一步都能看到數(shù)據(jù)是怎么流動、怎么被處理的。如果你也在做類似的事情建議一定保持這樣的習慣每處理完一個環(huán)節(jié)就把中間結(jié)果用圖畫出來看著數(shù)據(jù)走一遍遠比悶頭調(diào)算法參數(shù)要有效得多。 基于手機GNSS原始數(shù)據(jù)的四種定位算法對比實現(xiàn)與解析
手機上跑GNSS原始數(shù)據(jù)做定位這個事放在五年前還是件挺折騰的事現(xiàn)在門檻低了不少但真正把鏈路跑通、再把幾種算法放在一起對比出說服力強的結(jié)論依然有不少坑。這個項目做的就是典型的一條龍拿手機GNSS日志解析衛(wèi)星觀測值結(jié)合星歷自己算定位結(jié)果然后對比WLS、EKF、MHE、RTS四種算法在相同數(shù)據(jù)下的表現(xiàn)。我最初接這個活的時候心里想的是不就解個方程嘛真正動手才發(fā)現(xiàn)從日志格式的坑到濾波器發(fā)散的坑每一個都能讓人卡上小半天。這篇文章把我整個實現(xiàn)過程、參數(shù)整定思路、踩過的坑全部整理出來適合正在做GNSS數(shù)據(jù)處理課程設計、畢業(yè)設計或者想用低成本設備入門定位算法研究的朋友。你不需要買幾十萬的接收機一臺支持原始GNSS輸出的Android手機就夠了。1. 項目概覽從手機GNSS日志到定位結(jié)果的一整條鏈路1.1 這個項目到底在做什么展開說這個項目分四層第一層是數(shù)據(jù)獲取用Android手機上的GNSS Logger類應用記錄原始測量值包括偽距、載波相位、多普勒頻移、信號強度以及接收機時間戳。第二層是數(shù)據(jù)解析把日志文件喂給MATLAB腳本提取出每個歷元、每顆可見衛(wèi)星的觀測數(shù)據(jù)同時讀入廣播星歷計算衛(wèi)星在信號發(fā)射時刻的位置和鐘差。第三層是定位解算核心部分是四套算法WLS加權(quán)最小二乘、EKF擴展卡爾曼濾波、MHE模型預測估計、RTS平滑。它們各自輸出一條定位軌跡。第四層是對比評估把四條軌跡與參考真值比較統(tǒng)計水平誤差、垂直誤差、收斂時間、計算耗時等指標。這個項目最有價值的地方在于它把接收機-數(shù)據(jù)-算法-評估的完整鏈條都打通了不是那種只調(diào)現(xiàn)成庫函數(shù)的演示而是每個環(huán)節(jié)都能看到、能改、能復現(xiàn)的完整實現(xiàn)。1.2 為什么用MATLAB而不是Python或C我承認Python在GNSS領域現(xiàn)在很流行尤其是配合georinex、gnsspy這類庫解析RINEX確實方便。但這個項目我堅持用MATLAB有幾個現(xiàn)實原因第一MATLAB的矩陣運算和濾波器設計工具箱非常成熟。EKF、卡爾曼平滑這類遞推算法用矩陣表達幾乎是翻譯而非實現(xiàn)代碼可讀性極高后期調(diào)參非常順手。第二MATLAB的繪圖能力在論文和報告場景下無可替代。定位軌跡對比圖、誤差累積分布圖、衛(wèi)星天空圖幾行代碼就能輸出出版級質(zhì)量的圖。做算法對比研究圖就是臉面。第三MATLAB自帶一些GNSS相關的工具箱比如Navigation Toolbox里有衛(wèi)星位置計算、定位解算的參考函數(shù)。雖然我沒直接用但在驗證自己寫的星歷解析代碼是否正確時這些工具箱是不錯的對照基準。當然MATLAB的缺點也明顯最典型的就是循環(huán)效率。但在GNSS數(shù)據(jù)處理里歷元數(shù)一般也就幾千個MATLAB的向量化寫法足夠應付完全不會成為瓶頸。2. 數(shù)據(jù)鏈路構(gòu)建日志讀取與觀測值解析2.1 手機GNSS原始數(shù)據(jù)的獲取與格式要拿到原始觀測值手機必須支持Android 7.0及以上版本的原始GNSS測量接口Android 10以后支持得更完整。市面上大多數(shù)中高端手機都支持但有些廠商的固件會限制輸出。我在測試中主要用了兩臺設備一臺是Google Pixel系列一臺是某國產(chǎn)旗艦前者日志格式非常規(guī)范后者偶爾會出現(xiàn)偽距跳變建議有條件的話優(yōu)先用Pixel系或者原生Android系統(tǒng)的手機。采集軟件我用的是Google官方的GNSS Logger它可以直接輸出一個TXT格式的日志文件。這個文件里面有幾種類型的行#開頭的是注釋行記錄接收機型號、固件版本、AGPS輔助數(shù)據(jù)等信息。Raw開頭的行是原始測量值字段很多包括時間戳、衛(wèi)星號、載波頻率、偽距、偽距rate、載波相位、CN0信噪比等。Nav開頭的行是導航電文每行對應一顆衛(wèi)星的星歷參數(shù)。Meas開頭的行是GNSS測量匯總信息。值得說明的是GNSS Logger輸出的是偽距和載波相位已經(jīng)經(jīng)過Android框架處理的測量值不是I/Q采樣級別的原始中頻數(shù)據(jù)所以不需要自己處理信號捕獲跟蹤的問題。這對做定位算法研究反而是好事能把精力集中在更高層的處理上。如果你需要把這些日志轉(zhuǎn)成標準RINEX 3.03格式可以用GNSS Logger配套的轉(zhuǎn)換腳本也可以直接用我的MATLAB解析腳本一步到位。我選擇自己解析而不是轉(zhuǎn)RINEX原因是跳過一步轉(zhuǎn)換少一個可能出錯的環(huán)節(jié)而且MATLAB腳本可以直接對接后續(xù)處理省去文件格式來回折騰。2.2 關鍵解析細節(jié)偽距與載波相位的提取偽距觀測值的核心方程是ρ r c·δt_u - c·δt_s I T ε其中r是接收機到衛(wèi)星的幾何距離δt_u是接收機鐘差δt_s是衛(wèi)星鐘差I是電離層延遲T是對流層延遲。在單頻場景下電離層延遲只能用Klobuchar模型粗略改正如果手機支持雙頻L1L5就可以用無電離層組合消除一階電離層項。解析時最容易踩的坑是時間同步偽距對應的測量時刻是接收機接收到信號的時刻即接收機時間戳。計算衛(wèi)星位置時必須用信號的發(fā)射時刻也就是接收時刻減去偽距傳播時間。傳播時間約為偽距除以光速大約0.07秒左右。如果直接用接收時刻去查星歷算衛(wèi)星位置會產(chǎn)生幾十米的誤差這個在實現(xiàn)時必須嚴格處理。載波相位的使用門檻更高一些。手機上輸出的載波相位通常有整數(shù)周模糊度而且手機的天線相位中心和時鐘穩(wěn)定性遠不如測繪級接收機直接拿來做精密單點定位不現(xiàn)實我在項目里主要用它來做周跳探測和觀測值質(zhì)量評估輔助篩選數(shù)據(jù)不直接進定位方程。2.3 星歷文件與衛(wèi)星位置計算廣播星歷包含開普勒軌道參數(shù)和攝動改正項。每個參數(shù)的含義如下sqrtA軌道長半軸的平方根e軌道偏心率i0軌道傾角omega0升交點赤經(jīng)omega近地點幅角M0平近點角DeltaN平均運動角速度改正iDot、omegaDot傾角和升交點赤經(jīng)的變率Cuc, Cus, Crc, Crs, Cic, Cis諧波攝動改正系數(shù)toe星歷參考時刻衛(wèi)星位置計算的流程是標準的解Kepler方程得到偏近點角E M e·sin(E)迭代至收斂然后計算真近點角、升交角距加上攝動改正最后旋轉(zhuǎn)到地心地固坐標系。MATLAB里實現(xiàn)這段邏輯大概100多行核心就是查表寫方程。如果你手頭有Navigation Toolbox可以用gnssconstellation和ephemeris相關函數(shù)做交叉驗證。我第一次實現(xiàn)后隨機抽了幾顆GPS衛(wèi)星與在線SP3精密星歷對比位置誤差在10米量級這個量級對單點定位完全夠用。2.4 觀測值質(zhì)量篩查與預處理數(shù)據(jù)解析出來后不能直接拿去定位必須先做質(zhì)量篩查。我按以下順序處理一是高度角篩選。設置10度或15度的截止高度角。手機在城市峽谷環(huán)境下載波相位多徑嚴重低高度角衛(wèi)星觀測噪聲大且多徑誤差明顯篩掉是劃算的。二是信噪比篩選。CN0低于20dB-Hz的衛(wèi)星直接剔除30dB-Hz以上算健康。手機上信噪比數(shù)據(jù)經(jīng)常波動做滑動平均更穩(wěn)。三是偽距合理性檢查。計算偽距殘差如果某顆星的殘差超過100米大概率是周跳或者粗差剔除。四是衛(wèi)星幾何檢查。保留的衛(wèi)星少于4顆時直接放棄該歷元另外還計算DOP值HDOP大于5的歷元寧可丟點也不硬算。這一套預處理做完定位結(jié)果的穩(wěn)定性會有質(zhì)的提升。很多人算法寫了半天效果不好問題往往不在算法本身而在數(shù)據(jù)質(zhì)量。3. 四種定位算法原理拆解3.1 WLS加權(quán)最小二乘最直接的批處理定位WLS是GNSS單點定位的經(jīng)典解法。核心思想是在每個歷元用所有可見衛(wèi)星的偽距觀測值最小化加權(quán)殘差平方和。對于n顆衛(wèi)星、4個未知數(shù)三維位置加接收機鐘差的情況觀測方程線性化后可以寫成Δρ H·Δx其中H矩陣的每一行是接收機到衛(wèi)星方向的單位向量轉(zhuǎn)置最后一列為1。WLS解為Δx (H^T·W·H)^{-1}·H^T·W·Δρ權(quán)重矩陣W通常取偽距噪聲方差的倒數(shù)。我用的是基于高度角的權(quán)重模型σ2 a2 b2 / sin2(el)高度角el越低sin(el)越小噪聲方差越大權(quán)重越低。這是一種經(jīng)典的仰角加權(quán)模型簡單且實用。MATLAB實現(xiàn)時要注意迭代收斂判斷。因為觀測方程是線化后的第一次解算用的初始位置可能是地心或者上一次的粗略解需要迭代直到Δx的范數(shù)小于某個閾值一般1毫米或者位置變化小于0.001米就收斂了。我實測下來冷啟動時通常3到5次迭代就能收斂。WLS的優(yōu)點是簡單、穩(wěn)定、沒有狀態(tài)模型假設每歷元獨立解算誤差不累積。缺點也很明顯沒有利用載體運動的物理規(guī)律單歷元解在衛(wèi)星幾何差或者多徑嚴重時跳變很厲害。3.2 EKF擴展卡爾曼濾波遞推式動態(tài)定位EKF的思路是把定位問題建模為標準的狀態(tài)估計問題。狀態(tài)向量取x [X, Y, Z, Vx, Vy, Vz, cdt_u, cdt_dot]^T也就是三維位置、三維速度、接收機鐘差、鐘差變化率。有的實現(xiàn)不帶速度狀態(tài)位置直接用隨機游走模型也可以但帶上速度狀態(tài)在中低速場景下會更平滑。狀態(tài)方程是最簡單的常速模型CV過程噪聲主要來源于載體機動加速度。測量方程是偽距的完整觀測模型和WLS里的觀測方程一致。EKF的遞推分兩步時間更新用狀態(tài)轉(zhuǎn)移矩陣預測狀態(tài)和協(xié)方差。預測協(xié)方差的公式是P_pred F·P·F^T QQ是過程噪聲協(xié)方差矩陣反映你對運動模型不確定性的估計。測量更新計算卡爾曼增益K P_pred·H^T·(H·P_pred·H^T R)^{-1}然后更新狀態(tài)和協(xié)方差。MATLAB里實現(xiàn)EKF最怕的是協(xié)方差矩陣不正定。原因通常是數(shù)值精度問題或者Q、R矩陣設置不當。我實際測試的結(jié)果是在手機這種觀測噪聲較大的場景下EKF的精度相比WLS提升有限但軌跡平滑度明顯更好速度輸出也很穩(wěn)適合運動狀態(tài)分析。還有一個關鍵點在濾波初值。EKF對初始位置偏差非常敏感。我的做法是先跑2秒WLS用WLS的結(jié)果作為EKF的初始狀態(tài)這樣能避免濾波初期發(fā)散。3.3 MHE模型預測估計有限時域優(yōu)化MHE全稱Moving Horizon Estimation它的核心思想是把狀態(tài)估計表述為固定窗口內(nèi)的優(yōu)化問題。每個時刻只使用最近N個歷元的觀測數(shù)據(jù)估計窗口內(nèi)的初始狀態(tài)和噪聲序列取窗口末尾的狀態(tài)作為當前估計。數(shù)學上MHE在每步需要求解一個帶約束的最小二乘問題minimize Σ_{k0}^{N-1} ||v_k||2_{R^{-1}} ||w_k||2_{Q^{-1}} ||x? - x??||2_{P??^{-1}}其中v_k是測量噪聲w_k是過程噪聲x??和P??是先驗信息。在GNSS定位里約束條件可以包括載體速度的上下限等物理約束這是MHE相比EKF的優(yōu)勢——它能顯式處理狀態(tài)約束。但MHE的計算開銷很可觀。窗口長度N越大優(yōu)化問題的規(guī)模越大。我在實現(xiàn)時窗口取10個歷元用MATLAB的fmincon求解單步耗時約50到100毫秒實時性勉強能達到1Hz的實時處理要求但明顯比EKF慢得多。MHE的位置精度與窗口大小正相關窗口長一些、數(shù)據(jù)質(zhì)量好的時候精度比EKF略好尤其是在發(fā)生短時信號遮擋時MHE因為有歷史窗口約束不容易像EKF那樣被單個壞歷元拉偏。但如果窗口內(nèi)存在較多粗差MHE同樣會受影響。3.4 RTS平滑離線后處理精度上限RTS全稱Rauch-Tung-Striebel平滑器。它的思路是先用標準卡爾曼濾波正向跑一遍記錄每一時刻的狀態(tài)估計和協(xié)方差然后從末尾往前做反向平滑。反向平滑的核心公式是x_s,k x_k K_s,k·(x_s,k1 - F_k·x_k)其中K_s,k是平滑增益是協(xié)方差矩陣的函數(shù)。RTS的意義在于它同時利用了t時刻之前和之后的所有觀測信息理論上在所有線性高斯估計器中精度是最優(yōu)的。在GNSS場景下RTS把WLS/EKF那種只用過去數(shù)據(jù)的限制打破了。城市環(huán)境下衛(wèi)星信號經(jīng)常被遮擋幾分鐘遮擋期間的定位誤差很大但RTS在信號恢復后能回頭修正遮擋時段的位置估計。在靜態(tài)或低速場景RTS的精度可以比EKF提升30%以上。這個方案的代價是只能在事后處理且需要保存整個時段的濾波器狀態(tài)存儲量不小。手機上采集一小時數(shù)據(jù)EKF正向和RTS反向各跑一遍總的處理時間大約幾秒鐘完全可接受。3.5 四種算法特性橫向?qū)Ρ任野阉奶姿惴ǖ暮诵奶匦宰隽艘粡垖Ρ缺硭惴ㄓ嬎銖碗s度實時性相對精度抗粗差能力利用未來數(shù)據(jù)適用場景WLS低高基準弱否實時單點定位EKF低高略優(yōu)于WLS中否實時動態(tài)定位MHE高中優(yōu)于EKF中強部分(窗口內(nèi))有約束的實時估計RTS中低(事后)最優(yōu)中是全部高精度后處理這里的相對精度結(jié)論是基于我的實測數(shù)據(jù)不是絕對結(jié)論。不同數(shù)據(jù)質(zhì)量、不同場景排序可能會有變化但RTS在最末位平滑后精度最高這個結(jié)論在絕大多數(shù)場景都成立。4. 核心實現(xiàn)細節(jié)與參數(shù)調(diào)優(yōu)4.1 坐標系統(tǒng)轉(zhuǎn)換ECEF、LLA與ENU定位解算輸出的坐標是地心地固系ECEF下的XYZ但要評估誤差或者畫軌跡圖最好轉(zhuǎn)成經(jīng)緯高LLA再投影到以參考點為原點的東北天坐標系ENU。我最常用的轉(zhuǎn)換流程是ECEF轉(zhuǎn)LLA用標準的迭代公式或者MATLAB的ecef2lla函數(shù)DDM格式的經(jīng)緯度注意不要混結(jié)算符ENU坐標系以真值軌跡的起點或者某個參考站的坐標為原點這樣誤差分析能直觀看出水平方向和垂直方向的偏差。這里有個容易搞混的點手機GNSS輸出的是WGS84坐標系的坐標而地圖應用通常用的也是WGS84但如果你做的是國內(nèi)的高精度定位評估可能需要了解CGCS2000與WGS84的差異。前者和后者在大多數(shù)民用場景下差異在厘米級對單點定位精度評估來說可以忽略但如果做高程方向的精密分析還是要注意參考框架。4.2 權(quán)重矩陣的構(gòu)造策略WLS和EKF的測量噪聲矩陣R的構(gòu)造直接影響定位精度這是我調(diào)參過程中感受最深的地方。第一版腳本我用了等權(quán)模型所有衛(wèi)星的偽距噪聲方差都取1米2結(jié)果在城市環(huán)境下定位誤差很大尤其是低高度角衛(wèi)星的觀測噪聲被低估了導致結(jié)果被帶偏。第二版改成基于高度角的模型效果好了很多但依然存在個別衛(wèi)星異常拉偏的情況。第三版在高度角基礎上加入信噪比校正CN0越高權(quán)重越大。模型形式是σ2 a2 b2/sin2(el) × (CN0_ref/CN0)^2。這個經(jīng)驗模型的物理含義是信噪比直接反映接收信號的質(zhì)量低信噪比觀測值往往伴隨顯著的多徑誤差壓低權(quán)重是非常合理的處理。對于手機上經(jīng)常出現(xiàn)的異常偽距跳變我還在測量更新前加了一步殘差卡方檢驗。計算每個歷元的標準化殘差如果某顆星的殘差超過3倍標準差就把該衛(wèi)星的測量噪聲方差乘以一個大因子等效于降低權(quán)重而不是直接粗暴剔除。這樣處理保留了衛(wèi)星的幾何貢獻同時抑制了粗差影響。4.3 EKF的過程噪聲和測量噪聲整定EKF里Q矩陣和R矩陣的比值決定了濾波器的響應速度和穩(wěn)定性這是整定過程中最核心的平衡。Q矩陣物理含義是你對運動模型的信任程度Q越大代表你越相信測量值、越不信運動模型濾波器響應越快但越不平滑容易受噪聲影響Q越小則相反軌跡越平滑但對真實機動響應越慢。我的初始設置參考了經(jīng)驗值位置過程噪聲取0.1 m/√s量級對應的方差速度過程噪聲取0.5 (m/s)/√s鐘差過程噪聲取10 m/√s對應的方差鐘差變化率取1 (m/s)/√s。然后用前2分鐘的靜態(tài)數(shù)據(jù)做靈敏度分析把位置過程噪聲從小到大掃一遍找到水平誤差最小的量級。如果車載運動三軸加速度突變大位置過程噪聲需要上調(diào)一個數(shù)量級否則轉(zhuǎn)彎時濾波會明顯滯后于真實軌跡。手機步行場景則相對溫和可以維持較小值。還有一個容易被忽視的細節(jié)是R矩陣的時間一致性。偽距噪聲方差和接收機跟蹤環(huán)路帶寬有關手機與專業(yè)接收機不同室內(nèi)外切換時偽距噪聲特性會突然變化建議在R矩陣里加一個基于CN0的時變因子。4.4 性能評估方法算法對比不能只看軌跡看起來對不對要有量化指標。我從三個維度評估水平誤差2D位置誤差的均方根RMS和95%分位誤差。定義是ENU坐標系下北向和東向分量的平面距離誤差。這是民用導航最重要的指標。垂直誤差的RMS。手機單頻偽距定位的垂直誤差普遍比水平誤差大1.5到2倍這主要是衛(wèi)星幾何特性決定的。我實測WLS的垂直誤差RMS約8到12米水平誤差RMS約5到8米參考真值是手機自帶的加速度計行人航位推算結(jié)果。計算耗時。用MATLAB的tic/toc統(tǒng)計每個歷元的平均處理時間在需要強調(diào)實時性的場景這個指標很重要。另外我還畫了CDF誤差累積分布圖和逐歷元時間序列誤差曲線。CDF圖能直觀看出90%的時間誤差落在什么范圍內(nèi)比單看RMS更有說服力。關于參考真值這是個關鍵問題。手機定位實驗很難獲得厘米級的真值我用了三種參考來源靜態(tài)場景直接用已知坐標點動態(tài)場景用手機自帶的GNSS融合定位輸出做參考雖然有誤差但量級遠小于單頻偽距定位作為對比基準可以接受如果有條件可以在開闊場地用RTK接收機同步觀測得到真正的高精度參考軌跡。5. 實測結(jié)果與避坑實錄5.1 靜態(tài)場景下四種算法的表現(xiàn)在樓頂開闊環(huán)境做了15分鐘的靜態(tài)測試天線位置固定手機平放視野內(nèi)可見衛(wèi)星數(shù)平均10顆左右GPS北斗Galileo聯(lián)合定位。從這個場景拿到的數(shù)據(jù)顯示W(wǎng)LS水平誤差RMS約4.6米95%誤差9.2米。由于每歷元獨立解算位置序列呈點狀分布能明顯看到多徑帶來的隨機跳變。EKF水平誤差RMS約4.1米95%誤差7.8米。軌跡平滑不少但靜態(tài)下仍存在低頻漂移這主要來自偽距多徑的慢變分量。MHE窗口N10時水平誤差RMS約3.8米95%誤差6.5米略微優(yōu)于EKF。RTS水平誤差RMS約2.9米95%誤差5.4米。平滑后靜態(tài)精度提升最明顯低頻漂移被有效抑制。需要提醒的是這些數(shù)據(jù)只是單一場景的一次測試不代表算法在所有場景下的絕對水平。做對比研究時至少要在開闊地、半遮擋環(huán)境、城市峽谷各做一組綜合看結(jié)論才靠譜。5.2 動態(tài)步行場景的對比拿著手機沿校園道路正常步行約600米途經(jīng)一段兩側(cè)有建筑的半遮擋區(qū)域。速度約1.2m/s。動態(tài)場景下WLS單點解跳得非常厲害半遮擋區(qū)域甚至出現(xiàn)了連續(xù)多個歷元定位結(jié)果偏離真實路徑超過30米的情況。EKF因為有速度狀態(tài)約束偏離幅度被壓到10米內(nèi)軌跡連續(xù)但出現(xiàn)過沖。MHE在遮擋區(qū)域的軌跡最穩(wěn)定因為窗口內(nèi)的歷史信息形成了約束。RTS是整個時段處理完后輸出在遮擋段落的軌跡被后續(xù)信號恢復后的信息拉回視覺效果明顯更好實際誤差也最小。動態(tài)和靜態(tài)的結(jié)論一致RTS MHE ≥ EKF WLS精確的排序依數(shù)據(jù)質(zhì)量略有浮動。5.3 常見問題與排查技巧速查表我把這個項目里實際遇到的高頻問題整理成了一張速查表現(xiàn)象可能原因排查與解決解析日志后衛(wèi)星數(shù)為0日志時間戳格式不匹配檢查GNSS Logger輸出的是GPS周秒還是Unix時間先統(tǒng)一時間基準偽距全部為負值或異常大未按發(fā)射時刻計算衛(wèi)星位置衛(wèi)星鐘差未修正核實偽距對應的時間標簽補上衛(wèi)星鐘差改正項定位結(jié)果偏向某個方向幾十米電離層/對流層改正未加或符號錯誤單頻場景至少加Klobuchar電離層改正檢查對流層改正項符號WLS迭代不收斂初始位置距離真值太遠或衛(wèi)星幾何退化先用粗略位置如手機網(wǎng)絡定位做初值少于4顆星時放棄該歷元EKF軌跡在信號遮擋后偏移很大過程噪聲Q設置過小濾波器太相信運動模型適當增大Q或在遮擋期間提高位置先驗噪聲濾波協(xié)方差矩陣非正定數(shù)值精度問題或Q/R設置不合理改用平方根濾波或約瑟夫形式檢查矩陣對稱性同一歷元雙重定位結(jié)果差很多衛(wèi)星觀測值粗差未剔除加強殘差卡門檢驗對超3倍標準差的觀測值降權(quán)RTS結(jié)果比EKF還差平滑實現(xiàn)中矩陣求逆數(shù)值問題或狀態(tài)轉(zhuǎn)移矩陣時間步長不匹配檢查平滑增益公式確認狀態(tài)轉(zhuǎn)移矩陣用的是均勻時間間隔必要時使用穩(wěn)定化的矩陣求逆方法5.4 幾個值得一說的實現(xiàn)技巧MATLAB循環(huán)效率不高解析日志文件時逐行處理幾萬行數(shù)據(jù)會有點慢。我一開始用的是fgetl循環(huán)300MB的日志要跑好幾秒。后來改成文本擦操作一次讀入后用strsplit和cellfun做向量化處理速度提升了一個量級。解析GNSS日志這類文本處理工作盡量用MATLAB的文本批處理函數(shù)別一行行讀。另一個技巧是繪圖。EKF和MHE的位置輸出有協(xié)方差信息可以畫誤差橢圓。我在軌跡圖上每隔固定歷元畫一個95%誤差橢圓能直觀看出定位置信度變化。衛(wèi)星信號遮擋時誤差橢圓明顯變大這種可視化效果在報告里非常加分。還有一個容易被忽略的問題手機GNSS日志中的GNSS接收機時鐘受鐘差和漂移影響明顯幾十分鐘內(nèi)可能漂移數(shù)十毫秒。當間隔較長時間再處理時務必要考慮鐘差的狀態(tài)轉(zhuǎn)移模型是否能捕捉這種漂移否則濾波會出現(xiàn)系統(tǒng)性偏差。EKF必須把鐘差變化率納入狀態(tài)向量WLS則不需要擔心因為每歷元重新估計鐘差。6. 擴展方向與個人經(jīng)驗做到這一步四種算法的對比已經(jīng)完整了但這個項目的可擴展性很強給你幾個我覺得很有價值的方向增加RTK或者PPP處理模塊。雖然手機上行RTK的整數(shù)模糊度固定很難但利用雙頻觀測值做PPP精度可以大幅提升。加入慣導數(shù)據(jù)。手機日志里通常同時有加速度計和陀螺儀數(shù)據(jù)和GNSS做松組合在城市峽谷場景下的連續(xù)性和精度都會上一個臺階。數(shù)據(jù)驅(qū)動的方式優(yōu)化噪聲參數(shù)。我在調(diào)參時做了靈敏度分析能不能做個自動化的網(wǎng)格搜索或Bayesian優(yōu)化把Q/R參數(shù)自動整定出來效果大概率優(yōu)于手工調(diào)參。把算法封裝成可配置的框架。目前四套算法各自獨立你可以設計一個統(tǒng)一的接口讓新算法比如粒子濾波、無跡卡爾曼濾波插進來就能跑對比實驗這樣后續(xù)做研究寫論文非常方便。最后分享一點個人體會。這個項目做完我最大的感觸不是哪個算法精度最高而是數(shù)據(jù)質(zhì)量決定上限算法決定逼近上限的程度。我在實驗里用同一套數(shù)據(jù)反復測試結(jié)果差異最大的不是WLS和RTS之間的算法差距而是數(shù)據(jù)預處理做得好不好的差距。把觀測值質(zhì)量篩查、時間對齊、衛(wèi)星位置計算這些基礎細節(jié)做到位哪怕只用WLS也能有不錯的效果反過來再先進的濾波算法喂進去一堆粗差數(shù)據(jù)輸出的也只能是看起來很平滑的錯誤軌跡。做GNSS定位研究別急著上復雜算法先把數(shù)據(jù)鏈路搞扎實再談算法創(chuàng)新。這個項目的價值也正在于此——它逼著你走完整個鏈路每一步都能看到數(shù)據(jù)是怎么流動、怎么被處理的。如果你也在做類似的事情建議一定保持這樣的習慣每處理完一個環(huán)節(jié)就把中間結(jié)果用圖畫出來看著數(shù)據(jù)走一遍遠比悶頭調(diào)算法參數(shù)要有效得多。