$CV$和$CA$双模型，二维，基于粒子滤波的$IMM$（交互式多模型）
原创代码，包运行成功，可联系我获取讲解和一对一定制，请勿翻卖

代码介绍

算法简介

本程序为IMM-PF（交互多模型粒子滤波）。其核心思想是不再依赖单一运动模型，而是同时引入**匀速（CV）和匀加速（CA）**两种模型，对目标运动进行并行建模与估计。

在每个时刻，算法根据观测数据自动评估两种模型的可信度，并进行加权融合，从而实现对不同运动状态的自适应切换。

核心特点

• 多模型协同：同时跟踪多种运动模式
• 自适应切换：根据数据自动调整模型权重
• 粒子滤波驱动：适用于非线性、非高斯场景
• 统一融合输出：保证轨迹平滑且稳定

效果总结

相比单一模型方法，该算法在运动状态发生变化（如加速、减速）时表现更加稳定，整体跟踪精度更高，是一种兼顾灵活性与鲁棒性的目标跟踪方案。

运行结果

运行代码后，即可得到下面的结果：
运动轨迹示意图：

各轴估计的误差曲线：

各轴RMSE：

各模型概率曲线：

与实际仿真的运动模型更替相匹配：

命令行截图：

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

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