基于UKF-IMM无迹卡尔曼滤波与交互式多模型的轨迹跟踪算法matlab仿真,对比EKF-IMM和UKF

1.程序功能描述

基于UKF-IMM无迹卡尔曼滤波与交互式多模型的轨迹跟踪算法matlab仿真,对比EKF-IMM和UKF。
2.测试软件版本以及运行结果展示

MATLAB2022A版本运行

3.核心程序

1. .............................................................................
2. % 绘制目标运动与传感器分布的图形，展示 IMM - UKF 算法的跟踪效果
3. %目标运动与传感器分布
4. figure   
5. % 绘制目标的真实轨迹
6. plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
7. hold on
8. % 循环处理每个节点
9. for i = 1:NumberNode
10.     % 绘制 IMM - UKF 算法的状态估计轨迹
11.     plot(Xfstate(1,:),Xfstate(4,:),'-mo',...
12.     'LineWidth',1,...
13.     'MarkerSize',6,...
14.     'MarkerEdgeColor','k',...
15.     'MarkerFaceColor',[0.5,0.9,0.0]);
16.     hold on
17.     % 绘制真实节点的位置
18.     plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
19.     hold on
20.     % 在节点位置旁边标注节点编号
21.     text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
22.     hold on
23.     % 绘制 IMM - UKF 算法估计的节点位置
24.     plot(pest(1,i),pest(2,i),'rs','LineWidth',1);
25.     hold on
26.     % 添加图例说明不同线条和标记的含义
27.     legend('真实轨迹','IMM-UKF估计轨迹','真实节点','IMM-UKF节点');
28.     % 设置图形标题
29.     title('跟踪效果对比');
30. end
31. % 设置坐标轴为正方形，使图形比例合适
32. axis square
34. % 绘制目标运动与传感器分布的图形，展示 IMM - EKF 算法的跟踪效果
35. figure   
36. % 绘制目标的真实轨迹
37. plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
38. hold on
39. % 循环处理每个节点
40. for i = 1:NumberNode
41.     % 绘制 IMM - EKF 算法的状态估计轨迹
42.     plot(Xfstate2(1,:),Xfstate2(4,:),'-mo',...
43.     'LineWidth',1,...
44.     'MarkerSize',6,...
45.     'MarkerEdgeColor','k',...
46.     'MarkerFaceColor',[0.5,0.9,0.0]);
47.     hold on
48.     % 绘制真实节点的位置
49.     plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
50.     hold on
51.     % 在节点位置旁边标注节点编号
52.     text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
53.     hold on
54.     % 绘制 IMM - EKF 算法估计的节点位置
55.     plot(pest2(1,i),pest2(2,i),'rs','LineWidth',1);
56.     hold on
57.     % 添加图例说明不同线条和标记的含义
58.     legend('真实轨迹','IMM-EKF估计轨迹','真实节点','IMM-EKF节点');
59.     % 设置图形标题
60.     title('跟踪效果对比');
61. end
62. % 设置坐标轴为正方形，使图形比例合适
63. axis square
65. % 绘制目标运动与传感器分布的图形，展示 UKF 算法的跟踪效果
66. figure   
67. % 绘制目标的真实轨迹
68. plot(TargetState(1,:),TargetState(4,:),'k','LineWidth',2);
69. hold on
70. % 循环处理每个节点
71. for i = 1:NumberNode
72.     % 绘制 UKF 算法的状态估计轨迹
73.     plot(Para_sensor3(1,:),Para_sensor3(4,:),'-mo',...
74.     'LineWidth',1,...
75.     'MarkerSize',6,...
76.     'MarkerEdgeColor','k',...
77.     'MarkerFaceColor',[0.5,0.9,0.0]);
78.     hold on
79.     % 绘制真实节点的位置
80.     plot(NodeDistribution(1,i),NodeDistribution(2,i),'bo','LineWidth',1);
81.     hold on
82.     % 在节点位置旁边标注节点编号
83.     text(NodeDistribution(1,i)+0.5,NodeDistribution(2,i)+0.5,num2str(i));
84.     hold on
85.     % 绘制 UKF 算法估计的节点位置
86.     plot(pest3(1,i),pest3(2,i),'rs','LineWidth',1);
87.     hold on
88.     % 添加图例说明不同线条和标记的含义
89.     legend('真实轨迹','UKF估计轨迹','真实节点','UKF节点');
90.     % 设置图形标题
91.     title('跟踪效果对比');
92. end
93. % 设置坐标轴为正方形，使图形比例合适
94. axis square
96. % 绘制不同算法的误差随时间变化的曲线
97. figure
98. % 绘制 IMM - UKF 算法的误差曲线
99. plot(tms,err1(1,:),'-bs',...
100.     'LineWidth',1,...
101.     'MarkerSize',6,...
102.     'MarkerEdgeColor','k',...
103.     'MarkerFaceColor',[0.9,0.0,0.0]);
104. hold on;
105. % 绘制 IMM - EKF 算法的误差曲线
106. plot(tms,err2(1,:),'-mo',...
107.     'LineWidth',1,...
108.     'MarkerSize',6,...
109.     'MarkerEdgeColor','k',...
110.     'MarkerFaceColor',[0.5,0.9,0.0]);
111. hold on;
112. % 绘制 UKF 算法的误差曲线
113. plot(tms,err3(1,:),'-r>',...
114.     'LineWidth',1,...
115.     'MarkerSize',6,...
116.     'MarkerEdgeColor','k',...
117.     'MarkerFaceColor',[0.9,0.9,0.0]);
118. hold on;
119. % 添加图例，说明不同曲线对应的算法
120. legend('IMM - UKF ','IMM - EKF','UKF');
121. % 设置 x 轴标签为时间步
122. xlabel('Time Steps')
123. % 设置 y 轴标签为误差
124. ylabel('error')
126. % 绘制不同算法的平均误差柱状图
127. figure
128. % 绘制三个算法的平均误差柱状图
129. bar([mean(err1(1,:)),mean(err2(1,:)),mean(err3(1,:))]);
130. % 设置 x 轴标签，说明每个柱子对应的算法
131. xlabel(['1:IMM - UKF, 2:IMM - EKF, 3:UKF']);
132. % 设置 y 轴标签为误差
133. ylabel('error')
134. 93

复制代码

　　
4.本算法原理

在许多工程实践中，往往不能直接得到所需要的状态变量的真实值。例如雷达在探测目标时，可以通过回波信号等计算出目标的距离、速度和角度等信息。但雷达探测过程中会存在干扰（系统噪声、地杂波和非目标信号等）的问题，这些干扰会导致回波信号中夹杂有随机噪声。我们要在有随机噪声的回波信号中分离目标的运动状态量，准确的得到这个状态量往往是不可能的，只能根据观测信号估计这些状态变量。卡尔曼滤波就是这种通过估计或预测降低噪声影响的一种好的方法。特别是在线性系统中，卡尔曼滤波是最优的滤波算法。
在轨迹跟踪问题中，系统状态通常随时间变化，并且受到过程噪声的影响；同时，对系统状态的观测也包含观测噪声。我们的目标是根据一系列的观测值来估计系统的真实状态。UKF 是一种用于非线性系统状态估计的滤波算法。与传统的扩展卡尔曼滤波（EKF）不同，UKF 不依赖于对非线性函数的线性化，而是通过一组确定性采样点（Sigma 点）来近似状态的概率分布，从而更准确地处理非线性问题。
在kalman滤波算法中用到了状态转移方程和量测方程，被估计量随着时间的变化，呈现的是一个动态估计。在目标跟踪中，不需要知道目标的运动模型就能实时的修正目标的状态变量（速度、距离等），具有良好的适应性。但是当目标实施机动变化（突然加、减速或急转弯等），仅仅采用基本的kalman滤波算法往往得不到理想的结果。这时就需要采用自适应算法。交互多模型（IMM）就应用而生。
　　目标交互多模型kalman滤波算法在机动目标跟踪领域得到广泛应用。IMM算法使用两个或者多个模型来描述工作过程中可能出现的状态，最后通过有效的加权融合进行系统状态估计，很好的克服了单个模型估计误差较大的问题。
IMM 算法用于处理系统在不同模式下运行的情况。它假设系统存在多个可能的运行模式，每个模式对应一个不同的状态模型，通过在这些模型之间进行交互和切换，以适应系统模式的变化，从而提高状态估计的准确性。

5.完整程序

VVV
 

来源：程序园用户自行投稿发布，如果侵权，请联系站长删除
免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！