简介：这套PDR行人航位推算Matlab代码面向室内定位与惯性导航场景，适合需要实现步态识别、航向解算或研究行人位置估算的算法开发者和学生。压缩包共10个文件，以7个.m脚本/函数为核心，辅以2个.xls实测传感器数据和1个.asv自动保存备份，整体仅3.72MB，便于快速部署。代码按数据同步、步长估计、航向计算、坐标转换等功能划分模块，运行pdr_main.m即可加载内置采集数据，完成从加速度计/磁力计同步、加速度计/陀螺仪同步到步长推算与位置输出的完整流程，并可直接绘制推算轨迹。两组xls文件提供了实测加速度、角速度、磁力计等原始记录，可用于检验算法在不同行走模式下的表现；配套博客还给出了原理推导与参数选取说明，降低了PDR从理论到落地的门槛。目前该资源已有3909人学习，适合希望快速跑通基线流程并在此基础上扩展融合定位、滤波优化等方向的开发者参考。
1. PDR(Pedestrian Dead Reckoning)行人航位推算实现代码(matlab)到底是什么
PDR(Pedestrian Dead Reckoning)行人航位推算实现代码(matlab),本质是把惯性测量单元(IMU)输出的加速度和角速度翻译成一段连续坐标轨迹的完整流程。拆这类资源时,我最看重三件事:步态检测用了几层条件、步长模型是不是Weinberg形式、航向融合是纯积分还是有磁力计约束,因为这三点直接决定最终轨迹是能看还是不能用。它解决的是GPS失效场景下的连续定位问题——商场室内、地下车库、隧道,卫星信号完全不可用,而环境里又没布设额外定位硬件。适合手里有一份IMU数据、想快速验证PDR效果,或者准备做室内定位Demo但不想从头啃惯导理论的开发者。
2. 数据准备与预处理:IMU单位转换、重力去除与滤波参数选择
任何PDR流程的第一步都不是跑算法,而是把原始数据洗干净。我见过不少人在这一步翻车,明明算法没错,最后画出来的轨迹却是一团乱麻,根源往往出在单位不统一、坐标系没对齐、滤波过度这三个地方。
2.1 IMU数据导入与单位统一
MATLAB中最常见的IMU数据格式是用load直接读入的mat文件,里面一般是三个矩阵:加速度acc、角速度gyro、磁场mag,外加一个时间戳向量t。数据格式本身没什么好说的,但单位一定要先统一,否则后面所有阈值都失效。
% 读入IMU原始数据 data = load('imu_walk.mat'); t = data.t - data.t(1); % 时间归零,让起点为0 % 单位统一:加速度常见单位有g和m/s^2,陀螺仪常见单位有度/秒和弧度/秒 acc = data.acc * 9.8; % 如果原始加速度单位是g,乘9.8变成m/s^2 gyro = data.gyro * pi / 180; % 如果原始陀螺仪单位是度/秒,转成弧度/秒 mag = data.mag; % 磁场单位通常不需要换算,直接使用uT % 计算采样率并检查时间戳是否均匀 Fs = round(1 / mean(diff(t))); fprintf('采样率: %d Hz\n', Fs);这段代码的逻辑是先让时间戳从0开始,然后按需做单位换算。判断是否要乘9.8或乘pi/180,最直接的办法是看数据的量级:加速度量级在1左右就是g,量级在9.8左右就是标准单位;角速度量级在几十到几百往往是度/秒,量级在1左右的往往是弧度/秒。换算完成后,建议顺手检查一下fs是否跟数据采集时的设置一致,常见IMU采样率是50Hz或100Hz,如果算出来是49.7这种数字,说明时间戳有轻微抖动,不影响后续处理,但后面所有基于Fs的阈值参数都要按实际值来。
2.2 重力分量去除与坐标系对齐
PDR里加速度计输出的原始信号中包含约9.8m/s²的重力加速度,这个分量不剔除,步态检测的阈值就没办法收敛。常见做法是取一段静止数据求平均,得到一个近似重力方向向量,再把每个时刻的加速度投影到这个方向上并减去重力大小。
% 取前2秒静止段,求平均重力方向 static_len = 2 * Fs; g_vec = mean(acc(1:static_len, :), 1); % 1x3 平均加速度,量级约9.8 g_norm = norm(g_vec); g_unit = g_vec / g_norm; % 重力方向的单位向量 % 把三轴加速度投影到重力方向,得到竖直加速度分量(带符号) acc_vert = acc * g_unit' - g_norm;这里的关键逻辑是acc * g_unit'把每个时刻的三轴加速度向量投影到重力方向,得到的是一个标量,表示设备在重力方向上的加速度大小。减去g_norm后,静止状态下这个值应该接近0,行走时每踩一步会出现明显的正峰和负峰。为什么不做完整的三轴去重力?因为完整去重力还需要知道设备的姿态角,在没有陀螺仪积分之前就引入姿态估计会带来额外误差,而对步态检测来说,竖直方向这一个分量已经包含了主要信息,而且对设备放置方向不敏感——手机无论怎么握,重力方向投影都是一致的。常见做法是先用这种方式提取竖直加速度,等航向估计做完之后再回到坐标系对齐。
2.3 低通滤波:截止频率不是越高越好
IMU原始加速度信号里混着高频噪声,行走步伐频率通常集中在1Hz到2Hz之间,所以需要低通滤波。但滤波会带来两个副作用:一是过度平滑会让每步的波峰变小,二是相位滞后会让步态检测的时点偏晚。我一般用零相位滤波,也就是MATLAB里的filtfilt,它可以避免普通filter带来的相位失真。
fc = 5; % 截止频率设为5Hz [b, a] = butter(2, fc / (Fs / 2), 'low'); acc_vert_f = filtfilt(b, a, acc_vert); % 对比滤波前后的信号标准差,确认没有把有效步态信号滤掉 std_raw = std(acc_vert); std_filt = std(acc_vert_f); fprintf('滤波前标准差: %.3f, 滤波后标准差: %.3f\n', std_raw, std_filt);这段逻辑里,butter(2, fc/(Fs/2), 'low')设计的是二阶巴特沃斯低通滤波器,fc/(Fs/2)是截止频率对奈奎斯特频率的归一化。filtfilt做的是双向滤波,正向一遍反向一遍,两次滤波的相位偏移互相抵消,所以峰值的时刻不会滞后。截止频率选5Hz是因为要把2Hz以下的步态主能量完整保住,同时滤掉高频抖动;如果你发现滤波后波峰明显变小,常见做法是把截止频率提高到6Hz或7Hz,而不是降低——步态信号被滤掉了才是主要问题。这一段做完,数据就能交给步态检测了。
3. 步态检测与步长估计:峰值检测、Weinberg模型与参数校准
步态检测和步长估计是PDR里误差占比最大的两个环节:步数错一格,终端位置就偏几步;步长参数偏5%,走一百米就偏五米。这一章的思路是先检测峰,再选模型,最后校准参数。
3.1 竖直加速度峰值检测与伪峰过滤
行走时竖直加速度的周期性非常明显,每一步落地时会产生一个明显的正峰值。检测峰值的朴素做法是设定一个阈值,加速度超过阈值就认为是一步,但实际数据里手机在手里晃动、上下楼梯、身体起伏都会产生伪峰,所以成熟的实现不会只看阈值。
% 峰值检测:同时约束峰高度、峰间距离、峰显著性 MIN_PEAK_HEIGHT = 0.6; % 峰值高度阈值,单位m/s^2 MIN_PEAK_DIST = 0.3 * Fs; % 最小峰间距,300ms,对应最快步频约200步/分钟 MIN_PROMINENCE = 0.3; % 峰显著性,低于此值的峰视为噪声 [~, locs] = findpeaks(acc_vert_f, ... 'MinPeakHeight', MIN_PEAK_HEIGHT, ... 'MinPeakDistance', MIN_PEAK_DIST, ... 'MinPeakProminence', MIN_PROMINENCE);findpeaks的四个参数各有用途:MinPeakHeight排除幅度不足的抖动,MinPeakDistance保证两个峰之间至少间隔300毫秒——正常人类步频最快也就每分钟200步,再快就不可能是正常行走;MinPeakProminence排除那些虽然超过高度阈值但相对周围信号不够突出的毛刺,这是过滤伪峰最有效的一层。实际使用中,如果步数偏多,优先调高MIN_PROMINENCE和MIN_PEAK_HEIGHT;如果步数偏少,优先调低这两个参数。注意MIN_PEAK_DIST的单位是采样点数,不是秒,所以写成0.3 * Fs。
3.2 步长模型选型:Weinberg模型优先
检测到步伐之后,下一步是估算每一步走了多远。步长模型有很多种,常见的有恒定步长、步频线性模型、加速度方差模型,以及最常用的Weinberg模型。这套资源里默认用的是Weinberg模型,核心公式是:
步长L = k × (acc_peak - acc_valley)^(1/4)
其中acc_peak - acc_valley是这一步内竖直加速度波峰与波谷的差值,k是需要校准的系数。选这个模型的原因是它利用了每步的动力学特征,走得重的时候加速度摆幅大,步长自然大;走得轻的时候摆幅小,步长自然小。相比之下,恒定步长模型在速度变化时误差会线性放大,步频模型对慢走和快走的区分也不够灵敏。
% 计算每一步的波峰波谷差,生成步长特征序列 step_metric = zeros(length(locs), 1); for i = 1:length(locs) % 找到当前峰之前最近的谷值点 seg = acc_vert_f(1:locs(i)); [~, v_idx] = min(seg); delta = acc_vert_f(locs(i)) - acc_vert_f(v_idx); step_metric(i) = delta^(1/4); end这段代码里,每次找峰之前的最小值作为波谷,计算两者差值的1/4次方作为步长指标。实际项目中,更严谨的做法是在当前峰的前半个步态周期内找最小值,防止更早的其他谷值干扰。step_metric序列的物理意义是每一步的加速度摆幅特征,相同行走状态下这个值相对稳定,不同行走状态这个值差异明显。得到这个序列后,步长就是k乘以这个序列。
3.3 步长系数校准:用已知距离反推k
k值直接决定行走距离的刻度,不校准就用的后果是:走一百米,轨迹可能只有七十米或者一百三十米。常见做法是让被测者走一段已知距离,比如在走廊里量一条20米的直线,记录整个行走过程的IMU数据,算出这段路程中所有步的step_metric之和,然后反推k。
% 已知校准距离D0,单位米 D0 = 20; k = D0 / sum(step_metric); % 用标定后的k计算每一步步长 step_lengths = k * step_metric; fprintf('标定系数k = %.4f, 平均步长 = %.2f 米\n', k, mean(step_lengths));这段逻辑本质上是最小二乘意义上的单参数拟合:假设步长与步态特征严格成正比,用已知总距离除以特征总和得到一个比例系数。校准时的关键约束是行走节奏要自然,不能为了凑数据故意走得快或慢,否则k只对校准时的节奏有效。常见做法是分别校准慢走、正常走、快走三档,得到三组k值,实际使用时根据实时步频切换;如果只做验证性质的Demo,一档k值也能跑,只是误差会偏大。注意校准路线最好选择直线且地面平坦,尽量避免转弯——转弯时加速度计会引入额外离心分量,影响波峰波谷差。
4. 航向推算:互补滤波融合陀螺仪与磁力计,实际效果比选型更依赖参数
步数和步长决定你走了多远,航向决定你朝哪个方向走。航向一旦偏了,误差会随时间累积,直线行走的轨迹会慢慢弯掉。这一章的重点是互补滤波的实现细节与磁力计校准。
4.1 三种航向来源的特性对比
PDR里能用来定航向的信号有三种。第一种是陀螺仪积分,用的是垂直轴(Z轴)角速度对时间积分得到航向变化量,短时间精度高、响应快,但零偏会随积分时间累积,导致航向缓慢漂移。第二种是磁力计航向,通过磁场水平分量计算与北向的夹角,长期稳定但没有积分漂移,缺点是噪声大,而且室内钢筋、电气设备都会产生磁场干扰。第三种是加速度计,但它只能提供姿态参考,不能直接给出航向,通常用来辅助判断设备倾斜状态。
三种信号单独用都有硬伤,互补滤波的思路是用陀螺仪做短期预测,用磁力计做长期修正,两者按权重融合。权重系数alpha表示对陀螺仪的信任程度,一般取0.95到0.99之间。这个值取大了,航向响应平滑但漂移修正慢;取小了,磁力计噪声会直接污染航向,导致轨迹抖动。
4.2 互补滤波的MATLAB实现
alpha = 0.97; % 互补权重:0.97信任陀螺仪,0.03信任磁力计 yaw = zeros(length(t), 1); yaw(1) = atan2(mag(1, 2), mag(1, 1)); % 初始航向直接用磁力计 for i = 2:length(t) dt = t(i) - t(i-1); yaw_gyro = yaw(i-1) + gyro(i, 3) * dt; % 陀螺仪积分递推 yaw_mag = atan2(mag(i, 2), mag(i, 1)); % 磁力计瞬时航向 yaw(i) = alpha * yaw_gyro + (1 - alpha) * yaw_mag; end % 把航向从弧度转成角度并归一化到0~360度 yaw_deg = mod(yaw * 180 / pi, 360);这段代码是工程里最常见的互补滤波写法:每次迭代先用陀螺仪Z轴角速度乘以时间间隔,加到上一时刻航向上;再用磁力计X/Y分量计算瞬时航向角,与陀螺积分结果做加权平均。alpha决定两者的信任比例,0.97意味着97%依赖陀螺仪积分,3%用磁力计把长期漂移拉回来。按这个配置,短时间内的航向变化主要跟随陀螺仪,不会出现磁力计那种跳变;长时间行走时磁力计的微弱拉力又能抑制漂移。
4.3 磁力计硬铁校准:先转圈再算偏移
磁力计输出的X/Y分量如果存在固定偏置,算出来的航向会在某一侧整体偏移,这是室内行走轨迹整体偏向的常见原因。硬铁干扰来自设备附近的铁磁材料,表现为磁场测量值叠加一个固定偏移。校准方式是在同一位置让设备水平旋转一圈,记录X/Y分量的最大值和最小值,取中点作为偏移量。
% 硬铁校准:旋转一圈后取各轴最大最小值的均值作为偏移 offset_x = (max(mag(:, 1)) + min(mag(:, 1))) / 2; offset_y = (max(mag(:, 2)) + min(mag(:, 2))) / 2; % 校准后的磁力计 mag_cal(:, 1) = mag(:, 1) - offset_x; mag_cal(:, 2) = mag(:, 2) - offset_y; % 重新计算航向时使用校准后的磁力计 yaw_mag = atan2(mag_cal(:, 2), mag_cal(:, 1));这段代码的逻辑是把磁力计测量的X/Y散点中心拉回原点。校准前X/Y分量的散点图是一个偏离原点的圆,校准后圆心回到原点附近。注意校准要在开阔且远离铁磁物体的位置进行,如果在室内钢筋结构附近转圈,校准出的偏移量本身就带着环境干扰。软铁干扰通常用椭圆拟合处理,实现复杂一些,但原理类似——把散点椭圆拉回正圆。从工程角度,硬铁校准已经能解决大部分室内场景的航向整体偏移问题。
4.4 设备倾斜时的航向修正
上面互补滤波的代码隐含一个前提:设备水平放置,Z轴垂直,磁力计X/Y分量恰好反映水平磁场。但手机放在口袋里或拿在手里时,设备是倾斜的,直接用atan2(mag_y, mag_x)算出的航向会包含姿态误差。常见做法是用加速度计估算俯仰角和横滚角,把磁力计投影到水平面后再算航向。
% 利用加速度计计算俯仰角和横滚角 pitch = atan2(-acc_h, sqrt(acc_a^2 + acc_b^2)); roll = atan2(acc_a, acc_b);这里的pitch和roll分别表示设备绕Y轴和X轴的倾斜角,基于静止或慢速行走时加速度计主要感受重力的假设。算出这两个角之后,把磁力计三轴分量按罗德里格斯公式投影到水平坐标系,再取水平的X/Y分量算航向。严格做姿态融合一般会上卡尔曼滤波或姿态解算库,但PDR步行场景中,如果只是手持设备走路而不是剧烈晃动,互补滤波加简单倾斜补偿已经覆盖大多数情况。
5. 常见问题与排查:让跑起来的PDR调准的五条实测记录
这一章是实战里最容易出问题的地方。代码流程本身不难,难的是你以为跑通了,结果走一圈发现轨迹完全不对。下面五条是踩过坑的人大概率会遇到的,每一条都是现象、原因、解决一条线讲完。
5.1 步数检测多出一倍或漏掉一半
现象是轨迹总长度明显偏离实际,有时候走二十步,轨迹上却显示三十多步。
原因是竖直加速度的波峰波谷差阈值没匹配当前行走状态。走路轻的人加速度摆幅小,阈值设高了会漏检;手机在手里甩动产生的加速度,阈值设低了会误检。
解决方法是先用一段已知步数的短数据把MIN_PEAK_HEIGHT和MIN_PROMINENCE调好,记住一个原则:漏检优先降低MIN_PEAK_HEIGHT,误检优先提高MIN_PROMINENCE,同时把MIN_PEAK_DIST控制在0.25到0.35秒之间。我一般会打印出检出的峰位置,叠在滤波后的加速度波形上看一眼,确认每个峰都对齐在实际步行波峰上再批量处理数据。
5.2 直线行走轨迹持续向一侧弯曲
现象是明明走的是直线,画出来的轨迹却是一个大圆弧,走得越远越明显。
原因是陀螺仪存在零偏,也就是静止时Z轴角速度不严格为零。微分时间长了,零偏就会累积成持续的角度偏差。
解决方法是在行走前先采集两三秒静止数据,取Z轴角速度的平均值作为零偏,然后在积分前把每个时刻角速度减去这个零偏。如果陀螺仪零偏随温度漂移,最好在行走结束时再做一次静止测量,用前后两次零偏做线性插值补偿。
% 静止段零偏估计:取前2秒Z轴角速度均值 bias_z = mean(gyro(1:2*Fs, 3)); gyro_corrected = gyro(:, 3) - bias_z;5.3 初始航向不对导致整条轨迹旋转
现象是坐标轨迹的形状大致对,形状也完整,但整体旋转了几十度,朝东走的轨迹看起来像朝北走。
原因是初始航向直接取了atan2(mag_y, mag_x),没有修正磁偏角。地球磁北极和地理北极本身就不重合,不同城市的磁偏角从几度到十几度都有。
解决方法是查一下当地磁偏角,在初始航向和每次磁力计航向计算后减去磁偏角。另一个常见做法是进门时已知行走方向,直接把初始航向设成那个角度,后续只依赖航向变化量,这样磁场绝对角度误差就被绕过去了。
5.4 滤波后步峰被抹平,步长普遍偏短
现象是步数检测没大问题,但计算出的总距离明显小于实际距离,每步步长都在0.4米以下。
原因是低通滤波器截止频率设太低,把步态信号的波峰和波谷一起削平了。二阶巴特沃斯在截止频率附近有衰减,如果截止频率设在3Hz,最接近它的二次谐波也被压制,波峰幅度就变小了。
解决方法是把截止频率从3Hz提高到5到6Hz,同时检查滤波前后波形峰值的衰减幅度。如果峰值缩小超过10%,说明滤波过重。用filtfilt零相位滤波已经避免了一部分相位问题,但幅度衰减依然存在,调整截止频率是唯一直接的办法。
5.5 同一套步长参数换个人走误差明显变大
现象是某开发者用标定好的k值测试正常,另外一个人走同样距离,误差超过百分之十。
原因是Weinberg模型的k值对个体差异敏感,身高、步幅、走路习惯都会影响第一步的加速度摆幅特征。体重较轻的人落地冲击小,波峰波谷差值小,同样的k算出的步长偏小。
解决方法是在正式测试前,对每个使用者单独做一次步长校准。如果不想每个使用者都跑一遍校准流程,可以按快走、慢走、正常走三档分别标定步长系数,再按实时步频切换。做多层人员测试时,以相对误差而不是绝对误差来评估效果更科学。
6. 落地前的最后一步:用闭环轨迹验证误差,把残余误差按步数摊回去
PDR代码跑通只是开始,离能用还有一段距离。在交付之前,通常要做一次闭环验证:让被测者沿着一条已知路径走回起点,比如矩形或回字形路线,然后把终点坐标与起点坐标的距离误差除以总距离,得到相对定位误差。这个数字在百分之二以内才可以说定位结果可用,超过百分之五就要回头查航向或者步长参数。
% 闭环验证:终点与起点的直线距离除以总路程 pos_err = norm(pos(end, :) - pos(1, :)); total_dist = sum(step_lengths); relative_err = pos_err / total_dist * 100; fprintf('相对定位误差: %.2f%%\n', relative_err);这段代码本身不复杂,但它应该放在所有调试工作的最后一步做,因为误差数字是衡量前面每一步参数是否合适的唯一标准。如果相对误差偏大,需要区分主要误差来源:总距离偏差大优先查步长;起点终点连线方向偏差大优先查航向。闭环路线的好处是能够把距离误差和方向误差拆开看。
闭环验证还有一个实用延伸:如果确认终点位置已知,而轨迹没有闭合,可以把终点的残余位移差按步数线性分摊到每一步,得到一个校正后的轨迹。这是一种朴素的闭环修正,远不如卡尔曼平滑优雅,但胜在直观、容易实现,而且能把绝对误差控制在可接受范围。
% 残余误差线性分配:把终点偏差按步数分摊回去 correction = (pos(1, :) - pos(end, :)) / length(step_lengths); for i = 1:length(step_lengths) pos(i, :) = pos(i, :) + correction * i; end逻辑是假设误差随步数匀速累积,那么每一步应该补偿一个线性递增的修正量,走到终点时恰好回到起点。放在PDR项目最后使用,适合那些只需要一个平滑轨迹、不需要严格实时性的离线分析场景。从那以后我每次拆这类PDR项目,都会先把校准和闭环验证排在前面,而不是等轨迹走偏了再拿着尺子去量。希望帮到你。
本文还有配套的精品资源，点击获取
