本文所述的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]); %设置匀速圆周运动时的运行结束后，命令行窗口的输出如下：
代码详解
主要功能
-
状态建模：
- 初始化状态，包括位置和速度。
- 定义CV和CT模型的状态转移方程及协方差矩阵。
-
真实数据生成：
- 模拟匀速运动和圆周运动的真实状态数据，通过加入噪声生成测量数据。
-
IMM滤波：
- 使用IMM框架融合CV与CT模型的估计，更新状态和协方差，计算模型的转移概率。
-
卡尔曼滤波：
- 对每个模型（CV和CT）进行卡尔曼滤波，以获得状态估计。
- 通过预测与更新步骤，计算滤波增益和协方差。
-
误差分析：
- 计算不同模型下的估计误差，包括RMSE（均方根误差）和模型概率的变化。
-
结果可视化：
- 绘制真实值、测量值及不同模型的滤波结果，以便直观比较滤波性能。
代码结构
- 参数设置：包括仿真时间、采样间隔、状态初始值等。
- 状态转移矩阵生成：定义两个运动模型的状态转移矩阵。
- 数据生成循环：迭代产生真实状态数据，并加入观测噪声。
- 卡尔曼滤波实现：分别对CV和CT模型进行滤波。
- 结果绘图：展示估计结果与真实值的对比，以及不同模型的误差分析。
结构图如下：
完整代码
如需帮助，或有导航、定位滤波相关的代码定制需求，请点击下方卡片联系作者
