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