【MATLAB程序】CV和CA模型组成的IMM（交互式多模型），基于粒子滤波PF，背景为三维目标跟踪定位。附源代码

三维交互式多模型粒子滤波器（IMM-PF），其中包含两个运动模型：匀速模型（CV） 与 匀加速模型（CA）
代码包运行成功，不包免费讲解。原创程序，请勿翻卖

背景介绍

算法简介

本代码实现了一套三维交互式多模型粒子滤波器（IMM-PF），专门用于对机动目标的高精度轨迹跟踪。算法融合了匀速模型（CV）与匀加速模型（CA），能够自动识别目标的运动状态（巡航、加速、转弯等），并实时切换最优模型进行估计。

相比于单一模型的滤波方法（如单独使用 CV 或 CA），IMM-PF 在机动段误差降低 30% 以上，在平稳段也能保持出色的跟踪精度。

核心亮点

亮点	说明
自适应模型切换	通过模型概率实时计算，自动判断当前运动属于巡航还是机动，无需人工干预
粒子滤波框架	不受高斯线性假设限制，能处理强非线性、非高斯噪声场景
三维空间支持	完整实现 X/Y/Z 三轴位移、速度、加速度（CA 模型）的同步估计
交互式混合（IMM）	在模型切换时进行粒子混合，避免模式切换瞬间的估计跳变
完整可运行代码	附带模拟轨迹生成、噪声设置、结果绘图与性能统计，即拿即用

仿真效果展示

代码内置一套典型的三段式机动轨迹：

• 第 1~120 步：匀速直线运动
• 第 121~220 步：缓慢加速转弯（机动段）
• 第 221~300 步：恢复匀速运动

输出结果包括：

1. 三维轨迹对比图 —— 直观比较 IMM-PF、单独 CV、单独 CA 与真实轨迹的拟合程度
2. 位置误差曲线 —— 逐时刻展示各方法的跟踪误差
3. 模型概率曲线 —— 清晰显示算法对 CV/CA 模式的在线识别过程
4. 三轴位移分时对比图 —— 从 X/Y/Z 三个维度分别查看估计精度

运行结束后，命令行窗口会打印 RMSE（均方根误差） 与 平均误差 统计结果，量化对比三种方法的性能。

性能对比结论

方法	RMSE（位置误差）	机动段跟踪能力	平稳段精度
IMM-PF	最低	优秀（自适应切换）	优秀
单独 CV	中等	较差（滞后明显）	优秀
单独 CA	较高	较好	一般（噪声放大）

实验结果表明：IMM-PF 完美平衡了机动响应速度与稳态精度，是复杂机动场景下的首选方案。

运行结果

轨迹：

各轴状态曲线：

各轴状态误差曲线：

命令行窗口输出的误差统计特性：

MATLAB源代码

完整代码如下：

% CV和CA模型组成的IMM,PF，三维
% 讲解、定制，可联系V：matlabfilter
% 2026-04-09/Ver1

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

%% 参数
T = 1;
N = 300;
Np = 10000;
resample_thresh = 0.9;
% 噪声强度
sigma_a = 0.01;   % CV: 加速度噪声
sigma_j = 0.05;   % CA: jerk噪声
R = 0.01*diag([1,1,1]); % 测量噪声
% 模型转移概率
PI = [0.99 0.01;
      0.01 0.99];
mu = [0.5;0.5];
%% 真实轨迹

...

完整代码：
https://download.youkuaiyun.com/download/callmeup/92802650

如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者