【MATLAB代码】结合匀速运动 (CV) 和匀加速运动 (CA) 模型的EKF构造的IMM（交互式多模型）

程序简介

该函数实现了交互式多模型 (IMM) 滤波器，结合了匀速运动 (Constant Velocity, CV) 和匀加速运动 (Constant Acceleration, CA) 模型。通过卡尔曼滤波，系统能够在不同运动模式之间切换，从而提高状态估计的准确性。

运行截图

• 轨迹图：
  
• 误差与误差的$CDF$图像：
  
• 程序结构：
  
• 概率曲线：
  
• RMSE对比图：
  

程序代码

部分代码如下：

% CV和CA模型组成的IMM
% 如需付费讲解，联系微信：matlabfilter
% 2024-11-07/Ver1
%% 建模
clear; %清空工作区
clc; %清空命令行
close all; %关闭所有窗口（主窗口除外）
rng(0); %固定随机种子，让每次运行得到的结果相同 
N = 600; %定义仿真时间为600
T = 1; %定义采样间隔为1
x0 = [1000,10,1,1000,10,1]'; %状态初始化，四项为别为x轴位置、速度、y轴位置、速度
xA = []; %预定义输出的状态
% CV匀速运动
% x = A1*x + G1*sqrt(Q1)*[randn,randn]';
A1 = [1,T,0,0,0,0;
      0,1,0,0,0,0;
      0,0,0,0,0,0;
      0,0,0,1,T,0;
      0,0,0,0,1,0;
      0,0,0,0,0,0]; %定义匀速运动时的状态转移矩阵
G1=[T^2/2,0;
    T,0;
    1,0;
    0,T^2/2;
    0,T;
    0,1]; %设置匀速运动时的输入向量转移矩阵
Q1=0.001*diag([1,1]); %设置状态转移协方差矩阵
% CA匀加速运动
A2 = [1,T,T^2/2,0,0,0;
      0,1,T,0,0,0;
      0,0,1,0,0,0;
      0,0,0,1,T,T^2/2;
      0,0,0,0,1,T;
      0,0,0,0,0,1]; %定义匀速运动时的状态转移矩阵
G2=[T^2/2,0;
    T,0;
    1,0;
    0,T^2/2;
    0,T;
    0,1]; %设置匀速运动时的输入向量转移矩阵
Q2=0.001*diag([1,1]); %设置状态转移协方差矩阵

%% 产生真实数据
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 


%% IMM
H = [1 0 0 0 0 0;
    0 0 0 1 0 0]; %构建H矩阵
R = 2500*diag([1,1]); %构建观测协方差矩阵
t=1:T:N; %构建滤波的时间戳

完整代码下载链接：https://gf.bilibili.com/item/detail/1106613012

程序讲解

概述

该函数实现了一种交互式多模型 $(IMM)$滤波器，结合了匀速运动 (Constant Velocity, $CV$) 和匀加速运动 (Constant Acceleration, $CA$) 模型。通过卡尔曼滤波，系统能够在不同运动模式之间切换，从而提高状态估计的准确性。

主要功能

• 状态建模：定义状态转移矩阵、输入转移矩阵和协方差矩阵。
• 真实数据生成：模拟通过匀速直线和匀加速运动生成的真实数据。
• 卡尔曼滤波：实现针对 CV 和 CA 模型的卡尔曼滤波。
• IMM算法：实现IMM算法，结合多个模型的输出，更新状态估计和模型概率。
• 结果可视化：绘制真实值、测量值和滤波结果的比较图，以及误差的统计分析图。

详细讲解

1. 初始化

clear; % 清空工作区
clc; % 清空命令行
close all; % 关闭所有窗口
rng(0); % 固定随机种子
N = 600; % 仿真时间
T = 1; % 采样间隔
x0 = [1000,10,1,1000,10,1]'; % 初始化状态

• N定义了仿真时间为600个时间点。
• T是采样时间间隔。
• x0是状态的初始值，包含位置和速度信息。

2. 状态转移矩阵和协方差矩阵

% CV匀速运动
A1 = [...]; % 状态转移矩阵
G1 = [...]; % 输入转移矩阵
Q1 = 0.001*diag([1,1]); % 状态转移协方差矩阵

% CA匀加速运动
A2 = [...]; % 状态转移矩阵
G2 = [...]; % 输入转移矩阵
Q2 = 0.001*diag([1,1]); % 状态转移协方差矩阵

• A1和A2定义了CV和CA模型的状态转移矩阵。
• G1和G2是输入转移矩阵，描述了模型对输入的响应。
• Q1和Q2是过程噪声协方差矩阵。

3. 生成真实数据

x = x0; % 初始状态
for k = 1:150 % 匀速直线
   x = A1*x + G1*sqrt(Q1)*randn(size(Q1,1),1); % 状态转移
   xA = [xA x]; % 储存状态
end

• 通过循环生成包含匀速和匀加速运动的数据序列。

4. IMM算法

for mn = 1:MC % 蒙特卡罗次数
    % 模型初始化
    ...
    for k = 1:length(t) % 滤波过程
        ...
        % 残差与协方差计算
        ...
        % 状态更新
        ...
    end
end

• 使用蒙特卡洛方法来评估滤波器的性能。
• 在每次迭代中，计算不同模型的残差，并更新状态估计。

5. 结果后处理与可视化

% 计算均方根误差
EX1 = sqrt(sum(ex1.^2,1)/MC);
% 绘制结果
figure;
plot(xA(1,t), xA(4,t), 'k', ...);

• 计算滤波器的误差，并绘制真实值、测量值和滤波结果的比较图。

6. 卡尔曼滤波函数

function [P_k,P_k_k_1,X_k]=kalman(F,T,H,Q,R,Z,X0,P0)
    ...
end

• 该函数实现了卡尔曼滤波的核心算法，包括状态预测、协方差更新和增益计算。

总结

该MATLAB函数有效地实现了IMM算法，通过结合CV和CA模型，提高了对动态系统状态的估计精度。通过详细的可视化和误差分析，用户可以直观地观察到不同模型的表现和优势。