C V CV CV和 C A CA CA双模型，二维，基于粒子滤波的 I M M IMM IMM（交互式多模型）
原创代码，包运行成功，可联系我获取讲解和一对一定制，请勿翻卖
文章目录
代码介绍
算法简介
本程序为IMM-PF（交互多模型粒子滤波） 。其核心思想是不再依赖单一运动模型，而是同时引入**匀速（CV）和匀加速（CA）**两种模型，对目标运动进行并行建模与估计。
在每个时刻，算法根据观测数据自动评估两种模型的可信度，并进行加权融合，从而实现对不同运动状态的自适应切换。
核心特点
- 多模型协同：同时跟踪多种运动模式
- 自适应切换：根据数据自动调整模型权重
- 粒子滤波驱动：适用于非线性、非高斯场景
- 统一融合输出：保证轨迹平滑且稳定
效果总结
相比单一模型方法，该算法在运动状态发生变化（如加速、减速）时表现更加稳定，整体跟踪精度更高，是一种兼顾灵活性与鲁棒性的目标跟踪方案。
运行结果
运行代码后，即可得到下面的结果：
运动轨迹示意图：
各轴估计的误差曲线：
各轴RMSE：
各模型概率曲线：
与实际仿真的运动模型更替相匹配：
命令行截图：
MATLAB源代码
部分代码如下：
matlab
% CV和CA模型组成的IMM,PF，二维
% 2026-03-29/Ver1
clear; clc; close all;
rng(0); % 固定随机种子
%% 建模与初始化
N = 600; % 仿真时间步
T = 1; % 采样间隔
x0 = [100;1;0;100;1;0]; % 初始状态 [x位置, x速度, x加速度, y位置, y速度, y加速度]
xA = []; % 真实状态存储
%% 粒子滤波参数
Np = 5000; % 粒子数量
resample_thresh = 0.9; % 重采样阈值（有效粒子数 < Np*此值时重采样）
%% 模型定义
% CV 匀速运动模型（4维状态：位置+速度）
Q1 = 4 * diag([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)];
% CA 匀加速运动模型（6维状态：位置+速度+加速度）
Q2 = 4 * diag([1,1]);
fCA = @(x, w) [x(1) + T*x(2) + T^2/2*x(3) + T^3/6*w(1);
x(2) + T*x(3) + T^2/2*w(1);
x(3) + T*w(1);
x(4) + T*x(5) + T^2/2*x(6) + T^3/6*w(2);
x(5) + T*x(6) + T^2/2*w(2);
x(6) + T*w(2)];
%% 生成真实轨迹
x = x0;
for k = 1:150 % 匀速直线段
w = chol(Q1,'lower') * randn(2,1);
x_cv = fCV(x([1,2,4,5]), w); % 使用CV模型
x([1,2,4,5]) = x_cv;
x([3,6]) = 0; % 加速度为0
xA = [xA, x];
end
for k = 1:120 % 匀加速段（设置恒定加速度）
if k == 1
x(3) = 0.1; % x方向加速度 0.5 m/s²
x(6) = 0.2; % y方向加速度 0.3 m/s²
end
w = chol(Q2,'lower') * randn(2,1);
x = fCA(x, w);
xA = [xA, x];
end
for k = 1:N-270 % 匀速直线段
w = chol(Q1,'lower') * randn(2,1);
x_cv = fCV(x([1,2,4,5]), w);
x([1,2,4,5]) = x_cv;
x([3,6]) = 0; % 加速度回到0
xA = [xA, x];
end
%% 观测参数
H_cv = [1,0,0,0; 0,0,1,0]; % CV模型观测矩阵（仅观测位置）
H_ca = [1,0,0,0,0,0; 0,0,0,1,0,0]; % CA模型观测矩阵（仅观测位置）
R = 10 * diag([1,1]);
t = 1:N;
%% 蒙特卡罗代码下载链接：
https://download.csdn.net/download/callmeup/92769614
如需帮助，或有导航、定位滤波相关的代码定制需求，可点击下方卡片联系作者