目录
1.程序功能描述
2.测试软件版本以及运行结果展示
3.核心程序
4.本算法原理
5.完整程序
1.程序功能描述
基于UKF-IMM无迹卡尔曼滤波与交互式多模型的轨迹跟踪算法matlab仿真,对比EKF-IMM和UKF。
2.测试软件版本以及运行结果展示
MATLAB2022A版本运行
3.核心程序
.............................................................................
% 绘制目标运动与传感器分布的图形，展示 IMM - UKF 算法的跟踪效果
%目标运动与传感器分布
figure
% 绘制目标的真实轨迹
plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
hold on
% 循环处理每个节点
for i = 1:NumberNode
% 绘制 IMM - UKF 算法的状态估计轨迹
plot(Xfstate(1,:),Xfstate(4,:),'-mo',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.5,0.9,0.0]);
hold on
% 绘制真实节点的位置
plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
hold on
% 在节点位置旁边标注节点编号
text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
hold on
% 绘制 IMM - UKF 算法估计的节点位置
plot(pest(1,i),pest(2,i),'rs','LineWidth',1);
hold on
% 添加图例说明不同线条和标记的含义
legend('真实轨迹','IMM-UKF估计轨迹','真实节点','IMM-UKF节点');
% 设置图形标题
title('跟踪效果对比');
end
% 设置坐标轴为正方形，使图形比例合适
axis square
% 绘制目标运动与传感器分布的图形，展示 IMM - EKF 算法的跟踪效果
figure
% 绘制目标的真实轨迹
plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
hold on
% 循环处理每个节点
for i = 1:NumberNode
% 绘制 IMM - EKF 算法的状态估计轨迹
plot(Xfstate2(1,:),Xfstate2(4,:),'-mo',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.5,0.9,0.0]);
hold on
% 绘制真实节点的位置
plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
hold on
% 在节点位置旁边标注节点编号
text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
hold on
% 绘制 IMM - EKF 算法估计的节点位置
plot(pest2(1,i),pest2(2,i),'rs','LineWidth',1);
hold on
% 添加图例说明不同线条和标记的含义
legend('真实轨迹','IMM-EKF估计轨迹','真实节点','IMM-EKF节点');
% 设置图形标题
title('跟踪效果对比');
end
% 设置坐标轴为正方形，使图形比例合适
axis square
% 绘制目标运动与传感器分布的图形，展示 UKF 算法的跟踪效果
figure
% 绘制目标的真实轨迹
plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
hold on
% 循环处理每个节点
for i = 1:NumberNode
% 绘制 UKF 算法的状态估计轨迹
plot(Para_sensor3(1,:),Para_sensor3(4,:),'-mo',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.5,0.9,0.0]);
hold on
% 绘制真实节点的位置
plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
hold on
% 在节点位置旁边标注节点编号
text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
hold on
% 绘制 UKF 算法估计的节点位置
plot(pest3(1,i),pest3(2,i),'rs','LineWidth',1);
hold on
% 添加图例说明不同线条和标记的含义
legend('真实轨迹','UKF估计轨迹','真实节点','UKF节点');
% 设置图形标题
title('跟踪效果对比');
end
% 设置坐标轴为正方形，使图形比例合适
axis square
% 绘制不同算法的误差随时间变化的曲线
figure
% 绘制 IMM - UKF 算法的误差曲线
plot(tms,err1(1,:),'-bs',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.9,0.0,0.0]);
hold on;
% 绘制 IMM - EKF 算法的误差曲线
plot(tms,err2(1,:),'-mo',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.5,0.9,0.0]);
hold on;
% 绘制 UKF 算法的误差曲线
plot(tms,err3(1,:),'-r>',...
'LineWidth',1,...
'MarkerSize',6,...
'MarkerEdgeColor','k',...
'MarkerFaceColor',[0.9,0.9,0.0]);
hold on;
% 添加图例，说明不同曲线对应的算法
legend('IMM - UKF ','IMM - EKF','UKF');
% 设置 x 轴标签为时间步
xlabel('Time Steps')
% 设置 y 轴标签为误差
ylabel('error')
% 绘制不同算法的平均误差柱状图
figure
% 绘制三个算法的平均误差柱状图
bar([mean(err1(1,:)),mean(err2(1,:)),mean(err3(1,:))]);
% 设置 x 轴标签，说明每个柱子对应的算法
xlabel(['1:IMM - UKF, 2:IMM - EKF, 3:UKF']);
% 设置 y 轴标签为误差
ylabel('error')
934.本算法原理
在许多工程实践中，往往不能直接得到所需要的状态变量的真实值。例如雷达在探测目标时，可以通过回波信号等计算出目标的距离、速度和角度等信息。但雷达探测过程中会存在干扰（系统噪声、地杂波和非目标信号等）的问题，这些干扰会导致回波信号中夹杂有随机噪声。我们要在有随机噪声的回波信号中分离目标的运动状态量，准确的得到这个状态量往往是不可能的，只能根据观测信号估计这些状态变量。卡尔曼滤波就是这种通过估计或预测降低噪声影响的一种好的方法。特别是在线性系统中，卡尔曼滤波是最优的滤波算法。
在轨迹跟踪问题中，系统状态通常随时间变化，并且受到过程噪声的影响；同时，对系统状态的观测也包含观测噪声。我们的目标是根据一系列的观测值来估计系统的真实状态。UKF 是一种用于非线性系统状态估计的滤波算法。与传统的扩展卡尔曼滤波（EKF）不同，UKF 不依赖于对非线性函数的线性化，而是通过一组确定性采样点（Sigma 点）来近似状态的概率分布，从而更准确地处理非线性问题。
在kalman滤波算法中用到了状态转移方程和量测方程，被估计量随着时间的变化，呈现的是一个动态估计。在目标跟踪中，不需要知道目标的运动模型就能实时的修正目标的状态变量（速度、距离等），具有良好的适应性。但是当目标实施机动变化（突然加、减速或急转弯等），仅仅采用基本的kalman滤波算法往往得不到理想的结果。这时就需要采用自适应算法。交互多模型（IMM）就应用而生。
目标交互多模型kalman滤波算法在机动目标跟踪领域得到广泛应用。IMM算法使用两个或者多个模型来描述工作过程中可能出现的状态，最后通过有效的加权融合进行系统状态估计，很好的克服了单个模型估计误差较大的问题。
IMM 算法用于处理系统在不同模式下运行的情况。它假设系统存在多个可能的运行模式，每个模式对应一个不同的状态模型，通过在这些模型之间进行交互和切换，以适应系统模式的变化，从而提高状态估计的准确性。
5.完整程序
VVV
![[Collection与数据结构] B树与B+树](https://i-blog.csdnimg.cn/direct/b96afb008a104ddca272db3e6e105ace.png)
