【PDR2，MATLAB例程】用于IMU、步态检测、PDR融合的航位推算，适用于二维平面，融合方法为UKF。附完整代码的下载链接

完整代码，含中文注释

程序详解

例程功能

本例程面向二维行人导航，先用仿真 IMU 数据生成步态冲击峰，再通过阈值和最小步间隔完成步态检测。检测得到的步时刻和步态幅值用于构建 PDR 递推，随后使用稀疏位置修正量进入 UKF。程序结构集中在一个 .m 文件内，包含仿真、检测、滤波、误差统计和绘图函数。

纯 PDR 轨迹会随步数增加逐渐漂移；稀疏位置观测存在米级噪声且不是连续约束；UKF 将两者融合后得到连续平滑轨迹，并在本次调参后取得最低 RMSE。误差曲线和命令行 RMSE 表可以用于快速判断滤波器是否正确收敛。

运行结果

运行代码后，程序会完成以下流程：模拟 IMU 加速度与陀螺仪数据、检测步态峰值、构建 PDR 轨迹，并利用稀疏位置观测进行 UKF 校正。本例程为二维平面行人导航仿真，不考虑上下坡或三维高度变化，也不依赖 PSINS 工具箱。

• 步态检测结果：
  展示加速度模值中的步态峰值检测结果，红色标记对应被识别出的步态事件。
  

• 二维轨迹对比：对比真实轨迹、纯 PDR 轨迹、稀疏位置观测和 UKF 融合轨迹。

• 定位误差曲线：该图给出各方法随步数变化的平面位置误差，用于观察累积漂移和滤波修正效果。
  

• 命令行窗口会输出各方法的均值、中位数、标准差和 RMSE，便于直接对比 PDR、观测量和融合结果：

MATLAB源代码

部分代码如下：

%% PDR步态检测与UKF二维融合
% 作者：matlabfilter（V同号），接定位与导航、滤波相关的matlab代码定制
% 2026-07-24/Ver1
% 单文件例程：仿真IMU、步态检测、PDR航位推算和二维UKF校正。

clc; clear; close all;
rng(0);

algorithmName = 'PDR步态检测UKF_2Dim';

cfg.fs = 50;                  % IMU sample rate, Hz
cfg.nStep = 168;              % true walking steps
cfg.nominalPeriod = 0.62;     % nominal step period, s
cfg.mapLimit = [-5 95 -20 55];

[imu, truth] = simulatePedestrianImu(cfg);
[stepTime, stepAmp] = detectStepsFromAcc(imu.t, imu.accNorm, cfg.fs);
[pdr, matchedTruth] = buildPdrFromDetectedSteps(imu, truth, stepTime, stepAmp);

zFix = simulateSparsePositionFix(matchedTruth.pos, 8, 1.15);
ukf = runStepPdrUkf(pdr.stepLength, pdr.yawDelta, zFix, matchedTruth.pos(:, 1));

errPdr = pointError(pdr.pos, matchedTruth.pos);
errFix = pointErrorWithNan(zFix, matchedTruth.pos);
errUkf = pointError(ukf.pos, matchedTruth.pos);

printStats(algorithmName, errPdr, errFix, errUkf);
plotStepDetection(imu.t, imu.accNorm, stepTime, algorithmName);
plotTrajectory(matchedTruth.pos, pdr.pos, zFix, ukf.pos, cfg.mapLimit, algorithmName);
plotErrors(errPdr, errFix, errUkf, algorithmName);

%% Local functions

完整代码和函数：
https://download.csdn.net/download/callmeup/93180247

如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我