三个CT模型，分别是旋转速率不同，适用于定位、导航、目标跟踪
原创代码，包运行成功，禁止翻卖

代码介绍

这段代码实现了一个基于交互式多模型 (Interacting Multiple Model, IMM) 算法的二维目标跟踪仿真系统。与常规的 CV/CA 模型组合不同，本程序采用了三个不同转弯速率的匀速圆周运动 (CT) 模型来增强对复杂机动轨迹的跟踪能力。

场景概述

本程序模拟了一个在二维平面上做复杂转弯运动的目标。目标的运动轨迹包含了三个阶段的机动切换：

• 01s - 19s：以 1°/s 的小速率转弯。
• 20s - 40s：切换为 3°/s 的快速左转弯。
• 60s - 80s：切换为 -3°/s 的快速右转弯。
• 其他时段：恢复为 1°/s 的小速率转弯。

算法核心：三模型 IMM

为了精确捕获目标在不同机动强度下的状态，系统并行运行了三个 CT (Constant Turn) 滤波器，每个滤波器对应不同的转弯预设：

• 模型 1 (CT1)：转弯率为 $1^{\circ }/s$，对应平稳或微小机动。
• 模型 2 (CT2)：转弯率为 $3^{\circ }/s$，专门捕捉快速左转。
• 模型 3 (CT3)：转弯率为 $-3^{\circ }/s$，专门捕捉快速右转。

IMM 算法的四个关键步骤

代码严格遵循 IMM 算法的标准迭代逻辑：

1. 交互/混合 (Interaction)：根据预测概率和转移概率矩阵，对各个滤波器的初始状态进行加权混合。
2. 卡尔曼滤波 (Filtering)：三个子滤波器（Kalman）并行工作，计算各自的模型残差和状态估计。
3. 概率更新 (Probability Update)：根据观测值与模型预测值的匹配程度（似然度），动态调整三个模型的置信度（概率）。
4. 估计融合 (Combination)：以各模型概率为权重，融合输出最终的最优状态估计。

主要功能与输出

程序运行后将生成多维度结果图表，用于评估跟踪性能：

• 轨迹对比图：直观展示真实轨迹、含噪声观测值、以及 IMM 融合后的跟踪轨迹。
• 跟踪误差图：分别给出 X/Y 轴的位置误差与速度误差曲线。
• 概率切换曲线：这是 IMM 的核心特性图，展示了算法如何在目标机动时，自动识别并提升对应模型（如从模型1切换到模型2）的概率。
• 数值统计：在命令行窗口输出各模型及 IMM 算法的速度跟踪误差最大值，定量对比性能。

适用场景

该代码适合研究：

• 多模型状态估计与融合。
• 强机动目标（如无人机、导弹、避障车辆）的精确跟踪。
• 模型转移概率矩阵 (TPM) 对算法收敛性的影响。

代码构成

• 代码结构如下：
  

运行结果

• 目标的运动轨迹：
  
  放大后可以看到各个模型的估计效果：
  
  位置跟踪误差和速度跟踪误差曲线如下：
  
  三个模型交互时的概率曲线如下：
  
  输出各模型对X轴速度跟踪误差的最大值：
  

源代码

% 基于IMM算法的目标跟踪，三模型IMM（3种CT的模型）
% 2024-11-01/Ver1
clc; clear; close all;  % 清除命令窗口、工作空间和关闭所有图形窗口
rng('default'); rng(0); % 设置随机数生成器的默认状态，以确保可重复性

%% 仿真参数设置
time = 100;            % 仿真迭代次数
T = 1;                  % 采样间隔（时间步长）
w1 = 1 * 2 * pi / 360; % 模型2的转弯率（1度）
w2 = 3 * 2 * pi / 360; % 模型2的转弯率（3度）
w3 = -3 * 2 * pi / 360; % 模型3的转弯率（-3度）
H = [1, 0, 0, 0;       % 模型量测矩阵
     0, 0, 1, 0];     
G = [T^2 / 2, 0;      % 模型过程噪声加权矩阵
     T, 0;
     0, T^2 / 2;
     0, T];  
R = 10 * diag([1, 1]); % 模型量测噪声协方差矩阵
Q = 0.1 * diag([1, 1]); % 模型过程噪声协方差矩阵
F1 = [];  
F2 = [];             % 模型2状态转移矩阵（左转弯）
F3 = [];             % 模型3状态转移矩阵（右转弯）

x0 = [1000, 200, 100, 20]'; % 初始状态（位置X，速度X，位置Y，速度Y）

% 产生量测数据

完整代码：
https://blog.csdn.net/callmeup/article/details/143426507?spm=1011.2415.3001.5331
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者