基于交互多模型（Interacting Multiple Model, IMM）与粒子滤波（Particle Filter, PF）融合的三维目标跟踪MATLAB代码，用于目标在多种运动模式下的高精度位置估计。建立匀速运动模型（Constant Velocity, CV）与匀速转弯模型（Constant Turn, CT），并利用PF和IMM对两种模型进行滤波、概率融合，从而实现对机动目标轨迹的自适应跟踪。
原创代码，仅供研究和讨论，请勿翻卖
文章目录
程序简介
核心应用场景与背景
该代码模拟目标在三维空间中的运动，传感器仅观测其三维位置（X, Y, Z坐标）。目标在运动过程中改变运动模式（从"直线匀速"变为"圆周机动"，再变回"直线匀速"），此时单一的模型无法准确跟踪，引入IMM机制来切换和融合不同的运动模型，引入了 粒子滤波（PF） 来处理非线性状态估计。
核心状态与模型定义
系统将目标的三维状态定义为6维向量： x = [ x , v x , y , v y , z , v z ] T x = [x, v_x, y, v_y, z, v_z]^T x=[x,vx,y,vy,z,vz]T （即各轴的位置和速度）。
代码中定义了两个运动模型：
- CV模型（匀速运动模型，Constant Velocity）： 假设目标在X、Y、Z三个方向上都做匀速直线运动，带有一定的过程噪声。
- CT模型（匀速圆周运动模型，Constant Turn）： 假设目标在 XY水平面内 以固定的角速度
w_ct = 0.1做圆周转弯机动
仿真场景设计
代码生成了一段总长度为 N = 600 步的三维真实轨迹，分为三个阶段：
- 目标按照 CV模型 做三维匀速直线运动。
- 目标按照 CT模型 机动，在XY平面盘旋，Z轴继续匀速。
- 目标恢复为 CV模型 做匀速直线运动。
算法核心流程 (IMM-PF)
代码采用了 MC = 10 次的蒙特卡洛仿真来消除随机性造成的误差，单次仿真的主循环严格遵循了 IMM-PF 的四大步骤：
- 交互/混合 (Interaction/Mixing)：
根据马尔可夫状态转移概率矩阵Pi以及上一时刻的模型概率，计算混合概率。调用mix_particles函数，将 CV 和 CT 两个滤波器的粒子群进行交叉混合。加入了一点位置和速度的高斯抖动 (Jitter)，以防止粒子多样性丧失 - 独立粒子滤波预测与更新 (PF Predict & Update)：
- 预测： 根据各自的动力学方程（
fCV,fCT）推动粒子向前演化。 - 更新： 将传感器传来的真实观测值（带观测噪声）与粒子的预测位置进行对比，计算似然值
- 重采样 (Resample)： 通过判断有效粒子数 N e f f N_{eff} Neff，自适应触发系统重采样（Systematic Resampling），剔除权重过小的粒子，复制权重大的粒子
- 模型概率更新 (Model Probability Update)：
根据两个 PF 滤波器返回的"平均似然度"，结合马尔可夫转移概率，更新当前时刻目标处于 CV 或 CT 模型的概率。使用 Log-sum-exp 技巧 和平滑滤波 - 状态估计融合 (State Combination)：
将 CV 滤波器和 CT 滤波器得出的状态估计结果，根据各自的模型概率进行加权求和，得到最终的 IMM 最优输出xk。
数据统计与可视化图表
仿真结束后，代码会自动计算 RMSE（均方根误差）并输出详细的对比报告。主要会生成以下四幅核心图表：
- 三维轨迹对比图 & 三视图（XY/XZ/YZ投影）： 直观展示真实轨迹、带噪声的观测点、以及单一 CV-PF、单一 CT-PF 和融合 IMM-PF 的跟踪轨迹。
- 滤波误差对比与CDF图： 展示三个方向上的误差随时间的变化，以及误差的累积分布函数（评估误差落在某范围内的概率）。
- RMSE 均方根误差曲线： 量化对比不同算法在各个时间步的跟踪精度。
- 模型概率变化图
运行结果
轨迹跟踪效果展示：
各模型概率准确反映了目标在 151 步进入机动、271 步退出机动的状态切换：
滤波误差 证明了 IMM-PF 的平均误差低于单一模型（CV 或 CT），性能提升：
10次蒙特卡洛后的RMSE曲线：
MATLAB源代码
部分代码展示：
matlab
% CV和CT模型组成的IMM,PF，三维
% 作者：matlabfilter
% 2026-03-09/Ver1
clear; clc; close all;
% rng(1); % 固定随机种子
N = 600; % 仿真时间步
T = 1; % 采样间隔
x0 = [1000;10;1000;10;500;5]; % 初始状态 [x位置, x速度, y位置, y速度, z位置, z速度]
xA = []; % 真实状态存储
%% 粒子滤波参数
Np = 2000; % 粒子数量
resample_thresh = 0.5; % 重采样阈值
%% 模型定义
% CV 匀速运动模型（三维）
Q1 = 0.01 * diag([1,1,1]); % 过程噪声
fCV = @(x, w) [x(1) + T*x(2) + T^2/2*w(1);
x(2) + T*w(1);
x(3) + T*x(4) + T^2/2*w(2);
x(4) + T*w(2);
x(5) + T*x(6) + T^2/2*w(3);
x(6) + T*w(3)];
% CT 匀速圆周运动模型（三维：在XY平面转动，Z轴匀速）
w_ct = 0.1; % 角速度
Q2 = 0.01 * diag([1,1,1]); % CT过程噪声
fCT = @(x, w) [x(1) + sin(w_ct*T)/w_ct*x(2) - (1-cos(w_ct*T))/w_ct*x(4) + T^2/2*w(1);
cos(w_ct*T)*x(2) - sin(w_ct*T)*x(4) + T*w(1);
(1-cos(w_ct*T))/w_ct*x(2) + x(3) + sin(w_ct*T)/w_ct*x(4) + T^2/2*w(2);
sin(w_ct*T)*x(2) + cos(w_ct*T)*x(4) + T*w(2);
x(5) + T*x(6) + T^2/2*w(3);
x(6) + T*w(3)];
fprintf('\n====总体位置RMSE对比（三维）====\n');
fprintf('综合X/Y/Z三维的位置误差\n\n');
fprintf('%-12s %10s\n', '方法', '位置RMSE(m)');
fprintf('%s\n', repmat('-', 1, 26));
fprintf('%-12s %10.4f\n', 'CV-PF',pos_RMSE_cv);
fprintf('%-12s %10.4f\n', 'CT-PF',pos_RMSE_ct);
fprintf('%-12s %10.4f\n', 'IMM-PF', pos_RMSE_imm);
fprintf('%s\n', repmat('-', 1, 26));
% 计算IMM相对于CV和CT的改善百分比
imp_cv= (pos_RMSE_cv- pos_RMSE_imm) / pos_RMSE_cv* 100;
imp_ct= (pos_RMSE_ct- pos_RMSE_imm) / pos_RMSE_ct* 100;
fprintf('IMM-PF相比CV-PF 精度改善：%.2f%%\n', imp_cv);
fprintf('IMM-PF相比CT-PF 精度改善：%.2f%%\n', imp_ct);
fprintf('\n====分段性能分析====\n');
fprintf('第1段(1-150步,真实CV): CV=%.2f, CT=%.2f, IMM=%.2f\n', ...
pos_RMSE_cv_seg1, pos_RMSE_ct_seg1, pos_RMSE_imm_seg1);
fprintf('第2段(151-270步,真实CT): CV=%.2f, CT=%.2f, IMM=%.2f\n', ...
pos_RMSE_cv_seg2, pos_RMSE_ct_seg2, pos_RMSE_imm_seg2);
fprintf('第3段(271-600步,真实CV): CV=%.2f, CT=%.2f, IMM=%.2f\n', ...
pos_RMSE_cv_seg3, pos_RMSE_ct_seg3, pos_RMSE_imm_seg3);
fprintf('\n平均模型概率: CV=%.3f, CT=%.3f\n', mean(Um1), mean(Um2));
fprintf('第1段平均模型概率: CV=%.3f, CT=%.3f\n', mean(Um1(seg1)), mean(Um2(seg1)));
fprintf('第2段平均模型概率: CV=%.3f, CT=%.3f\n', mean(Um1(seg2)), mean(Um2(seg2)));
fprintf('第3段平均模型概率: CV=%.3f, CT=%.3f\n', mean(Um1(seg3)), mean(Um2(seg3)));完整代码（包运行成功）：https://download.csdn.net/download/callmeup/92729216
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者