原创代码，包运行成功。如需讲解、定制相关代码，可联系我
文章目录
程序详解
本程序先仿真IMU加速度和陀螺仪数据，再通过峰值检测提取步态事件，最后由步长和航向增量递推PDR轨迹。稀疏位置观测用于EKF校正。整套程序为单个 .m 文件，不依赖 工具箱，在MATLAB中打开即可直接运行。
状态与量测
例程采用二维平面/空间状态建模，PDR融合状态可概括为 [x, y, 航向角, 步长比例, 航向偏置]。量测包括检测步态构造的PDR输入和间隔出现的稀疏位置观测，适合展示PDR漂移被外部定位修正的过程。
滤波流程
程序首先生成真实轨迹和带噪观测，然后按步执行预测、量测更新和误差统计。EKF分支通过数值雅可比线性化非线性状态转移和量测方程，适合对比传统扩展卡尔曼滤波在PDR融合中的表现。所有曲线和命令行统计均由脚本运行后直接生成。
运行结果
-
轨迹对比图：对比真实轨迹、PDR观测轨迹和EKF融合轨迹，展示二维定位修正效果。
-
步态检测图：展示仿真IMU加速度模值和检测到的步态时刻，用于验证步态检测结果是否合理。
-
定位误差曲线：给出各方法随步数变化的位置误差，用于观察PDR累积漂移和滤波校正效果。
-
命令行输出：命令行窗口输出各方法的均值、中位数、标准差和RMSE，便于直接进行数值对比。
MATLAB源代码
完整代码如下：
matlab
%% PDR步态检测与EKF二维融合
% 作者：matlabfilter（V 同号），接定位与导航、滤波相关的matlab代码定制
% 2026-07-24/Ver1
% 2026-07-29/Ver2 修复问题
% 2026-07-29/Ver3将航向偏置改为陀螺角速度偏置，使用步态周期进行补偿；
clc; clear; close all;
rng(0);
cfg.dim = 2;
cfg.dt = 0.62;
cfg.fs = 50;
cfg.nStep = 174;
cfg.mapLimit = [-8 108 -28 70];
cfg.fixInterval = 8;
cfg.fixSigma = 0.82;
[imu, truth] = simulatePedestrianImu(cfg);
[stepTime, stepAmp] = detectStepsFromAcc(imu.t, imu.accNorm, cfg.fs);
[pdr, matchedTruth] = buildPdrFromDetectedSteps(imu, truth, stepTime, stepAmp);
zFix = simulateSparsePositionFix( ...
matchedTruth.pos, cfg.fixInterval, cfg.fixSigma);
est = runPdrPositionFilter( ...
pdr.stepLength, ...
pdr.yawDelta, ...
pdr.stepPeriod, ...
zFix, ...
matchedTruth.pos(:, 1), ...
pdr.yaw(1), ...
cfg);
errPdr = pointError(pdr.pos, matchedTruth.pos);
errFix = pointErrorWithNan(zFix, matchedTruth.pos);
errEst = pointError(est.pos, matchedTruth.pos);
% 原表保留：PDR和EKF统计全部步态，稀疏定位只统计有效观测时刻
printStats( ...
'PDR步态检测EKF', ...
{'PDR', '稀疏定位', 'EKF'}, ...
{errPdr, errFix, errEst});
% 公平比较：仅在稀疏定位实际存在的时刻比较定位观测和EKF
fixMask = all(isfinite(zFix), 1);
errEstAtFix = nan(size(errEst));
errEstAtFix(fixMask) = errEst(fixMask);
printStats( ...
'相同稀疏观测时刻对比', ...
{'稀疏定位', 'EKF'}, ...
{errFix, errEstAtFix});
plotStepDetection( ...
imu.t, imu.accNorm, stepTime, ...
'PDR步态检测EKF');
plotTrajectory( ...
{matchedTruth.pos, pdr.pos, zFix, est.pos}, ...
{'真实轨迹', 'PDR', '稀疏定位', 'EKF'}, ...
cfg, ...
'PDR步态检测EKF');
plotErrors( ...
{errPdr, errFix, errEst}, ...
{'PDR', '稀疏定位', 'EKF'}, ...
'PDR步态检测EKF');
%% 本地函数
function [imu, truth] = simulatePedestrianImu(cfg)
dim = cfg.dim;
fs = cfg.fs;
N = cfg.nStep;
stepTime = zeros(1, N);
stepLen = zeros(1, N);
yaw = zeros(1, N);
pos = zeros(dim, N);
period = cfg.dt * ones(1, N);
verticalDelta = zeros(1, N);
pos(1:2, 1) = [0; 0];
yaw(1) = 0.10;
stepTime(1) = 0.65;
for k = 2:N
if k < 42
dYaw = 0.004 * sin(k / 9);
stepLen(k) = 0.72 + 0.015 * randn;
elseif k < 78
dYaw = 0.032 + 0.006 * randn;
stepLen(k) = 0.69 + 0.018 * randn;
elseif k < 112
dYaw = -0.004 + 0.006 * randn;
stepLen(k) = 0.79 + 0.018 * randn;
period(k) = 0.55;
elseif k < 138
dYaw = -0.012 + 0.005 * randn;
stepLen(k) = 0.61 + 0.018 * randn;
period(k) = 0.70;
else
dYaw = -0.030 + 0.007 * randn;
stepLen(k) = 0.68 + 0.016 * randn;
end
yaw(k) = wrapToPiLocal(yaw(k - 1) + dYaw);
pos(1:2, k) = pos(1:2, k - 1) ...
+ stepLen(k) * [cos(yaw(k)); sin(yaw(k))];
stepTime(k) = stepTime(k - 1) ...
+ period(k) + 0.025 * randn;
end
stepLen(1) = stepLen(2);
verticalDelta(1) = verticalDelta(2);
t = 0:1 / fs:(stepTime(end) + 0.8);
accNorm = 9.81 + 0.05 * randn(size(t));
gyroZ = 0.002 * randn(size(t));
for k = 1:N
tau = t - stepTime(k);
accNorm = accNorm ...
+ (1.00 + 0.12 * randn) .* exp(-(tau / 0.055).^2) ...
- 0.34 .* exp(-((tau - 0.10) / 0.075).^2);
end
for k = 2:N
idx = t >= stepTime(k - 1) & t < stepTime(k);
dtStep = max(stepTime(k) - stepTime(k - 1), 1 / fs);
gyroZ(idx) = ...
wrapToPiLocal(yaw(k) - yaw(k - 1)) / dtStep ...
+ 0.0025 ...
+ 0.006 * randn(1, sum(idx));
end完整代码下载链接：
https://download.csdn.net/download/callmeup/93199398
或前往专栏查看更多代码和介绍：https://blog.csdn.net/callmeup/category_13193577.html
如需帮助，或有导航、定位滤波相关的代码定制需求，可从个人主页左侧联系我