简介：这套MATLAB项目面向定位技术学习者，将全球导航卫星系统（GNSS）与步进定位/航迹推算（PDR）耦合，用于解决室内外无缝定位、行人导航中卫星信号弱或中断时的精度补偿问题。包内共91个文件，包含28个.m算法脚本、62个txt格式的GPGGA数据日志及1个.mat地图文件，整体大小仅4.59MB，目录划分清晰。算法层面覆盖GNSS坐标转换、PDR步频与航向估计、Madgwick姿态解算、扩展卡尔曼滤波融合等典型模块；日志数据可复现融合前后的定位结果，还提供手机传感器读取、静止检测、步长估计、航向修正等配套函数。适合测绘、导航或机器人方向的研究者以此为基线，深入理解GNSS/PDR原理，并在实际数据上开展算法验证、参数调优或二次开发。目前已有416人学习下载。

1. 为什么把PDR和GNSS绑在一起：这个MATLAB工程到底解决什么问题

在地下停车场或者高架桥下打开导航，定位要么原地打转，要么跳到隔壁街区。这不是手机坏了，是GNSS信号被人造环境遮挡、反射后，解算出来的位置开始“撒谎”。PDR_coupled_GNSS这个MATLAB工程做的事很朴素：让行人航位推算（PDR）和全球卫星导航系统（GNSS）互相兜底。卫星信号好的时候用GNSS修正PDR长时间走路累积的漂移，信号差的几十秒里用PDR按步频、步长和航向把轨迹续上。适合做手机定位、人员定位工牌、室内外一体化导航的工程师，把这类仓库的融合框架借过来，换自己的数据跑通。下面直接讲怎么把这个工程跑起来，再拆核心融合逻辑和最容易踩的坑。

2. 跑通PDR_coupled_GNSS：目录怎么认、数据怎么备、最小命令怎么写

2.1 先认目录：主脚本、数据目录和PDR_brightow子模块的职责

拿到这类“PDR_coupled_GNSS-master”仓库，第一件事不是翻代码，而是先认目录。常见布局是外层一个总目录，里面按处理链路拆开，名字里带GNSSMaster的目录通常是GNSS日志解析和PVT解算模块，带PDR的目录是行人航位推算模块。PDR_brightow这种子目录，一般是一版PDR输出的中间结果或者某条实验轨迹的落盘文件，不必纠结它是谁的代码，重点是找到主入口脚本。我一般会先搜run_、main、demo开头的文件，找不到就直接搜function定义，把函数文件按“输入输出”的关系串起来，整个工程的骨架就出来了。

我拿到手通常做三件事。第一，把仓库里的README或注释头扫一遍，确认它要求的MATLAB版本和依赖工具箱，这步能省掉后面大量莫名其妙的报错。第二，打开主入口脚本，把加载数据那几行路径改成自己的数据路径，不急着改算法参数。第三，把自带demo轨迹跑通，确认环境没问题。这三步做完，再谈融合和调参才有意义。特别注意：如果工程里同时有.mat数据和.m脚本，优先跑自带.mat，因为坐标系、采样率和参数大概率是作者调好的，能跑通说明你的环境没问题，跑不通则是版本或工具箱的兼容问题。

2.2 数据从哪来：仿真数据、手机采集与公开数据集

跑PDR耦合GNSS，数据是最容易卡住的一环。数据来源常见有三种。第一种是仓库自带的仿真轨迹，最省事，但只能验证流程，不能验证真实传感器噪声下算法的鲁棒性。第二种是手机采集，现在不少安卓手机在开发者选项里开启原始GNSS测量后，能用GNSS Logger这类工具记录伪距、载波相位和历书，同时把加速度计、陀螺仪、磁力计一并写进日志。采集时记住把手机别在腰间或拿在手里，保持一个相对固定的姿态，避免一会儿横屏一会儿竖屏，否则PDR的航向解算会先翻车。

第三种是公开数据集，比如智能手机GNSS挑战赛发布的行人轨迹数据，里面同时给了IMU和GNSS观测值的对齐时间戳，省去自己写时间同步。用公开数据有个额外好处：别人的精度指标是公开的，你拿同一份数据调参数，能直接对比出自己差在哪。但无论用哪种数据，启动前都要确认三件事：IMU采样率是多少（多数手机是100Hz），GNSS输出频率是多少（NMEA协议常见1Hz），GNSS定位点是经纬度还是已经转好的本地坐标。这三项决定后面的坐标系转换怎么做、滤波器更新时间怎么设，也是后面排查问题时的第一嫌疑对象。

2.3 最小运行命令：加载数据、跑融合、出图一条龙

跑通最小工程不需要理解每一个函数。下面这个启动脚本是我在类似工程上常用的最小复现写法，可以直接替换主入口脚本之前的启动段：

% run_pdr_gnss.m % 最小启动脚本：读数据 -> 解算 PDR -> 融合 GNSS 修正 -> 输出轨迹图 % 常见做法：把脚本放在仓库根目录，数据放在 data/ 子目录 clc; clear; close all; % 1) 加载数据：仓库自带 demo 轨迹，内含 IMU 和 GNSS 两组观测 data = load('data/sample_track.mat'); % 结构体里一般有 imu 和 gnss 两个字段 imu = data.imu; % 字段: ax, ay, az, gx, gy, gz, mx, my, mz gnss = data.gnss; % 字段: lat, lon, alt, t_gnss % 2) 集中管理参数，后面调参不用翻散落各处的大括号 cfg = struct(); cfg.imu_fs = 100; % IMU 采样率，手机内置 IMU 多数是 100 或 200Hz cfg.gnss_fs = 1; % GNSS 输出频率，NMEA 消息常见 1Hz cfg.ref_lat = data.origin_lat; % ENU 坐标系原点纬度 cfg.ref_lon = data.origin_lon; % ENU 坐标系原点经度 cfg.step_coef = 0.42; % 步长系数，Weinberg 模型常用 0.35~0.5 cfg.heading_smooth = 5; % 航向平滑窗口（帧） % 3) 主流程：PDR 解算 + GNSS 融合，一行调用 result = pdr_gnss_fusion(imu, gnss, cfg); % 4) 出图：把 GNSS、PDR、融合三条轨迹叠在一起看 figure; plot(result.gnss_enu(:,1), result.gnss_enu(:,2), 'k--'); hold on; plot(result.pdr_enu(:,1), result.pdr_enu(:,2), 'g-'); plot(result.fused_enu(:,1), result.fused_enu(:,2),'b-'); legend('GNSS', 'PDR', 'PDR+GNSS', 'Location', 'best'); axis equal; grid on; xlabel('East (m)'); ylabel('North (m)');

这段脚本里的参数有几个必须记清楚。imu_fs是后面做步频检测的基准，设错了整个步数都会错；gnss_fs用于卡尔曼滤波的更新时间，把1Hz的实际数据设成10Hz不会让定位更频繁，只会让滤波器空转，并引入多余的插值误差。ref_lat和ref_lon是GNSS经纬度转到本地坐标系的锚点，工程里通常取第一个有效GNSS点。step_coef是步长模型全局系数，后面详细说。跑完如果能看到三条轨迹大致重合、融合线比GNSS虚线更平滑，说明全链路是通的。

注意：如果仓库里的融合函数不叫pdr_gnss_fusion，以仓库实际主函数为准。最小复现的目标是把环境错误暴露在你自己的数据上，不是纠结函数名。

数据文件换成你自己的实测轨迹时，记得把origin_lat / origin_lon改成你轨迹的起点，否则坐标系原点错位后，融合结果会整体偏出图，而且这个偏出非常隐蔽——轨迹形状看起来正常，只是位置不对，特别容易误判成算法问题。

3. 融合逻辑拆到底：PDR解算、ENU坐标转换与卡尔曼滤波

PDR和GNSS不能直接相加：二者坐标系不同、输出频率不同、错误模式也不同。把它们绑在一起的核心工程问题只有一个——在什么时间、用什么权重，把GNSS的位置修正量加到PDR的递推轨迹上。

3.1 PDR四件套：步频检测、步长模型、航向推算、坐标递推

PDR的本质不是惯性积分，是事件驱动。把加速度、陀螺仪原始数据变成每一步的长度和方向，然后一步步往前走。事件驱动的第一件事是步频检测。常见做法是对三轴加速度求模值，去掉静态重力分量后做峰值检测：

% 步频检测：加速度模值减去重力，剩下的就是运动分量 acc_mag = sqrt(imu.ax.^2 + imu.ay.^2 + imu.az.^2) - 9.8; % 峰值检测：找局部极大值，且幅度和最小间隔都满足条件 [pks, locs] = findpeaks(acc_mag, 'MinPeakHeight', 0.8, ... 'MinPeakDistance', round(cfg.imu_fs * 0.3)); n_steps = numel(locs); step_time = imu.t(locs); % 每一步的时间戳

这里的MinPeakHeight单位是 m/s²，取0.8意味着加速度波动超过0.8才计一步，太小会把掏手机、转身这类动作当成迈步；MinPeakDistance设成0.3秒，是防止一次迈步被检测出两三个峰值。这两个值是PDR精度的地基，大部分虚检和漏检都出在这一步。

步长模型常见有两种：固定步长最简单，适合匀速直线；Weinberg经验模型用一步之内的加速度峰谷差估算步长，对手机放口袋、拿手里都适用：

% Weinberg 步长模型: 步长 = k * (acc_max - acc_min)^(1/4) % k 就是 cfg.step_coef，通常在 0.35~0.5 之间 for i = 1:n_steps idx = step_idx(locs(i), imu.t); % 找这一步对应的采样点区间 seg = acc_mag(idx(1):idx(2)); step_len(i) = cfg.step_coef * (max(seg) - min(seg))^0.25; end

Weinberg模型的好处是只用加速度幅值，不依赖安装方向。那个指数0.25不是玄学，它描述的是步长和摆动幅度之间的非线性关系，实际标定时通常只用系数k去拟合，指数保持固定。k和人的身高、步速强相关，后面调参章节会讲怎么标定。

航向推算分两支：陀螺仪积分给出相对航向变化，磁力计或GNSS速度方向给出绝对航向，再用互补滤波把两者融合。落代码时最直接的做法是：

% 航向融合：陀螺仪积分航向 + 磁力计航向的互补滤波 heading_gyro = cumsum(imu.gz * dt); % 陀螺仪积分，注意去零偏 heading_mag = atan2(imu.my, imu.mx); % 磁力计航向 heading = alpha * heading_gyro + (1 - alpha) * heading_mag; % alpha 取 0.9 左右

alpha取0.9意味着短期更信陀螺仪，因为磁力计噪声大且易受环境干扰；长期靠磁力计把陀螺漂移拉回来。这一个小数决定转弯半径误差和直行方向的稳定度，是航向模块里最值得调的参数。最后坐标递推非常简单：E_new = E_old + step_len * sin(heading)，N_new = N_old + step_len * cos(heading)。到这里，PDR已经能输出一条连续的米制坐标轨迹，但它是开环的，走100米漂几米是常态。

3.2 GNSS数据怎么变成能用的位置修正量

GNSS输出的是经纬度和椭球高，不能直接和PDR的米制坐标相加，必须先转换到同一坐标系。常见做法是取轨迹第一个有效GNSS点作为ENU原点，把后续每个经纬度都投影到以该点为原点的东北天坐标系下：

% WGS84 经纬度转 ENU：用第一个有效点做原点 phi = deg2rad(lat); lam = deg2rad(lon); dx = (lam - lam0) * cos(phi0) * R_earth; % 东向，R_earth 取 6378137m dy = (phi - phi0) * R_earth; % 北向

这段看着简单，实际工程里容易踩的坑是：R_earth要不要用当地曲率半径、高度变化要不要处理。对于几百米的短轨迹，固定R_earth = 6378137米足够；如果轨迹跨几公里，就要用两个主曲率半径分开算。转换完之后，GNSS轨迹和PDR轨迹就都在同一个米制平面上，可以进滤波器了。另一个常见问题是高度：PDR本身不产生高度，如果轨迹有上下坡，GNSS高度又噪声大，一般干脆把融合限制在水平二维，高度单独滤波，别把高度噪声带进水平位置。

3.3 用卡尔曼滤波把两者绑在一起：事件驱动和时间驱动怎么对齐

PDR和GNSS最难融合的点不在导航方程，在驱动频率。PDR是事件驱动——走一步才更新一次，跨步频率大概1.5到2Hz，而且不匀速；GNSS是时间驱动——每秒固定来一帧。两者频率不一致，直接导致卡尔曼滤波的预测和更新不好对齐。常见做法是把PDR的一步位移量作为滤波器的状态转移控制量，GNSS的定位点作为观测更新输入：

% 卡尔曼滤波核心：状态 [E, N, vE, vN] % 初始化 x = [0; 0; 0; 0]; P = eye(4) * 1e-2; F = eye(4); % 单位转移矩阵 % 预测：一步PDR位移作为控制量 u x(1:2) = x(1:2) + [step_len * sin(heading); step_len * cos(heading)]; P = F * P * F' + Q; % Q 是过程噪声，来自步长和航向的估计误差 % 更新：只有收到新 GNSS 帧时才执行 if gnss_new_frame z = gnss_enu(k, :)'; % GNSS 观测：E, N H = [1 0 0 0; 0 1 0 0]; K = P * H' / (H * P * H' + R); x = x + K * (z - H * x); P = (eye(4) - K * H) * P; end

这里的工程细节是：GNSS更新帧之间可能跨了0到2步的PDR位移，所以状态必须持续保存，GNSS帧到了就直接更新，不用管上一帧是什么时候到的。如果强行把GNSS和PDR都插值到同一个固定频率再滤波，反而会引入插值误差，轨迹在转弯处会更肉、更滞后。

代码中Q和R分别是过程噪声协方差和观测噪声协方差。Q反映你对步长和航向模型的信任程度，R反映你对GNSS定位点的信任程度。Q取太大、R取太小，滤波器会被每一条GNSS点拖着走，PDR的平滑作用就没了；反过来，融合轨迹会继承PDR的所有漂移。初值我一般从Q = 1e-3 * eye(4)、R = 1 * eye(2)起步，再按第4节的调参方法往两个方向试。

3.4 质量门控：GNSS信号差时别让PDR跟着翻车

卡尔曼滤波不是救命仙丹，它只会按你的Q和R把两个噪声源折中。如果GNSS在城市峡谷里直接漂出几百米，滤波后的融合轨迹也会被拉走，此时反而是PDR在前几秒更可信。所以融合链路里必须有一个质量门控：把GNSS的卫星数、HDOP、伪距残差作为准入条件，质量差时直接跳过本次更新。

% GNSS 质量门控：不满足条件就跳过卡尔曼更新 if gnss.sats < 4 || gnss.HDOP > 2.5 % 信号差，本次不修正，交给 PDR 续跑 skip_update = true; end

这个2.5的阈值不是拍脑袋。HDOP大于2.5说明卫星几何分布很差，水平方向解出的位置已经有明显发散趋势，硬更新进去等于给PDR添乱。另外，也可以用新息卡方检验：计算出滤波器的新息（观测值和预测值的差）归一化距离，超过阈值就认为是野值：

% 新息卡方检验：拒绝异常 GNSS 帧 innov = z - H * x; % 新息 S = H * P * H' + R; % 新息协方差 d2 = innov' / S * innov; % 马氏距离 if d2 > chi2inv(0.95, 2) % 超过95%置信门限 skip_update = true; % 这次 GNSS 点不可信 end

这个检验能在GNSS跳变的一两帧内就把异常点拦下来，而不是让滤波器慢慢适应。和BLE指纹定位与PDR融合的方案相比，GNSS+PDR的优势是室外开阔场景完全自给自足，不依赖额外部署信标；缺点是进了深室内卫星数降到0，质量门控直接失效，只能纯靠PDR加零速修正硬撑。所以工程落地时，这条融合链通常还要留一个外部观测接口，给蓝牙或WiFi指纹融合留位置。

4. 六个必调参数：融合效果好坏一大半看这里

4.1 参数表：影响方向和适用场景

同样的代码，参数不同，融合轨迹可能一个像样一个翻车。把PDR_coupled_GNSS这类融合工程里最常见的六个参数列在下面，调参前先对着这个表确认自己动的每个数影响的是哪个环节：

这张表的核心逻辑是：参数之间不是独立的。step_coef标定不准，会让PDR轨迹整体变长或变短，GNSS修正再好也拉不回来；Q和R实际控制的是两个来源的信任分配，不是单个数值。所以调参顺序必须是：先定传感器参数，再标步长模型，最后调Q和R。

4.2 一次完整的调参过程：从默认值到地下车库场景

拿一个典型场景举例：从开阔停车场走进地下车库再走出来，全程约400米。默认参数起步时，融合轨迹在开阔区表现很好，一进地库就开始逐渐偏离真值，出来后被GNSS拉了一下但拉不回原来的位置线。这说明PDR在地下段漂移太大，或者GNSS门控没有把车库出口的临时定位点拦住。

第一步，把MinPeakHeight从0.8调到1.0，因为地库地面平整、步态更规律，0.8容易把你掏门禁卡的动作也计成步。这一步改完虚检明显减少。第二步，标定step_coef：找一段已知30米距离的直线走一遍，数出实际步数，用标定公式k = 实际距离 / (步数 * (acc峰谷差)^(1/4))反推，通常反推出来在0.4附近。第三步，调Q和R：在开阔区先调到R = 2，让轨迹不离散；进入地库前把HDOP门控阈值从2.5提高到2.0，因为地库入口卫星少但HDOP可能还在2.0附近，硬更新会引入错误修正。

第四步也是很多人漏掉的：GNSS从地库回到开阔区后的第一次修正会出现一个大的跳变，如果新息卡方检验的置信度设得太宽（比如0.99），这条跳变会直接砸进滤波器，把融合轨迹整个往错误方向拽。把置信度收到0.95，异常帧被拒后，下一帧正常GNSS才会生效。整个调参过程走完，再对比同样的路段上融合轨迹和纯GNSS轨迹的偏差，基本能看出PDR在遮蔽段保住的几十米优势。做完这四个步骤如果融合轨迹还是明显不对，先回去看数据质量，别继续调参——数据里缺了时间戳或IMU楼层装反了，参数怎么调都救不回来。

4.3 调参验证：别凭感觉，用RMSE和CEP95说话

参数调完不是看轨迹顺不顺眼，要量化。常见指标是轨迹的均方根误差（RMSE）和95%圆误差概率（CEP95），把融合结果和真值对比：

% 计算 RMSE 与 CEP95：err 是每一帧的定位误差， % 真值可以来自地图道路、RTK 或者人工标注的参考轨迹 rmse = sqrt(mean(err.^2)); cep95 = prctile(err, 95); % 升序排列后取第95百分位 fprintf('RMSE = %.2f m, CEP95 = %.2f m\n', rmse, cep95);

如果真值只有起点和终点，那就比起点终点闭合误差和轨迹总距离误差，这两个指标能反映步长标定和航向漂移的大局，但不能反映中间段的形状误差。做验证时建议至少跑三个场景：开阔直道、城市峡谷、室内或遮蔽段，分开统计。因为PDR和GNSS在不同场景下扮演的角色完全不同，合在一起算的误差会把它们互相掩盖——这正好引出下一章要说的坑。

5. 避坑与常见问题排查：现象、原因、解决一条条对齐

融合定位的坑大多不是算法本身的问题，而是坐标、时间、物理单位这些外围工程问题。下面五条是我在类似工程里遇到次数最多的记录，按现象到解决对齐。

5.1 现象：GNSS一进来，融合轨迹就跳回起点或整体偏移

原因基本是坐标系没对齐。PDR用的是以某点为原点的本地米制坐标，GNSS是经纬度，如果仓库里没做ENU转换，或者ref_lat / ref_lon取错了（比如取了第二个有效点而不是第一个），融合轨迹会整体偏出一大截。解决办法：把第一个有效GNSS点作为ENU原点，在进入融合之前统一坐标基准；同时检查GNSS时间戳和IMU时间戳是否同源，如果GNSS是UTC、IMU是本地时钟，需要先做时间对齐，否则坐标偏了还会叠加时间偏。

5.2 现象：人站定不动，PDR还在数步，里程自己往上涨

原因是MinPeakHeight阈值太低，加上静止时加速度计噪声本身有0.2~0.5 m/s²的波动，把噪声峰值检测成了步。解决：把MinPeakHeight提高到1.0以上，并加一个静止检测器——3秒窗口内加速度方差小于0.1就认为静止，静止期间强制不检测步频。这个静止检测对后续的零速修正（ZUPT）也很关键，能顺手把陀螺仪零偏的问题一起压下去。实测时，手机放在桌上和放在口袋里的加速度噪声水平完全不同，阈值最好按自己设备先放一段数据看分布再定，别直接抄别人的值。

5.3 现象：走了100米，航向偏了15度，轨迹像在画龙

原因通常有两个：陀螺仪零偏没有补偿，或者磁力计受环境和手机内部磁场干扰。解决：陀螺零偏可以用静止段的均值估计，在积分前扣除；磁力计用椭球拟合做硬磁和软磁校准，校准一次能管住大部分放置姿态变化。航向里alpha取0.95可以压住短期抖动，但陀螺零偏没去掉时，alpha越大，漂移越固执。另外，手机里的喇叭、马达、无线充电线圈都是强磁源，磁力计离它们越远越好，这就是为什么有些设备要求把手机平放而不是贴着手臂。

5.4 现象：GNSS点突然跳出去二三十米，融合轨迹跟着被拽飞

原因是GNSS在城市峡谷、树下或者多径环境下出的野值，没进滤波器之前就被放行了。解决：加HDOP和卫星数门控，加新息卡方检验，两个一起上。尤其新息检验，它是针对单条异常观测的最后一道闸门，只靠信噪比和卫星数挡不住所有跳变。如果外接GNSS天线，还要额外检查天线相位中心偏移——天线的物理位置和融合输出的原点往往不是一个点，这个固定偏差会被滤波器当成真值吸收掉，表现为融合轨迹整体偏一个固定方向。

5.5 现象：换了一台电脑或MATLAB版本，脚本跑起来报函数不存在

原因很可能是用到了Navigation Toolbox或Signal Processing Toolbox里的函数，旧版本没有。解决：先看报错函数名，如果是坐标转换类的工具函数，直接手写替代，就十几行的事；如果是滤波类的，换用当前工具箱支持的等价写法。安装工具箱时要看具体版本，不要只看MATLAB主版本号。顺手把ver输出的一次截图存在工程目录里，下次换环境对照起来快很多。

6. 进阶验证：跑一条自己的轨迹并把精度量化出来

前面把最小工程跑通了，也知道了哪些参数能调。下一步最值得做的事，是采集一条自己的实测轨迹，把融合效果算成数字，而不是停在“看起来轨迹重合了”这一步。

采集方法很简单：找一段已知长度的路线，操场跑道一圈400米最好。手机开启原始GNSS和IMU记录，放进衣服口袋，姿态尽量固定。走之前站定20秒，让静止检测拿到陀螺零偏；走完再站定10秒。这样一条轨迹包含静止段、直线段、弯道段，足够暴露步频虚检、航向漂移和GNSS跳变三类问题。事后用地图测距标定起终点，就拿到了真值参照。

验证代码可以这样写：

% 验证融合效果：对比融合轨迹与真值轨迹 ref = load('track_ref.mat'); % 真值轨迹，每帧对应 E, N err_fusion = sqrt(sum((fused_enu - ref).^2, 2)); err_gnss = sqrt(sum((gnss_enu - ref).^2, 2)); fprintf('融合 RMSE=%.2f m vs GNSS RMSE=%.2f m\n', ... sqrt(mean(err_fusion.^2)), sqrt(mean(err_gnss.^2)));

一个容易忽略的习惯：把整段轨迹按开阔区和遮蔽区分开统计。如果融合结果在开阔区比纯GNSS差，说明Q和R的信任分配偏向PDR过多；如果在遮蔽区还能修复GNSS跳变，说明门控生效了。这两条结论直接指导下一轮调参方向，比只看一个总均方根有用得多。

我之前犯过一个错：第一次做融合验证时只看总RMSE，觉得融合比纯GNSS好两米就收工了，结果在地下车库那段，融合轨迹整整偏了十米——因为总误差被开阔段的良好表现拉平了。后来把统计口径拆开，才发现是地库入口的HDOP门控设得太宽。现在每调一次参数都分场景出指标，不再笃信单个数字。希望帮到你。

本文还有配套的精品资源，点击获取