【MATLAB例程】自适应IMM，CV和CT两个运动模型，包含观测噪声自适应，使用Sage-Husa EKF作为滤波主体，适用于二维平面下的非合作目标的定位。附代码下载链接-优快云博客

在这里插入图片描述

用于二维平面中对目标运动状态的自适应估计。它融合了 交互多模型（IMM）滤波框架、Sage-Husa自适应噪声估计方法 以及 扩展卡尔曼滤波（EKF），使用CV和CT两种运动模型。

文章目录

程序详解
- 误差评估与性能输出
运行结果
MATLAB源代码

程序详解

本程序实现二维目标跟踪的自适应交互多模型滤波（AIMM）算法，结合经典的状态估计技术与自适应噪声处理机制，适用于运动状态复杂、观测条件不稳定的环境。

两种运动模型：
- 匀速运动模型（CV）：适用于直线匀速场景。
- 匀角速度转弯模型（CT）：适用于圆弧或曲线路径场景。
观测信息：系统仅使用位置观测（不含速度测量），模拟实际传感器输出。

交互式多模型滤波框架：

AIMM结构融合了多个滤波器结果，通过模型概率加权机制进行状态估计。系统能够根据信息残差自动调整模型的优先级，使滤波结果在不同运动阶段间自适应切换。

噪声自适应机制（Sage-Husa）：

考虑到实际测量过程中噪声水平常常不稳定，算法引入了自适应观测噪声估计机制，可随观测误差动态调整测量协方差矩阵，提高估计准确性与稳健性。

误差评估与性能输出

📊系统提供多种后处理工具，用于评估滤波性能：

滤波轨迹与真实轨迹对比图；
每种滤波器的估计误差趋势分析；
误差累积分布图（CDF）；
各模型的均方根误差（RMSE）对比；
模型概率随时间变化图（反映模型切换情况）；
最大误差、平均误差、标准差等统计结果。

运行结果

轨迹真值与各方法估计值对比：
在这里插入图片描述
误差与误差CDF图像：

IMM概率曲线：

命令行输出结果：

MATLAB源代码

部分代码如下：

% 自适应IMM算法（CV/CT模型），二维平面
% 包含观测噪声自适应，使用Sage-Husa EKF作为滤波主体，带误差统计特性输出
% 2025-07-24/Ver1

%% 建模
clear;clc;close all; %初始化
rng(0); %固定随机种子，让每次运行得到的结果相同 
N = 600; %定义仿真时间为600
T = 1; %定义采样间隔为1s

% 修改状态向量定义：[x, vx, y, vy] - 4维状态
x0 = [1000,10,1000,10]'; %状态初始化，四项分别为x轴位置、速度、y轴位置、速度
xA = []; %预定义输出的状态

% CV匀速运动模型
A1 = [1,T,0,0;
      0,1,0,0;
      0,0,1,T;
      0,0,0,1]; %CV状态转移矩阵

G1 = [T^2/2,0;
      T,0;
      0,T^2/2;
      0,T]; %CV噪声输入矩阵

Q1=0.001*diag([1,1]); %CV状态转移协方差矩阵

% CT匀角速度转弯模型
omega = pi/180; % 角速度，单位rad/s（1度/秒）
sin_wT = sin(omega*T);
cos_wT = cos(omega*T);

A2 = [1, sin_wT/omega, 0, -(1-cos_wT)/omega;
      0, cos_wT, 0, -sin_wT;
      0, (1-cos_wT)/omega, 1, sin_wT/omega;
      0, sin_wT, 0, cos_wT]; %CT状态转移矩阵

G2 = [T^2/2,0;
      T,0;
      0,T^2/2;
      0,T]; %CT噪声输入矩阵

Q2=0.001*diag([1,1]); %CT状态转移协方差矩阵

% SAGE Husa自适应参数
alpha_1 = 0.95; % 衰减因子，越接近1表示对历史信息依赖越强
alpha_2 = 0.95; % 衰减因子
beta_1 = 0.9;   % 过程噪声自适应衰减因子
beta_2 = 0.9;   % 过程噪声自适应衰减因子

%% 产生真实数据
x = x0; %在迭代产生真值前，给x赋初值

% 第一段：匀速直线运动
for k = 1:150
   x = A1*x + G1*sqrt(Q1)*randn(size(Q1,1),1); %状态一步转移，后面的是误差
   xA = [xA x]; % 将迭代得到的x加在xA后面一列
end

% 第二段：匀角速度转弯运动
for k = 1:120
   x = A2*x + G2*sqrt(Q2)*randn(size(Q2,1),1); %状态一步转移，后面的是误差
   xA = [xA x]; % 将迭代得到的x加在xA后面一列
end

% 第三段：匀速直线运动
for k = 1:N-270
   x = A1*x + G1*sqrt(Q1)*randn(size(Q1,1),1); %状态一步转移，后面的是误差
   xA = [xA x]; % 将迭代得到的x加在xA后面一列
end 

%% AIMM（自适应IMM）
% 更多内容和注释，详见完整代码
    
% MM滤波初始化参数%%%%%%% %%%%
 

%% 结果后处理