【改进PDR，MATLAB例程(6)】基于PDR与运动模型的三维IMM-EKF行人导航融合定位，附完整的代码下载链接

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

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

2 ·

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

GEO检测

收录于

定位应用

话题

#CodingPlan·八月创作之星博客挑战赛

在这里插入图片描述

原创代码，包运行成功。仅供研究与讨论，禁止翻卖

面向三维行人导航中的多运动模式估计问题，将带漂移的PDR位置观测与IMM-EKF滤波结合，可以直观看到纯PDR轨迹、融合轨迹、误差曲线和模型概率的变化。代码采用单个 m 文件组织，适合用于课程实验、仿真、滤波算法改造和行人定位方案验证。

运行结果

轨迹对比图：对比真实轨迹、PDR观测轨迹和IMM-EKF融合轨迹，展示三维定位修正效果。

在这里插入图片描述

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

在这里插入图片描述

MATLAB源代码

部分代码如下：

%% 三维行人导航IMM-EKF
% 作者：matlabfilter（V同号），接定位与导航、滤波相关的matlab代码定制
% 2026-07-24/Ver1
% 单文件例程：三维多运动模式行人运动、含噪PDR位置观测和IMM-EKF滤波。

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

cfg.dim = 3;
cfg.dt = 0.62;
cfg.nStep = 180;
cfg.modelNames = {'停止', '匀速-CV', '加速', '转弯', '机动'};
cfg.mapLimit = [-8 108 -28 70 -2 14];

[truth, pdrObs] = simulateMultiModePedestrian(cfg);
est = runImmPositionFilter(pdrObs.pos, cfg);
errPdr = pointError(pdrObs.pos, truth.pos);
errEst = pointError(est.pos, truth.pos);
printStats('PDR_IMMEKF_3Dim', {'PDR观测', 'IMM-EKF'}, {errPdr, errEst});
plotTrajectory({truth.pos, pdrObs.pos, est.pos}, {'真实轨迹', 'PDR观测', 'IMM-EKF'}, cfg, 'PDR_IMMEKF_3Dim');
plotErrors({errPdr, errEst}, {'PDR观测', 'IMM-EKF'}, 'PDR_IMMEKF_3Dim');
plotModelProbabilities(est.modelProb, truth.mode, cfg.modelNames, 'PDR_IMMEKF_3Dim');

%% 本地函数
function [truth, pdrObs] = simulateMultiModePedestrian(cfg)
N = cfg.nStep; dim = cfg.dim; dt = cfg.dt;
state = zeros(2 * dim, N);
mode = zeros(1, N);
state(:, 1) = [0; 0; 1.2; 1.02; 0.16; 0.04];
for k = 2:N
    if k < 38
        mode(k) = 2;
    elseif k < 70
        mode(k) = 3;
    elseif k < 108
        mode(k) = 4;
    elseif k < 126
        mode(k) = 1;
    elseif k < 152
        mode(k) = 5;
    else
        mode(k) = 2;
    end
    state(:, k) = trueModeStep(state(:, k - 1), mode(k), dt, k, dim);
end
mode(1) = mode(2);
pos = state(1:dim, :);
drift = zeros(dim, N);
z = zeros(dim, N);
for k = 2:N
    baseDrift = [0.010; -0.004; 0.003];
    drift(:, k) = drift(:, k - 1) + baseDrift(1:dim) + 0.012 * randn(dim, 1);
end
for k = 1:N
    sigma = 0.42 + 0.15 * (mode(k) == 5) + 0.10 * (mode(k) == 4);
    z(:, k) = pos(:, k) + drift(:, k) + sigma * randn(dim, 1);
end
truth.state = state;
truth.pos = pos;
truth.mode = mode;
pdrObs.pos = z;
end