简介：面向行人惯性导航（PDR）研究与开发人群，压缩包内整合了完整的行人航位推算算法实现与配套实测数据。内容覆盖惯性导航系统（INS）基础、步态检测、步长估算、角度校正、多传感器数据融合及漂移修正等核心环节，包含Kalman滤波、扩展Kalman滤波（EKF）等融合算法对应的MATLAB脚本，以及加速度计、陀螺仪、方向角与坐标等实际行走过程的txt数据，可用于算法调试、精度评估与室内定位方案验证。压缩包共40个文件，以24个.m算法代码和16个txt原始数据为主，整体仅344KB，结构清晰、轻量易用。该资源已有4591人学习，适合具备惯性导航或滤波理论基础的开发者用于课程设计、科研实验或室内定位项目预研，尤其适用于GPS信号受限的室内环境。 做室内定位这些年，一个绕不开的话题就是PDR。要精度没精度，要稳定性没稳定性，但你发现做室内导航的最终都要回到PDR这条路。GPS在室内直接失效，UWB和蓝牙基站方案要么贵要么布点麻烦，WiFi指纹一辈子都在跟信号抖动作斗争，算下来还是惯性导航里这套行人航位推算（Pedestrian Dead Reckoning，PDR）最接地气——手机里本来就有加速度计、陀螺仪和磁力计，不用额外硬件，纯靠算法从传感器数据里估算位移。这篇文章我就想好好聊聊PDR这套东西：四步核心链路、各模块常用算法、数据处理上的那些隐蔽陷阱，以及实测时最影响精度的几个真实瓶颈。
1. 为什么室内定位绕不开PDR
先说大背景。户外定位有GPS/GNSS，精度几米到十几米，够用。人一进商场、地下车库、大型展馆、医院走廊，卫星信号被楼板钢架遮得七七八八，多径效应更是把伪距测量搅得一塌糊涂，定位精度直接掉到几十米开外，导航根本没意义。替代方案一个比一个麻烦：
- 基站定位（Cell ID / OTDOA）：精度太大，百米级，只能区分你在哪个街区。
- Wi-Fi指纹定位：需要前期采集指纹，环境一变（店铺重装、AP挪位置）指纹库就失效，日常维护成本极高。
- 蓝牙Beacon：同样要布设和维护，且实际精度受环境影响波动很大。
- UWB：精度确实好（厘米级），但终端普及率低，基础设施投入大。
PDR的方案思路完全不一样，它不依赖任何外部基础设施。导航坐标系的基本方程很简单：
x(k+1) = x(k) + S(k) * sin(ψ(k)) y(k+1) = y(k) + S(k) * cos(ψ(k))说白了就是：这一步大概走了多远，朝哪个方向走的，从上一个已知位置推过去。所谓航位推算，就是不断做这种相对位移累加。它解决什么问题？它能覆盖GNSS失效区域，给出一段连续的、短期（几十秒到几分钟）相对可靠的轨迹；它能作为指纹、基站等多源方案之间的“缝合线”，把离散的定位点串成平滑路径；它还能和地图匹配结合，把手持终端的位置约束到走廊、通道这类可通行区域内。
PDR真正适合的人群也很清楚：做室内定位导航产品、研究行人运动分析、做AR/VR里姿态与位移估算、做井下或隧道等高危场景人员定位的工程师。它不是万能的，航向漂移是它的阿喀琉斯之踵，但控制好误差源，用在室内导航的最终输出端完全够用。
2. PDR四步核心链路：测步、算步长、定航向、更新位置
PDR的主流技术路线几乎都是同一套框架——步态检测、步长估计、航向估计、位置解算。四个环节环环相扣，任何一环拉胯，最终轨迹都好不了。我拆开来讲。
2.1 步态检测：从加速度计信号里数步数
步态检测最常见的手段是检测加速度计模值的周期性波动。人走路时，垂向加速度会呈现明显的近似正弦波动，迈步触地时会产生一个冲击峰值。把三轴加速度取模：
a_mag = sqrt(ax² + ay² + az²)然后减去重力分量，再对模值做低通滤波（通常用截止频率3~5Hz的Butterworth或滑动平均），得到的就是“动态加速度”信号。在这个信号上做峰值检测，步数就能数出来。
峰值检测有几个硬件级的坑需要注意：
- 采样率不能太低，IMU至少以50Hz以上采样，否则峰值会被削平，过零不易检测。
- 不能只设一个固定阈值，走路快慢变化时峰值幅度会剧烈变化。我习惯的做法是自适应阈值法，维护一个滑动窗口（比如最近2秒）内的最大最小值，动态更新阈值。
- 峰值之间的最小间距要限制，防止一次落脚的震颤被误判成两步，一般设0.25~0.3s。
除了峰值检测，还有零点交叉法、自相关分析、FFT频谱分析以及零速检测（Zero Velocity Update，ZUPT）等方案。零速检测在脚步绑缚式IMU（比如鞋绑式定位靴）里几乎成了标配，因为脚掌触地瞬间有一个明显的静止段，用加速度方差和角速度模值联合判断非常可靠。如果是手机放在口袋里这种应用场景，峰值检测加自适应阈值就足够了，这也是便携设备上数量级更广泛的做法。
2.2 步长估计：不可能精确，但可以稳定
很多入门者在这里栽跟头，以为能精确测出每一步的长度。对不起，没有任何一款消费级IMU能做到步级的精确测量。步长对个体差异、走路速度、路面状态、鞋型都非常敏感。一个1.8米的人慢走和快走的步长差可能达到20cm以上。所以工程上要做的不是“测准步长”，而是“估计一个统计上稳定、残差可控的步长”。
最入门的方案直接给固定步长，比如身高乘以0.45。这只能跑通Demo，精度随个人差异和速度变化迅速恶化。
比较实用的动态步长模型有两类：
线性模型（Weinberg等人提出的经验方法）。它利用摆腿过程中垂直方向的加速度振幅来估计步长：
L = K * (a_max - a_min)^(1/4) # 或直接写成 L = K * pow(a_diff, 0.25)其中a_max和a_min是在当前一个步态周期内，动态垂直加速度的最大值和最小值，K是校准系数。这个式子隐含的意思很直观：步子迈得大，垂直加速度波动大，能耗和振幅都是步长的一个代理指标。
非线性步长模型（如Kim模型、Zijlstra模型）。通常形式为：
L = α * f + β * v + γ即把步频f和速度v（可以对f和步长做平滑递推）作为自变量，对特定行人做回归校准。实测中在稳定步速下这类模型往往比固定参数模型好10%以上。
我个人的经验是：在手机配戴位置变化不大的前提下，固定K系数的Weinberg模型，表现比很多花里胡哨的回归模型都稳定。原因很简单——K本质上吸收了该行人当前步态和佩戴位置的大部分系统偏差，且不需要额外输入速度变量。
2.3 航向估计：PDR漂移的罪魁祸首
航向误差是PDR误差的主导来源。位置是步长和航向的积分，航向偏了1度，走100米就偏了约1.7米，走500米就是8.7米。航向估计通常用三种信息融合：
- 陀螺仪Z轴（或三轴）角速度的积分：短时稳定，长期漂移大。
- 磁力计：提供绝对航向参考，但极容易受铁磁干扰和地磁环境畸变影响。
- 加速度计：提供俯仰和横滚角（姿态），间接辅助航向。
工程上最常用的是互补滤波或扩展卡尔曼滤波（EKF），把陀螺仪的短期精度与磁力计的长期稳定性融合到一起。我给一个简化的互补滤波结构：
ψ_gyro(k+1) = ψ_gyro(k) + ω_z * dt # 陀螺仪积分 ψ_mag = atan2(m_y * cosφ - m_z * sinφ, ...) # 磁力计解算航向 ψ_f(k+1) = α * (ψ_gyro(k+1)) + (1-α) * ψ_magα的值通常取0.95~0.98，即相信短时的陀螺仪，用磁力计做长期的拉拽。这样做的好处是：短暂磁干扰下，陀螺积分仍能保持轨迹平滑连续，发生机械震动时也不会让航向突跳。
这里必须多说一句磁力计的问题。室内钢筋、钢柱、电梯井、配电箱、金属货架都会让磁力计读数出现几度到几十度的偏差。有些人一进室内就发现航向几秒钟内转了三十度，多半是磁力计被环境磁化干扰了。做好磁力计的校准只是第一步，更稳妥的方式是在PDR更新中动态检测磁力计的一致性，遇到异常值就降低权重，甚至完全切到纯陀螺积分，等环境恢复正常再逐步加回磁力计修正。这个逻辑我后面细说。
2.4 位置更新：从“局部坐标”到全局轨迹
有了步数、步长和航向，位置更新就是最机械的一步。在局部水平坐标系（通常取ENU：东-北-天）里：
E(k+1) = E(k) + L * sin(ψ) N(k+1) = N(k) + L * cos(ψ)这里要注意几个工程细节。首先是坐标系对齐：所有惯性数据在进PDR链路之前，必须把载体坐标系的加速度、磁力计数据转换到导航坐标系，而这个转换依赖于实时姿态（Roll/Pitch/Yaw）。姿态解算的质量直接决定了后面所有步骤的质量。姿态解算通常用Madgwick或Mahony互补滤波算法完成，前者精度高、计算量略大，后者更轻量但需要精心调参。
其次是参考点设置：启动PDR前需要赋予一个初始位置（比如你从商场入口开始走），初始位置通常由外部定位（GNSS、Wi-Fi、用户点选）提供。初始位置给得不准，后续整个轨迹就会整体平移，影响不大；但初始航向如果设偏了，影响会随行走距离线性累积。
第三是轨迹修正：纯PDR必然漂移，所以实践中几乎都是“PDR+地图匹配（Map Matching）”的搭配。地图匹配有两个作用：一是把轨迹约束在人能走的走廊/通道/楼梯内，二是利用走廊的约束反向修正航向漂移。有些方案还会用粒子滤波做匹配，让算法对位置不确定性有显式建模。粒子滤波的计算量稍大，但精度和鲁棒性要比单纯的“找最近走廊”强得多。
3. 数据采集与预处理：拿到干净的数据比调算法更关键
PDR性能上限大概只有20%取决于你选的算法模型，剩下80%取决于你喂进去的数据够不够干净。这个是行业内普遍被低估的部分。我用过的IMU模块从十几块钱的消费级MPU6050，到几千块的工业级ADIS16470都有，差异确实巨大，但每个级别的数据都有各自的预处理策略。
3.1 传感器选型的几个关键参数
不是所有IMU都适合做PDR。你至少要关注：
| 参数 | 消费级（手机内置或几十元模块） | 工业级/战术级 |
|---|---|---|
| 陀螺仪零偏稳定性 | 10~100 °/h | 0.1~10 °/h |
| 加速度计噪声密度 | 100~400 μg/√Hz | 20~80 μg/√Hz |
| 量程 | ±8g/±2000°/s | 可配置 |
| 采样率 | 最高1kHz | 最高2kHz以上 |
消费级够不够用？够，但你需要自己在代码里做更多补偿。我实测过手机内置的BMI160/MPU6050做的PDR，经过良好的标定和滤波，百米级轨迹终点误差能控制在2~3米以内，完全能接受。
3.2 四步预处理链：降噪、跳变剔除、单位统一、坐标对齐
低通滤波：陀螺仪和加速度计都建议做低通滤波，截止频率设置在15~30Hz之间，把高频振动（电机、开关门、机械共振）滤掉。但注意，滤波会引入相位延迟，对步态检测里的峰值定位有影响。我通常先用零相位滤波（如
sosfiltfilt）离线处理来分析数据，在线处理时则用普通的IIR滤波并补偿固定延迟。离群值剔除：加速度计偶尔会出现明显的脉冲异常值，比如撞击、跌落。用一个滑动窗口中位数法（rolling median）比简单的均值鲁棒得多。窗口大小取1秒数据（50~100个点）。
单位统一：不同IMU的输出单位五花八门。加速度计有的给g，有的给m/s²，陀螺仪有的给°/s，有的给rad/s。这个错误真的很常见，进算法前先debug一把，统一换算成m/s²和rad/s，后面所有参数才有意义。
坐标对齐：如果你的IMU模块没有严格对齐到载体坐标系（比如手机横竖屏、模块斜装了），那就要通过标定获得旋转矩阵。很多板卡用的时候直接贴双面胶固定，没有对齐，角度全偏，PDR结果必然崩。
3.3 零偏标定：算法之外最值得投入的一步
陀螺仪零偏（bias）的标定效果，直接决定了航向能维持多久不漂。这个我到后面避坑部分还会专门讲。简单说：陀螺仪在静止时的输出均值就是零偏的估计，但它随温度漂移，所以最好是上电预热后静置10~30秒，取平均，然后把这个零偏从后续数据里全部减掉。更专业一点可以在整个采集过程里周期性地检测静止状态，实时更新零偏估计。很多消费级陀螺仪在温度变化20°C时零偏会漂移好几°/s，这个量级如果不管，航向几秒钟就废了。
磁力计标定更是整个PDR里最容易被忽略的环节。裸的磁力计在无干扰环境下测得的xyz数据分布在一个椭球面上，受软磁、硬磁干扰后会变形偏移。要恢复正确的航向基准，至少要做椭球拟合。最经典的做法是在空间里缓慢旋转设备到多个姿态，采集足量数据后用最小二乘拟合椭球中心（硬磁偏置）和尺度矩阵（软磁），然后用拟合结果反算补偿。没有做过磁力计标定的PDR，在室内磁环境复杂的地方几乎不可用。
4. 实测中那些躲不开的坑：漂移、磁干扰、手机姿势
我做过不少真实场景测试，商场、写字楼、地下车库、矿区巷道都跑过，有些坑是几乎每次都踩的。这里不打官腔，直接把最难受的几个问题摊开讲。
4.1 航向漂移是最致命的，尤其在地下室
有过一次在地下二层车库的测试。整个地下区域钢筋密集、车辆密集，磁力计读数不断跳变，陀螺仪积分不到三分钟，航向就已经偏出30度以上。位置轨迹更是螺旋式飞出去，从起点出发回到起点附近时，位置误差已经超过30米。
这个场景非常典型，也正是从那次以后，我把PDR的架构从“开门见山纯惯导”改成了带磁力计故障检测和恢复的版本。核心逻辑是这样的：保留一个陀螺仪的“航向基准”和一个磁力计的“短期观测”，在磁干扰剧烈的环境下，动态计算磁力计读数与当前航向之间的一致性（用内积或叉积判断偏差），如果偏差超过阈值（比如15度），就把磁力计权重降为零，近乎纯陀螺积分；等磁力计读数恢复稳定且与原基准重合，再把它加回来。
这种做法的本质是：陀螺仪提供短时连续、磁力计提供长时参考，两个信号的质量评估必须并行运行。这样哪怕磁干扰来了，也至少保证三分钟内的航向不会立即崩。你问我为什么不直接用EKF揉在一起？因为EKF只会在状态层面估计航向，但不会告诉你磁力计此时是不是被一辆SUV的铁壳吸走了——状态层面的异常检测在强干扰下远不如信号层面的检测干净利落。
4.2 步长估计要跟“佩戴方式”强关联，不能一套参数走天下
同样一个人，把手机拿在手里看导航、放在裤子口袋里、放在书包里，加速度计看到的步态波形完全不同。放在口袋里时，垂向加速度振幅明显减小，峰值也变得更“钝”，如果用同一套K阈值，步长估计会明显偏小。我的做法是：
- 事先定义几种工作模式：手持（Handheld）、口袋（Pocket）、打电话（Calling）、摆臂（Swinging）。
- 每种模式对应一套独立的K系数和滤波参数。
- 启动时或运行中通过加速度计统计特征（幅度、方差、过零率）自动判断当前模式，模式切换时平滑过渡参数。
有些论文会提“模式自适应”方案，但实际工程落地时，两到三种模式就已经能覆盖95%的使用场景了。过度细分只会增加参数维护成本，还容易误判。
4.3 台阶和楼梯：每一步的步长和步态都不一样
电梯厅、扶梯、楼梯是室内导航最绕不开的场景，但PDR在这里简直是灾难。楼梯的步态和水平步行差异已经很大，垂直方向的位移还需要显式建模。如果不处理，一个三层楼的楼梯走下去，你的轨迹会在平面上多出几十米，再叠加航向不准，结果就是起点对不上终点。
楼梯处理比较粗暴但有效的方法：
- 用气压计（barometer）检测高度变化。注意气压计的数据不能直接用，它对开门、空调、楼层气流的扰动非常敏感，最好是做斜率检测：只有持续稳定的气压变化才判定为楼层变化。
- 检测到阶梯步行时，把步长模型切换为“阶梯模式”，每步步长固定为0.15~0.2m，航向则完全依赖陀螺仪。
- 平台和转弯处用零速修正的变体来处理，利用短暂停顿把位置误差重置。
4.4 初始航向的偏差会成为全链路最大的系统性误差
这不是传感器噪声问题，而是很多方案测完直接忽略初始Yaw的问题。用户在起点转了个方向，起点时的姿态没有对准地图坐标，PDR一开始就在错误航向上走。这种偏差不会自我修正，只能通过后续的地图匹配和外部定位源来纠正。
我强烈建议PDR系统在启动时做一个“姿态初始化”状态：引导用户在平地上站定2~3秒，用加速度计解算俯仰横滚，用磁力计计算初版航向，然后让航向估计在初始位置附近停留数秒再正式起步。如果磁力计本身就不可靠（比如在地下车库启动），那就直接提示用户面朝某个已知方向，手动校准初始航向——土办法反而最稳。
5. 精度怎么评、怎么提升：目标不是“定位准”，而是“漂移可控”
很多刚接触PDR的人会问：你们这个精度到底多少米？严格来说，这个问题没法一句话回答。PDR是相对定位，位置误差随行走距离累积，所以更科学的评估指标是相对误差率（比如路程的百分比）和终点位置误差。参考行业经验：
| 场景/条件 | 步行总长约200米 | 步行总长约1公里 |
|---|---|---|
| 光线良好、手持、磁干扰较小 | 终点误差约1~3米 | 约5~10米 |
| 口袋模式、普通商场环境 | 约2~4米 | 约8~15米 |
| 严重磁干扰、手机置于口袋 | 3~8米 | 15~30米，且航向可能发散 |
测出来的“1%~3%误差”不见得是算法好，也可能是步长校准恰好合适、路径弯少。评估时必须把行走路径的复杂度、佩戴方式、环境磁干扰水平全部记录在案。
5.1 提升精度最有效的三件事
- 航向系统做多级融合与故障检测。这一条投入产出比最高，能撑起70%的精度提升。磁力计、陀螺积分、地图航向约束三者之间的信任权重要动态调整，而不是永远五五开。
- 步长模型针对性校准。每个真实用户在正式使用前，走一段已知距离（比如20米直线）做一次校准，把步长模型的K系数拉定。这个校准过程可以做成用户首次使用时的一次引导操作，成本极低，收益却非常明显。
- 地图约束兜底。只要你的场景是室内，就一定要有地图数据。最粗的拓扑地图（走廊中心线、门的位置、连接关系）就能让PDR的误差被限制在单条走廊内，而不是发散到整个平面。
5.2 离线调试技巧
我把调PDR的经验浓缩成几条可操作的建议：
- 先把所有传感器数据录成CSV或Rosbag，离线复现问题，不要上真机盲调。调试时用plotly或matplotlib画轨迹，叠加参考真值（可以用陀螺仪Z轴积分出来的航向做互相验证）。
- 做一个简单的“直线回走”测试——从起点沿直线走20米，再掉头走回起点，PDR轨迹越接近一条直线且终点越靠近起点，说明航向和步长越稳。这个测试可以快速筛掉很多低级错误。
- 步数检测验证：让志愿者走固定步数（比如40步），连续测10次，统计步数检测的召回率和精确率。检测误差超过2%就得检查滤波器和阈值参数。
6. 数据如何驱动算法迭代
最后补一段关于“PDR+数据”这个组合的看法。PDR算法的迭代非常依赖高质量标注数据。单独跟踪一段IMU原始数据很难定位是哪个模块出了问题。所以我在项目中始终会建立一个“数据-标注-重放”的闭环：
- 采集时同步记录视频或行人脚踩标记的路径作为参考真值。
- 离线为每一步打上步态标签（正常走、快走、上楼、下楼、原地转身）。
- 用同一份数据驱动不同算法版本，做回归对比，防止调好一个模块毁掉另一个模块。
这一步看着费时，但长期下来价值极大。你会发现，很多你认为“算法不行”的问题，其实是数据标注或采集方式的问题。比如有次我们反复调不好口袋模式下的航向，最后查了视频才发现测试者走路的时候经常摸出手机看一眼再放回去，这种动作根本没有被系统的模式识别捕捉到。
PDR不是那种“装个库调个参”就能跑得漂亮的算法，它需要你对手里的IMU数据特性、佩戴方式、场景磁环境、地图结构都形成感性认识，然后才能把算法参数调得恰到好处。如果你正在做这个方向，建议先别急着上复杂模型，把步数检测、步长校准、航向融合、零速修正这些基本功扎实过一遍，再慢慢加地图匹配多源融合——这条路走稳了，PDR是真的能在产品里扛大梁的。
本文还有配套的精品资源，点击获取
