本文所述的MATLAB代码为三维的交互式多模型（IMM）滤波器，结合了匀速直线运动（CV模型）和匀速圆周运动（CT模型）的状态估计。使用扩展卡尔曼滤波（EKF）来处理状态更新与观测数据，旨在提高对动态系统状态的估计精度。

程序结构

运行结果

三维轨迹图：

三维位移曲线：

三轴位移误差曲线：

概率变化曲线：

部分代码

% 三维IMM，CV和CT模型，EKF
% 2025-03-01/Ver1
%% 建模
clear; %清空工作区
clc; %清空命令行
close all; %关闭所有窗口（主窗口除外）
rng(0); %固定随机种子，让每次运行得到的结果相同
N = 600; %定义仿真时间为600
T = 1; %定义采样间隔为1
x0 = [1000,10,1000,10,30,1]'; %状态初始化，四项为别为x轴位置、速度、y轴位置、速度
xA = []; %预定义输出的状态
% CV匀速运动
fCV = @(x, y) [
    x(1) + T*x(2);
    x(2);
    x(3) + T*x(4);
    x(4);
    x(5) + T*x(6);
    x(6)]; % CV的状态方程
A1 = [1,T,0,0,0,0;
    0,1,0,0,0,0;
    0,0,1,T,0,0;
    0,0,0,1,0,0;
    0,0,0,0,1,T;
    0,0,0,0,0,1]; %定义匀速运动时的状态转移矩阵
G1=[T^2/2,0,0;
    T,0,0;
    0,T^2/2,0;
    0,T,0;
    0,0,T^2;
    0,0,T] ; %设置匀速运动时的输入向量转移矩阵
Q1=0.01*diag([1,1,1]); %设置状态转移协方差矩阵
% CT匀速圆周运动
w = -pi/360;
% f1=1;
% f2=sin(w*t)/w;
% f3=(1-cos(w*t))/w;
% f4=cos(w*t);
% f5=sin(w*t);
% F=[ f1 f2 0 -f3 ;
%     0  f4 0 -f5 ;
%     0 f3 f1  f2 ;
%     0 f5 0 f4;];
fCT = @(x, y) [
    x(1) + sin(w*T)/w*x(2) - (1-cos(w*T))/w*x(4);
    cos(w*T)*x(2) - sin(w*T)*x(4);
    (1-cos(w*T))/w*x(2) + x(3) + sin(w*T)/w*x(4);
    sin(w*T)*x(2) + cos(w*T)*x(4);
    x(5)+T*x(6);
    x(6)]; % CV的状态方程
A2=CreatCTF(-pi/360,T); %设置匀速圆周运动时的状态转移矩阵
G2=CreatCTT(T); %设置匀速圆周运动时的输入向量转移矩阵
Q2=0.0144^2*diag([1,1,1]); %设置匀速圆周运动时的

运行结束后，命令行窗口的输出如下：

代码详解

主要功能

1. 状态建模：

   • 初始化状态，包括位置和速度。
   • 定义CV和CT模型的状态转移方程及协方差矩阵。

2. 真实数据生成：

   • 模拟匀速运动和圆周运动的真实状态数据，通过加入噪声生成测量数据。

3. IMM滤波：

   • 使用IMM框架融合CV与CT模型的估计，更新状态和协方差，计算模型的转移概率。

4. 卡尔曼滤波：

   • 对每个模型（CV和CT）进行卡尔曼滤波，以获得状态估计。
   • 通过预测与更新步骤，计算滤波增益和协方差。

5. 误差分析：

   • 计算不同模型下的估计误差，包括RMSE（均方根误差）和模型概率的变化。

6. 结果可视化：

   • 绘制真实值、测量值及不同模型的滤波结果，以便直观比较滤波性能。

代码结构

• 参数设置：包括仿真时间、采样间隔、状态初始值等。
• 状态转移矩阵生成：定义两个运动模型的状态转移矩阵。
• 数据生成循环：迭代产生真实状态数据，并加入观测噪声。
• 卡尔曼滤波实现：分别对CV和CT模型进行滤波。
• 结果绘图：展示估计结果与真实值的对比，以及不同模型的误差分析。

结构图如下：

完整代码

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