【PDR，MATLAB例程】(4) 二维平面下，行人航位推算（PDR）与无线测距融合，IMM-EKF融合。附MATLAB例程下载链接

最新推荐文章于 2026-08-05 21:27:02 发布

原创最新推荐文章于 2026-08-05 21:27:02 发布 · 368 阅读

3 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

收录于

定位应用

在这里插入图片描述

原创代码，请勿翻卖。有注释，包运行成功

文章目录

运行结果
MATLAB源代码

例程面向室内无线定位与PDR融合场景，使用锚点测距提供外部约束，并通过IMM-EKF处理行人运动状态变化。程序同时输出纯PDR、无线定位和融合定位结果，适合学习UWB/蓝牙测距、PDR组合导航和非线性滤波融合框架。

运行结果

直接运行后，程序会完成以下流程：生成带步长和航向误差的PDR轨迹、锚点无线测距观测，并使用IMM-EKF完成PDR/无线融合定位。本例程为二维行人导航仿真，便于后续改造成其他方法和环境。

轨迹对比图：真实轨迹、纯PDR、无线定位、IMM-EKF融合轨迹以及锚点位置。
定位误差曲线：各方法随步数变化的位置误差，用于观察PDR累积漂移和滤波校正效果。
模型概率曲线：展示IMM中不同运动模型概率随步数变化，以及主导模型和真实模式之间的对应关系。
命令行输出：输出各方法的均值、中位数、标准差和RMSE，便于直接进行数值对比。

MATLAB源代码

部分代码如下：

%% 二维无线/PDR IMM-EKF融合
% 作者：matlabfilter（V同号），接定位与导航、滤波相关的matlab代码定制
% 2026-07-24/Ver1
% 单文件例程：PDR步长航向输入、锚点测距观测和二维IMM-EKF融合。

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

algorithmName = 'PDR无线融合IMMEKF_2Dim';

cfg.dim = 2;
cfg.dt = 0.62;
cfg.nStep = 174 + 6 * (cfg.dim == 3);
cfg.modelNames = {'停止', '匀速-CV', '加速', '转弯', '机动'};
cfg.mapLimit = [-8 108 -28 70];
cfg.anchors = [-5 -20; 28 -24; 75 -16; 104 24; 65 62; -2 48];

[truth, pdr, wireless] = simulateWirelessPdrScenario(cfg);
est = runWirelessPdrImmFilter(pdr.stepLength, pdr.yawDelta, pdr.verticalDelta, wireless.ranges, cfg.anchors, truth.pos(:, 1), cfg);
errPdr = pointError(pdr.pos, truth.pos);
errWireless = pointError(wireless.pos, truth.pos);
errEst = pointError(est.pos, truth.pos);
printStats(algorithmName, {'PDR', '无线定位', 'IMM-EKF'}, {errPdr, errWireless, errEst});
plotTrajectory({truth.pos, pdr.pos, wireless.pos, est.pos}, {'真实轨迹', 'PDR', '无线定位', 'IMM-EKF'}, cfg, algorithmName, cfg.anchors);
plotErrors({errPdr, errWireless, errEst}, {'PDR', '无线定位', 'IMM-EKF'}, algorithmName);
plotModelProbabilities(est.modelProb, truth.mode, cfg.modelNames, algorithmName);

%% 本地函数