这阵子一直在折腾手机GNSS原始数据的定位算法，从Android的GnssLogger导出的txt日志开始，到MATLAB里解析观测值、算卫星位置，再到把WLS、EKF、MHE、RTS四种算法挨个实现对比了一轮。整个过程踩了不少坑，但结果挺有价值——四种算法在同一份手机数据上的定位精度、收敛速度和抗差能力差异非常直观。这篇就把整个项目从头到尾拆开讲一遍，包括日志怎么读、卫星位置怎么算、四种算法在MATLAB里怎么写、实测对比怎么设计，以及那些文档里不会写的坑。
如果你正打算用手机采集GNSS原始测量值来做定位实验，或者想对比不同定位算法的实际表现，又或者就是想看看MATLAB在卫星导航数据处理里到底怎么落地，这篇文章应该能帮你省不少时间。
1. 项目背景与整体思路
1.1 为什么用手机GNSS日志做实验
传统GNSS接收机（比如测量型接收机）价格不菲，门槛也高。但Android从7.0开始开放了原始GNSS测量值的读取接口，一台支持GNSS原始输出的手机就能记录伪距、载波相位、多普勒、信噪比等核心观测数据，相当于把一台低成本GNSS接收机握在手里。Google官方的GnssLogger应用可以在手机上直接输出txt日志，不需要root，也不改硬件，出门带着手机就能采集数据。
用手机数据做定位算法对比有个天然优势：同一份数据可以喂给不同算法，所有差异都来自算法本身，数据噪声、多径、时钟跳变等干扰因素完全一致。这在对比WLS、EKF、MHE、RTS四种方法时非常关键，因为你不需要费劲控制变量，数据集本身就是同一个变量。
不过手机GNSS数据也有它的问题：伪距噪声大、载波相位周跳频繁、接收机晶振质量一般导致时钟漂移明显、天线增益低导致低仰角卫星信号弱。这些恰恰是最好的测试素材——算法能不能扛住真实恶劣数据，一眼就能看出来。
1.2 四种算法的选择逻辑
WLS（加权最小二乘）是单点定位的经典解法，逐历元解算位置，不依赖历史信息，实现简单，是所有定位算法的锚点。EKF（扩展卡尔曼滤波）在WLS基础上引入运动模型和观测模型，利用时间序列相关性来平滑结果，是实时动态定位的主流框架。MHE（移动时域估计，也叫模型预测估计）把估计问题转成滚动时窗内的优化问题，可以显式加入约束，在非线性更强、约束更多的场景下更有优势。RTS平滑（Rauch-Tung-Striebel smoother）则是卡尔曼滤波的离线版本，先正向滤波再反向平滑，利用全部历元数据，通常能拿到最好精度。
这四种算法从简单到复杂、从在线到离线、从无约束到有约束，正好构成一条完整的对比链条。我把它们全部用MATLAB实现，加载同一份手机日志，跑出轨迹后统一用参考坐标算误差。
1.3 MATLAB在这里的核心价值
做GNSS数据处理的常见工具还有Python和RTKLIB，但我最终选了MATLAB。一方面矩阵运算是MATLAB的看家本领，WLS的H矩阵求逆、EKF的协方差递推、RTS的后向增益计算，写起来几乎和教材公式一一对应，调试时还能直接画图检查中间量；另一方面，MATLAB的脚本化工作流适合这种“数据读取→解析→定位→评估”的线性流程，改算法时只需要替换定位核心模块，IO和评估代码可以完全复用。
一个实用建议：如果你要长期做GNSS算法研究，MATLAB适合算法验证和教学，Python适合工程化部署，RTKLIB适合快速获得参考结果。这篇文章的脚本定位在研究对比层面，所以MATLAB是性价比最高的选择。
2. 手机GNSS日志的获取与解析
2.1 用GnssLogger采集原始测量值
采集工具我用的是Google的GnssLogger应用，也可以选GitHub上一些第三方的GNSS Logger，但官方版本稳定、字段完整，我用下来的体验最好。操作很简单：装上应用，到室外开阔环境，打开应用点击开始记录，走一段或者静止坐一会儿，停止后导出txt文件即可。
需要注意一个细节：手机必须开启“精确位置”权限，并且在开发者选项里确认“位置服务”选的是“高精度”或“仅GNSS”。如果没开权限，日志里只有位置结果没有原始测量值。另外，不同手机的GNSS芯片差异很大，骁龙平台的手机通常支持全星座原始输出，个别手机（尤其部分入门机）可能只有GPS。建议采集前先拿GnssLogger测一下有没有Raw数据输出。
2.2 日志文件的核心字段与格式
GnssLogger输出的txt文件里，最关键的是以Raw,开头的原始测量值记录。每一行代表一颗卫星的一个频率观测，核心字段有：
TimeNanos：接收机本地时间基准的纳秒计数FullBiasNanos：本地时间与GPS时间的总偏差（通常是负数）ReceivedSvTimeNanos：卫星信号的发射时间（在卫星时间系统内）SvName：卫星编号，例如“G01”表示GPS 1号卫星ConstellationType：星座类型，1-8分别对应GPS、SBAS、GLONASS、QZSS、北斗、Galileo、IRNSS等Cn0DbHz：载波噪声密度比，单位dBHz，是信号质量的核心指标PseudorangeRateMetersPerSecond：多普勒测量值转换的伪距率CarrierFrequencyHz：载波频率，用于区分L1/L5等频点
伪距并不能直接从日志里读到一个字段，需要自己计算。基本公式是：伪距 = （本地接收时间 - 卫星发射时间）× 光速。其中本地接收时间要用TimeNanos加上FullBiasNanos等修正项转换到GPS时间，卫星发射时间直接用ReceivedSvTimeNanos换算。这里很容易出符号错误，我在踩坑部分会单独讲。
2.3 MATLAB读取与预处理步骤
我习惯先用fopen加textscan按行读取，再用strsplit切分Raw行。核心流程分四步：
第一，过滤出Raw行的数据。逐行判断行首是否为Raw，是则把后面逗号分隔的字段存入cell数组。
第二，数据清洗。剔除Cn0DbHz小于20 dBHz的观测值，剔除卫星高度角小于10度的观测值（低仰角卫星受多径影响大，伪距噪声很容易几十米甚至上百米），剔除伪距为0或负值的异常记录。
第三，按历元分组。手机日志的测量输出频率通常是1Hz，但每颗卫星的记录是独立到达的，需要按时间戳归并成一个历元内所有卫星的观测集合。我按照TimeNanos四舍五入到秒来对齐历元，每行时间戳波动在10ms以内的归为同一历元。
第四，进行时间系统转换和伪距计算。这是整个解析环节最关键的一步，我会在下一节连同卫星位置计算一起说明。
预处理完成后，推荐先把每颗卫星的伪距随时间变化画出来。如果伪距曲线不是连续平滑递减或递增，说明时间解析有问题，这时候停下来检查比后面跑了半天定位发现结果全乱更高效。
3. 卫星位置计算与误差修正
3.1 广播星历计算卫星位置
定位的前提是知道每颗卫星在信号发射时刻的三维坐标。手机日志本身不带星历，需要另取广播星历文件。我采用的是下载当天对应的BRDC文件（RINEX 3格式的导航电文），可以从公开数据服务站点获取。下载后把文件路径填进脚本，代码从文件里解析开普勒轨道参数。
广播星历算卫星位置的流程是固定的：先算卫星的平均角速度，减去摄动校正项得到改正后的平均角速度；再用开普勒方程迭代求解偏近点角；然后算真近点角、升交角距，并加上二阶谐波摄动修正；最后算卫星在轨道平面内的位置，再转到地心地固坐标系（ECEF）。整个过程在MATLAB里就是几十行公式，注意角度单位统一用弧度，半长轴的单位是米，sqrt(mu)里的地球引力常数mu取3.986005e14。
常见错误是把toc（星历参考时间）和当前信号发射时刻混为一谈，没有做周内秒翻转处理。实际代码里要用tk = mod(t - toe, 604800)来保证时间差落在正负半周内。
3.2 必须做的误差修正项
伪距从卫星到手机，途中要经过电离层、对流层，还要考虑卫星钟差和相对论效应，这些误差不修正，定位误差会直接拉到几十米甚至上百米。我按修正量从大到小排一下：
卫星钟差必须修正，使用星历里的钟差多项式系数a0、a1、a2计算。相对论效应也要修正，简单做法是用-2 * dot(r_sat, v_sat) / c^2（卫星位置向量与速度向量点乘后乘上系数），精确做法是算偏近点角修正。对流层延迟用Saastamoinen模型，输入卫星高度角、观测站高程、气压温度湿度，能修正掉大部分对流层影响。电离层延迟对单频手机用户来说是个大问题，Klobuchar模型配合星历里的电离层参数可以修正约50-60%的电离层延迟，剩下的残差就交给定位算法本身去吸收了。此外，卫星信号发射时刻的地球自转效应（Sagnac效应）也要修正，大约能影响10-20米级别的伪距。
对于手机伪距定位，还要特别注意接收机钟差不能当已知量处理，必须作为未知参数参与解算。手机晶振精度较低，钟差可能在几十微秒到几百微秒之间变化，这个量级的钟差换算成距离就是几公里到几十公里，千万不能忽略。
3.3 多系统组合的时间与坐标基准处理
如果日志里有GPS、北斗、Galileo等多系统数据，组合定位前需要统一时间基准。GPS用GPST，北斗用BDT，两者存在整秒差异（北斗系统时间比GPST快14秒），Galileo系统时间与GPST只差几十纳秒级别，但为了严谨还是建议先归一到GPST。坐标基准方面，WGS-84、CGCS2000、GTRF之间的差异在米级定位精度下可以忽略，直接混用问题不大，但代码里要注明这一点，防止有人拿去搞厘米级定位时在这里栽跟头。
多系统组合最大的好处是可见卫星数变多。单GPS在城市环境可能只有6-8颗卫星，加北斗和Galileo能到15-20颗，几何构型明显改善，定位精度特别是高程分量（Z方向）会有显著提升。我在脚本里留了一个开关，可以单独跑GPS-only和GPS+BDS+Galileo的组合模式，实测对比发现水平误差能降低30%左右。
4. 四种定位算法的MATLAB实现对比
4.1 WLS加权最小二乘：从观测方程到定位解
WLS是整条链路里最基础的一环，也是我确认数据解析、卫星位置计算、误差修正是否正确的最快手段。算法核心是线性化伪距观测方程：把伪距对接收机位置和接收机钟差求偏导，得到设计矩阵H，然后用加权最小二乘迭代求解。
MATLAB伪代码思路如下：
recPos = [0;0;0]; % 初始位置，通常选地心或者上一次定位结果 recClk = 0; % 接收机钟差初始值 for iter = 1:10 rhoPred = sqrt(sum((satPos - recPos').^2, 2)) + recClk; H = [-(satPos - recPos')./sqrt(sum((satPos - recPos').^2, 2)), ones(n,1)]; z = rhoMeas - rhoPred; dx = (H' * W * H) \ (H' * W * z); recPos = recPos + dx(1:3); recClk = recClk + dx(4); end权重矩阵W的选取我这里用了高度角定权：每颗卫星的伪距噪声方差与仰角相关，sigma = a / sin(elev)，权值取1/sigma^2。这比等权最小二乘在低仰角卫星噪声较大的场景下效果明显更好。在开阔环境下，WLS单点定位水平精度一般能到3-5米，高程精度差一些，约6-10米。
WLS的局限在于它逐历元独立解算，完全没用历史信息，动态定位时位置点会有肉眼可见的抖动，但在算法对比里它是最公平的基准线。
4.2 EKF扩展卡尔曼滤波：状态递推与观测更新
EKF把定位问题当成一个动态系统的状态估计问题。我这里选的是8维状态向量：三维位置、三维速度、接收机钟差、接收机钟速。状态转移模型用恒定速度模型，过程噪声按经验调参：位置过程噪声设成0.1 m/s^2级别的加速度扰动，钟差过程噪声根据手机晶振特性设为较大值（钟差噪声谱密度约1e-6 m^2/s级别）。
EKF实现里最关键的坑是状态协方差矩阵P的初始化和数值发散问题。我采用的办法是用前20个历元的WLS解均值作为初始位置，速度设为0，钟差和钟速用WLS解算结果里对应值赋初值，P初始化为对角阵，对角线元素位置部分设10^2（位置不确定度10米），速度部分设5^2，钟差部分设(3000)^2（因为手机钟差可能很大），钟速部分设(10)^2。过程噪声Q和观测噪声R的比值决定了滤波器的平滑程度，这个需要根据实际数据调，我建议在脚本里做成可调参数，多跑几组对比。
EKF在动态场景下比WLS稳得多，位置散点明显收敛。但要注意它的滞后性：在车辆转弯或者加减速时，恒定速度模型会产生系统性滞后，观测噪声越大滞后越明显。手机上载波相位不太可靠，所以我只用伪距做观测，在卫星数突变的场合（比如过桥洞、进隧道边缘），EKF的预测阶段能帮忙撑住一两秒。
4.3 MHE移动时域估计：把估计变成优化
MHE的核心思想是：在一个长度为N的滚动时窗内，把整个窗口的状态序列作为优化变量，最小化先验误差、过程噪声和观测噪声的加权平方和，同时可以加入状态约束。和EKF最大的不同是，MHE处理约束和非线性更自然——你不需要把约束塞进噪声协方差里，直接作为优化问题的边界条件即可。
我这里的实现做了简化：时窗长度N取10个历元，优化变量是窗口内10个时刻的状态向量，约束条件包括位置在 (0,5000) 公里范围内、速度在 -100 到 100 m/s 之间、钟差变化的幅度限制。代价函数前半部分是窗口起始状态相对先验的偏差，后半部分是过程模型残差和观测残差的加权和。我用的是fmincon配合sqp算法求解，观测量还是伪距。
MHE在手机GNSS数据上的表现非常有意思：当某颗卫星出现大的周跳或者多径异常时，MHE因为带有约束和窗口内的信息关联，不容易被单点异常带偏，鲁棒性好。代价是计算量暴涨，10个历元的窗口优化一次大约需要几十到几百毫秒，实时性远不如EKF，比较适合离线解算场景。
% MHE核心：构建窗口内的代价函数，调用fmincon % x_seq: nState x N 窗口内状态序列 cost = @(x_seq) mhe_cost(x_seq, x_prior, P_prior, ... sat_pos_win, rho_meas_win, ... Q, R, dt); nonlcon = @(x_seq) mhe_constraints(x_seq); x_opt = fmincon(cost, x_init, [], [], [], [], ... lb, ub, nonlcon, options);4.4 RTS平滑：离线后处理的精度上限
RTS平滑是EKF的两遍等价形式：第一遍正向跑标准EKF（或者标准卡尔曼滤波），保存每步的状态估计、协方差、预测协方差和增益；第二遍从最后一步开始反向递推，把未来信息“玩现在”。平滑公式就三行：
% 反向平滑 G = P_filt(:,:,k) * F' / P_pred(:,:,k+1); x_smooth(:,:,k) = x_filt(:,:,k) + G * (x_smooth(:,:,k+1) - x_pred(:,:,k+1)); P_smooth(:,:,k) = P_filt(:,:,k) + G * (P_smooth(:,:,k+1) - P_pred(:,:,k+1)) * G';实现上要注意：第一遍滤波必须保存P_pred，因为反向递推需要预测协方差的逆。另外如果中间有观测数据缺失的历元，正向滤波的协方差会变大，RTS依然能处理，因为预测协方差里包含信息衰减。
实测来看，RTS是四种算法里精度最高的。同样的手机静态数据，WLS水平CE95约6米，EKF约4米，RTS能压到2.5米左右。如果有些场景对实时性没要求（比如事后分析、测绘后处理），RTS是最佳选择。解码角度说，RTS不改变滤波器的模型假设，它只是利用未来观测“回看”了过去，所以它不能修正模型本身的错误（比如恒定速度模型在剧烈转弯时不成立），但能显著压低随机噪声。
4.5 四种算法参数与性能速查
| 算法 | 需要先验 | 实时性 | 约束处理 | 相对精度 | 适用场景 |
|---|---|---|---|---|---|
| WLS | 无 | 高（实时） | 无 | 基准 | 单点定位、算法交叉验证 |
| EKF | 有 | 高（实时） | 隐式（靠噪声协方差） | 优于WLS | 车载/行人动态定位 |
| MHE | 有 | 低（离线或准实时） | 显式（边界条件） | 接近EKF，抗差更强 | 多径、异常观测较多的场合 |
| RTS | 有 | 中（需两遍处理） | 隐式 | 最高 | 离线后处理、精密分析 |
参数调整方面我强烈建议：先用WLS定位结果画轨迹，确认数据没问题再跑EKF；再把EKF调通后，MHE和RTS才有意义。不要一上来就调滤波参数，否则你根本分不清是数据问题还是算法问题。
5. 实测结果与算法对比分析
5.1 我的测试场景和数据来源
测试用的是某款支持全星座原始输出的安卓手机，在校园一处开阔操场采集10分钟静态数据，同时在手机旁架了一台测量型接收机做参考坐标真值。另外还跑了一段约1公里的步行动态数据，手持手机沿人行道绕一圈，参考轨迹通过测绘级的RTK事后处理获得。
静态数据卫星数通常在18-22颗之间，星座分布均匀。手机伪距噪声统计下来大约在1-3米（1σ），比测量型接收机高出不少，这对算法是个很现实的考验。
5.2 静态定位精度对比
不管哪种算法，静态时都收获了一个判断精度的基本框架。我取最后200个历元做统计，以RTK参考坐标为真值，各算法的水平误差95%分位数如下：
- WLS：约5.8米
- EKF：约4.1米
- MHE：约4.3米（抗差略强，但精度和EKF接近）
- RTS：约2.7米
有意思的是MHE在个别历元遇到低仰角卫星的粗差时，误差被约束条件压住，没有出现WLS那种单历元跳到15米以上的情况。这说明如果你的应用场景观测质量波动大，MHE的约束价值会超过它的计算开销。
5.3 动态场景的收敛与抗差表现
动态步行数据更能看出差别。WLS轨迹点密集抖动，每隔几秒就出现一个异常跳点，基本不能用来看轨迹。EKF轨迹平滑度明显好，但遇到过街天桥下卫星信号遮挡时，因为观测中断，滤波靠预测撑了大概3-5秒，出来后有小幅位置偏差恢复过程。MHE在这段的表现比较稳，信号恢复后更快回到参考轨迹附近。RTS因为是离线处理，整段轨迹最贴近参考线，特别在转弯处能看出平滑效果，不过恒速模型在转弯处的滞后在RTS里依然存在，只是被反向平滑削掉了一部分。
5.4 计算效率对比
以500历元（约8分钟数据）为例，在我的笔记本（i5-1240P，16GB内存）上：
| 算法 | 单次运行耗时 | 说明 |
|---|---|---|
| WLS | 约0.5秒 | 纯逐历元矩阵运算 |
| EKF | 约0.8秒 | 主要是矩阵递推和观测更新 |
| RTS | 约1.2秒 | 正反向两遍 |
| MHE | 约45秒 | 每历元滚动优化，是瓶颈 |
MHE的耗时是硬伤，但也不是完全没救。可以缩短窗口长度、降低优化求解精度、或者把不含异常观测的历元用EKF替代，只对异常窗口跑MHE，这样能大幅降低计算量。不过在纯对比实验里，我没有做这层优化，保持算法最本真的实现形式对结论更有说服力。
6. 踩坑记录与调试心得
6.1 伪距计算中的TimeNanos符号陷阱
手机GNSS日志里FullBiasNanos通常是负的，含义是“本地时钟相对GPS真时间的偏差”，而TimeNanos是接收机本地计时的纳秒数，数值大且含跳变。最初写伪距公式时我把符号搞反了一次，结果所有卫星的伪距都差了一个固定的量级，WLS定位结果直接落到了地心附近，排查了一下午。正确做法是把本地接收机时间先转到GPS时间，再与卫星发射时间做差，公式写清楚、注释写清楚，分步打印中间量检查。
6.2 手机时钟跳变与卫星时间异常
手机晶振会有秒跳现象——系统时间可能被自动同步调整，导致TimeNanos不连续。这个跳变会直接污染伪距。我的处理办法是：检测相邻历元之间每颗卫星伪距的变化率，如果同一历元内绝大多数卫星的伪距同时出现一个公共跳变（比如某个历元所有伪距突变了约300米对应的3微秒钟跳），就判定是接收机钟跳，把该历元所有伪距统一修正回来。
6.3 卫星数不足和几何构型差怎么办
城区环境卫星数少于8颗、或者卫星集中在一侧天空时，WLS解算结果会明显恶化，Z方向可能漂几十米。遇到这种情况我的做法是：在WLS解算后检查位置方差阵的条件数，如果Hessian矩阵条件数超过一个阈值（比如1e7），就把该历元结果标记为低置信度，后续画图时单独标灰，不让它把平均误差统计拉坏。EKF在这种时刻就体现优势了，它靠预测撑着不发散。MHE也能通过位置约束把Z方向飘移限制住。
6.4 多路径与低仰角卫星的取舍
手机天线没有抗多径设计，低仰角卫星伪距误差可能被地面反射拉偏十几米。我在预处理里强制仰角低于10度的卫星不参与解算。但这里要提醒一句：在卫星数稀缺的场景下（比如室内窗边、地下车库出口），砍掉低仰角卫星可能直接导致卫星数不足，这时候我会放宽到5度，同时对低仰角卫星的权值进一步压低，让它们参与但不主导解算。
6.5 MATLAB代码组织与运行优化建议
这个项目涉及读取、解析、定位、评估多个环节，我的建议是把脚本按功能拆成独立函数模块：
readGnssLogger.m：解析原始txt，输出观测结构体readBrcdNavigation.m：解析BRDC星历，输出星历参数computeSatPos.m：给定星历和时间，计算卫星ECEF坐标correctErrors.m：伪距误差修正（卫星钟差、相对论、对流层、电离层）solveWLS.m、solveEKF.m、solveMHE.m、solveRTS.m：四种定位算法plotTrajectory.m：画轨迹和误差图
MATLAB有个常见性能坑：在循环里不断拼接数组（比如逐历元[x; new]）会导致内存频繁重分配。建议先根据日志行数预估文件大小，用zeros(n,E)预分配矩阵，运行速度快很多。另外一个调试技巧：每次改动算法后，先跑一小段裁剪过的数据（比如前50个历元）验证正确性，再跑全量，能省不少等待时间。
我个人在实际操作中最大的体会是：做算法对比最忌讳直接跳进EKF甚至MHE，先把WLS跑通一次，把伪距曲线和卫星位置画出来人工检查一遍，数据不对后面全是白做。另外，这四种算法不要只盯着精度一个指标，计算复杂度、抗差能力、实时性这些维度在真实项目里往往比小数点下的精度差距更关键。如果你打算把这套流程继续往下延伸，可以考虑引入载波相位差分定位，或者把IMU惯性数据和GNSS观测做成松耦合/紧耦合，那样城市峡谷环境下的定位表现还能再上一个大台阶。
