【MATLAB例程】三维空间内，交互式多模型（IMM），匀速转弯ct、Singer、当前统计CS三种模型滤波【附下载链接】

本文所述的代码实现了交互式多模型（IMM）滤波算法在三维空间中的应用，结合了三种经典的运动模型（CT、Singer、CS模型），并通过仿真数据验证了算法的性能。主要功能包括目标轨迹仿真、IMM滤波的实现、误差分析与可视化。

程序介绍

代码介绍

这段代码实现了交互式多模型（IMM）滤波算法在三维空间中的应用，结合了三种经典的运动模型（CT、Singer、CS模型），并通过仿真数据验证了算法的性能。主要功能包括目标轨迹仿真、IMM滤波的实现、误差分析与可视化。

主要功能和实现说明

1. 目标描述

   • 算法类型：交互式多模型（IMM）滤波器，用于多模型切换情况下的状态估计。
   • 应用场景：适用于目标跟踪、导航定位等动态环境下的状态估计问题。
   • 滤波模型：
     • CT模型（匀速转弯模型）：适用于匀速曲线运动。
     • Singer模型（加速度随机模型）：适用于加速度变化较大的场景。
     • CS模型（匀速运动模型）：适用于直线匀速运动。

2. 代码结构

   • 参数初始化：
     • 定义了三种运动模型的状态转移矩阵（F_CT、F_Singer、F_CS）和过程噪声协方差矩阵（Q_CT、Q_Singer、Q_CS）。
     • 设置了模型初始权重和转移概率矩阵，用于描述模型切换的可能性。
   • 仿真数据生成：
     • 使用三种不同的运动模型依次生成目标真实轨迹（true_state），模拟目标在不同模型之间的切换运动。
   • IMM滤波实现：
     • 针对每个模型分别进行Kalman滤波。
     • 利用IMM框架将各模型的估计状态和协方差进行加权合并，输出最终的估计值。
   • 误差统计：
     • 计算了各轴的误差均方值（MSE）和均方根误差（RMSE），用于评估滤波器的性能。
   • 可视化：
     • 通过三维轨迹图展示目标真实轨迹和估计轨迹。
     • 绘制各轴的误差曲线，直观展示估计误差随时间的变化。

相关公式

离散时间的状态空间模型由以下两个方程描述：

• 状态转移方程：
  $${\mathbf{x}}_k=\mathbf{F}{\mathbf{x}}_{k-1}+{\mathbf{w}}_{k-1}$$
  其中：

  • ${\mathbf{x}}_k$：时刻$k$的状态向量（如位置、速度、加速度等）。
  • $\mathbf{F}$：状态转移矩阵，定义状态随时间的演变关系。
  • ${\mathbf{w}}_{k-1}$：过程噪声，服从高斯分布：${\mathbf{w}}_{k-1}\sim \mathcal{N}(0,\mathbf{Q})$。

• 观测方程：
  $${\mathbf{z}}_k=\mathbf{H}{\mathbf{x}}_k+{\mathbf{v}}_k$$

Kalman滤波公式略。

运动模型公式：

CT模型（匀速转弯模型）：

• 状态向量：$\mathbf{x}=[x,\dot{x},y,\dot{y},\omega ,\theta {]}^T$
• 状态转移矩阵
• 适用场景：目标匀速转弯运动（如车辆或飞机的转弯）。

Singer模型（加速度随机模型）：

• 状态向量：$\mathbf{x}=[x,\dot{x},y,\dot{y},\ddot{x},\ddot{y}{]}^T$
• 状态转移矩阵
• 适用场景：目标存在随机加速度变化的运动（如机动飞行器）。

CS模型（匀速运动模型）：

• 状态向量：(\mathbf{x} = [x, \dot{x}, y, \dot{y}, z, \dot{z}]^T)
• 状态转移矩阵
• 适用场景：目标匀速直线运动。

模型概率更新

• 模型概率预测：
  $${\mu }_{j|k-1}=\sum _{i=1}^M{p}_{ij}{\mu }_{i|k-1}$$
  其中：

  • $p_{ij}$：模型$i\to j$的转移概率。
  • ${\mu }_{i|k-1}$：时刻 $k-1$模型$i$的后验概率。

• 模型概率更新：
  $${\mu }_{j|k}=\frac{{\Lambda }_j{\mu }_{j|k-1}}{{\sum }_{m=1}^M{\Lambda }_m{\mu }_{m|k-1}}$$
  其中：

  • ${\Lambda }_j=\mathcal{N}({\mathbf{z}}_k;\mathbf{H}{\mathbf{x}}_{j|k-1},\mathbf{H}{\mathbf{P}}_{j|k-1}{\mathbf{H}}^T+\mathbf{R})$：模型$j$的似然函数。

状态/协方差合并

• 状态向量合并：
  $${\mathbf{x}}_k=\sum _{i=1}^M{\mu }_{i|k}{\mathbf{x}}_{i|k}$$
• 协方差矩阵合并：
  $${\mathbf{P}}_k=\sum _{i=1}^M{\mu }_{i|k}\left({\mathbf{P}}_{i|k}+\left({\mathbf{x}}_{i|k}-{\mathbf{x}}_k\right){\left({\mathbf{x}}_{i|k}-{\mathbf{x}}_k\right)}^T\right)$$

误差分析公式：

• 误差向量：
  $${\mathbf{e}}_k={\mathbf{x}}_{true,k}-{\mathbf{x}}_{est,k}$$
• 均方误差（MSE）：
  $${\text{MSE}}_i=\frac{1}{N}\sum _{k=1}^N{e}_{i,k}^2$$
• 均方根误差（RMSE）：
  $${\text{RMSE}}_i=\sqrt{{\text{MSE}}_i}$$

运行结果

各方法估计的轨迹对比：

误差曲线：

命令行截图（误差输出）：

MATLAB源代码

% IMM算法的三维实现代码，包含CT、Singer和CS模型
% 2025-06-15/Ver1

clc; clear; close all;
rng(0);
% 1. 初始化参数
T = 1; % 采样周期
N = 100; % 仿真步数

% 状态向量长度
n_CT = 6; 
n_Singer = 6; 
n_CS = 6;

%% 各个模型
% 转移矩阵和协方差矩阵初始化
% CT模型
omega = 0.1; % 转动角速度
F_CT = [...];
Q_CT = 0.1 * eye(6); % 过程噪声

% Singer模型
tau = 5; % 时间常数
F_Singer = [...];
Q_Singer = 0.05 * eye(6); % 过程噪声

% CS模型
F_CS = [...];
Q_CS = 0.01 * eye(6); % 过程噪声

%% 模型初始权重和转移概率矩阵
model_prob = [0.33; 0.33; 0.34]; % 初始模型概率
P_trans = [0.9, 0.05, 0.05; 
           0.05, 0.9, 0.05; 
           0.05, 0.05, 0.9]; % 模型转移概率矩阵

%% 初始化状态和协方差
x_CT = [0; 1; 0; 1; 0; 0]; P_CT = 0.1 * eye(6);
x_Singer = [0; 1; 0; 1; 0; 0]; P_Singer = 0.1 * eye(6);
x_CS = [0; 1; 0; 1; 0; 0]; P_CS = 0.1 * eye(6);

%% 仿真数据生成（目标运动）
true_state = zeros(6, N);
true_state(:, 1) = [0; 1; 0; 1; 0; 0]; % 初始真实状态
for k = 2:N
    if k<N/3
        true_state(:, k) = F_CT * true_state(:, k-1) + mvnrnd(zeros(6,1), Q_CT)';
    elseif k<N/2
        true_state(:, k) = F_Singer * true_state(:, k-1) + mvnrnd(zeros(6,1), Q_Singer)';
    else
        true_state(:, k) = F_CS * true_state(:, k-1) + mvnrnd(zeros(6,1), Q_CS)';
    end
end

完整代码的下载链接如下：https://download.csdn.net/download/callmeup/91023555

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