IMM与自适应EKF组合，自适应使用Sage Husa，结合出来的自适应IMM能更加适应系统的不确定性，比标准IMM跟踪误差更低。例程使用CA和CT双模型，可根据示例和注释修改为其他的运动模型。
人工编写，非AI生成，包运行成功
文章目录
介绍
程序为二维目标机动跟踪的 AIMM（Adaptive IMM）自适应交互多模型滤波器 。系统同时包含 CA（Constant Acceleration，匀加速）模型 与 CT（Coordinated Turn，匀角速度转弯）模型 两类运动模式，通过 IMM 框架根据模型似然动态调整其权重，从而在机动目标跟踪中兼顾稳态精度与转弯响应能力。滤波主体采用 Sage-Husa 自适应噪声估计 EKF，能够在观测噪声不稳定或统计未知的情况下实时调整测量噪声协方差，提高鲁棒性。
主要特点
- 多模型动态切换（CA + CT）
- Sage-Husa 自适应观测噪声估计
- 真实轨迹生成（CA → CT → CA）
- 独立 EKF 对照 + IMM 综合性能比较
程序结构概述
（1）建立 CA、CT 状态转移矩阵及噪声模型
（2）生成带噪真实轨迹
（3）构建观测
（4）执行独立 EKF
（5）自适应 IMM
（6）误差分析与统计
运行结果
轨迹对比：
各轴误差对比：
概率曲线：
命令行输出的结果：
MATLAB源代码
部分代码如下：
matlab
% 自适应IMM算法（CA/CT模型），二维平面，状态为 6 维：[x, vx, ax, y, vy, ay]
% 包含观测噪声自适应，使用Sage-Husa EKF作为滤波主体，带误差统计特性输出
% 作者：matlabfilter
% 2025-12-01/Ver1
%% 建模
clear; clc; close all;
rng(0);
N = 400; % 仿真总步数
T = 1; % 采样间隔
% 状态向量：[x, vx, ax, y, vy, ay]
x0 = [1000, 10, 0, 1000, 10, 0]'; % 初值
xA = [];
%% CA（匀加速）模型 - 6x6
A1 = [1, T, 0.5*T^2, 0, 0, 0;
0, 1, T, 0, 0, 0;
0, 0, 1, 0, 0, 0;
0, 0, 0, 1, T, 0.5*T^2;
0, 0, 0, 0, 1, T;
0, 0, 0, 0, 0, 1];
% G1：过程噪声输入把加速度噪声影响到状态
G1 = [0.5*T^2, 0;
T, 0;
1, 0;
0, 0.5*T^2;
0, T;
0, 1];
Q1 = 0.01 * diag([1,1]); % 过程噪声协方差（加速度噪声在两个轴）
%% CT（匀角速度转弯）模型
omega = pi/180; % rad/s
sin_wT = sin(omega*T);
cos_wT = cos(omega*T);
A2_4 = [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];
A2 = zeros(6);
A2(1,1) = A2_4(1,1); A2(1,2) = A2_4(1,2); A2(1,4) = A2_4(1,3); A2(1,5) = A2_4(1,4);
A2(2,1) = A2_4(2,1); A2(2,2) = A2_4(2,2); A2(2,4) = A2_4(2,3); A2(2,5) = A2_4(2,4);
A2(4,1) = A2_4(3,1); A2(4,2) = A2_4(3,2); A2(4,4) = A2_4(3,3); A2(4,5) = A2_4(3,4);
A2(5,1) = A2_4(4,1); A2(5,2) = A2_4(4,2); A2(5,4) = A2_4(4,3); A2(5,5) = A2_4(4,4);
% ax,ay 保持常量
A2(3,3) = 1;
A2(6,6) = 1;
% G2 与 CA 使用相同结构
G2 = G1;
Q2 = 0.001 * diag([1,1]);
%% Sage-Husa 参数
alpha_1 = 0.98; alpha_2 = 0.98;
% beta_1 = 0.9; beta_2 = 0.9;
%% 产生真实数据（先生成一段 CA，然后 CT 再 CA）
x = x0;
% 第一段：匀加速（CA）直线或恒加速度段
for k = 1:150
w = sqrtm(Q1) * randn(2,1); % 2x1
x = A1 * x + G1 * w;
xA = [xA, x];
end
% 第二段：CT 转弯
for k = 1:220
w = sqrtm(Q2) * randn(2,1);
x = A2 * x + G2 * w;
xA = [xA, x];
end
% 第三段：再回到匀加速
for k = 1:N - 370
w = sqrtm(Q1) * randn(2,1);
x = A1 * x + G1 * w;
xA = [xA, x];
end
%% AIMM（自适应IMM）完整代码：
https://download.csdn.net/download/callmeup/92424273
或：
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者