【SLAM】扩展卡尔曼滤波器实现超宽带传感器UWB和惯性测量单元IMU数据融合定位（Matlab代码实现）

科研_G.E.M.

于 2025-04-16 09:08:03 发布

阅读量874

点赞数 27

文章标签： matlab 开发语言

本文链接：https://blog.csdn.net/qq_46009436/article/details/147265019

版权

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

⛳️赠与读者

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码、数据、文章下载

⛳️赠与读者

👨‍💻做科研，涉及到一个深在的思想系统，需要科研者逻辑缜密，踏实认真，但是不能只是努力，很多时候借力比努力更重要，然后还要有仰望星空的创新点和启发点。建议读者按目录次序逐一浏览，免得骤然跌入幽暗的迷宫找不到来时的路，它不足为你揭示全部问题的答案，但若能解答你胸中升起的一朵朵疑云，也未尝不会酿成晚霞斑斓的别一番景致，万一它给你带来了一场精神世界的苦雨，那就借机洗刷一下原来存放在那儿的“躺平”上的尘埃吧。

或许，雨过云收，神驰的天地更清朗.......🔎🔎🔎

💥1 概述

摘要——本文将涵盖无人机同步定位与地图构建（SLAM）的主题。为此，我们使用了扩展卡尔曼滤波器（EKF），结合了超宽带传感器（UWB）和惯性测量单元（IMU）的测量数据。此外，还采用了一种有趣的方法，通过最小二乘法（Least Squares, LS）来确定系统的初始状态估计。最终，通过仿真和实际结果验证了扩展卡尔曼滤波器和最小二乘法初始化的有效性。
关键词——SLAM，扩展卡尔曼滤波器，无人机，超宽带，惯性测量单元，EKF，UWB，IMU。

**位置和姿态，以及三个地标特征的位置，是通过扩展卡尔曼滤波器（EKF）实现的SLAM算法来确定的。本报告的主要目标之一是不仅为读者提供关于卡尔曼滤波器的扎实介绍，还提供足够的关于滤波器实现的信息，以作为类似项目的指导。第一部分致力于对卡尔曼滤波器进行简要介绍，随后深入讲解扩展卡尔曼滤波器。接下来，在下一部分中，解释了滤波器设计的过程以及其初始化的程序。最后两部分则分别致力于在仿真环境和现实环境中对EKF的实验实现。
II. 理论基础
解决SLAM问题的关键是扩展卡尔曼滤波器（EKF）。因此，本节将提供对它的深入理解。EKF是用于线性系统的卡尔曼滤波器的一个变体，因此首先将对线性滤波器进行简要概述，以引入概念和一些符号，随后详细探讨扩展卡尔曼滤波器。
A. 卡尔曼滤波器
卡尔曼滤波器是一种算法，它使用一系列随时间变化的测量值zk，并产生对未知变量xk的估计值，这些估计值通常比基于单次测量的估计值更准确。
该算法是一个两步过程：预测步骤和更新步骤。
1) 预测步骤：
在预测步骤中，算法根据前一状态xk对未知变量xk+1的当前状态进行预测。为此，滤波器必须知道基于当前状态的系统行为的近似值。这个近似值将从现在起被称为运动模型f(x)，这意味着预测步骤可以描述为
xk+1 = f(xk) + ε, (1)
其中ε是运动模型与车辆真实状态之间的偏差。
2) 更新步骤：
第二步称为更新步骤，在这一步中，滤波器根据可用的系统测量值zk+1对预测进行修正。为此，滤波器必须有一个关于当前状态下应观察到的测量值的近似值。这个近似值将被称为观测模型h(x)，使得
zk+1 = h(xk+1) + δ, (2)
其中δ是传感器读数与观测模型输出之间的偏差。
为了将修正应用于初始预测，需要计算卡尔曼增益Kk，这将在下一节中详细讨论。然后通过将两个步骤结合起来应用修正，如下所示
xk+1 = xk+1 + Kk(zk+1 - h(xk+1)). (3)
算法的估计是通过计算每个时间框架内变量的联合概率分布来完成的。为此，滤波器在所有误差均为高斯分布的特殊情况下，得出精确的条件概率估计。
正如前面提到的，本节的目的是介绍卡尔曼滤波器的概念及其一些符号，这意味着虽然一些中间步骤极为必要，但在此被省略，并将在后续章节中介绍。
总之，卡尔曼滤波器可以应用于估计任何线性系统中的任何数量的未知变量，其行为由f(x)描述，其观测值可以通过h(x)建模，前提是系统的误差和不确定性可以假设为具有高斯概率分布。**

**VI. 结论**
同时估计地图特征和车辆定位的问题似乎不可能实现。但正如本报告所展示的，通过将其分解为关键组成部分并逐一解决，这个高度复杂的问题变得可以用简单的解决方案来应对。
整个解决方案的核心是扩展卡尔曼滤波器（EKF）。尽管它是一个非常简单的算法，但要高效地使用它，必须对其应用和多功能性有扎实的理解。此外，了解所使用系统的优缺点以及其传感器的特性也同样重要。
正如提到的，无人机的行为特征意味着可以做出一些合理的假设，从而简化系统的建模和整体解决方案。
为了使滤波器正常工作，需要良好的初始化。为此，我们实现了一种使用最小二乘法的初始化程序，并采用了一种非传统的例程，这不仅为地标的估计提供了良好的初始化结果，还为其方差提供了有效的估计。
总体而言，滤波器在仿真和现实世界中的响应表现稳健，符合对地标和位置估计的期望结果。尽管姿态估计能够跟踪实际姿态，但仍然存在显著的偏差。
该项目的主要困难可能在于找到一种初始化地标位置的方法，这需要一些“跳出常规”的思维方式，以及使用真实数据实现算法。后者可能占据了项目时间的70%，主要是因为我们对相关材料的使用经验不足。

详细文章下载见第4部分。

📚2 运行结果

部分代码：

figure(1);
clf;
hold on;
plot(x(1,t),x(2,t),'*', 'MarkerEdgeColor', 'r', 'LineWidth', 2);
plot(xreal(1,t),xreal(2,t),'+', 'MarkerEdgeColor', 'b', 'LineWidth', 2);
plot(x(5,t),x(6,t),'o', 'MarkerEdgeColor', 'g', 'LineWidth', 2);
plot(LS(1,1),LS(1,2),'s', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
plot(x(7,t),x(8,t),'o', 'MarkerEdgeColor', 'g', 'LineWidth', 2);
plot(x(9,t),x(10,t),'o', 'MarkerEdgeColor', 'g', 'LineWidth', 2);
plot(x(1,1:t),x(2,1:t),'r', 'LineWidth', 2);
plot(xreal(1,1:t),xreal(2,1:t),'b', 'LineWidth', 2);
plot(LS(2,1),LS(2,2),'s', 'MarkerEdgeColor', 'k', 'LineWidth', 2);
plot(LS(3,1),LS(3,2),'s', 'MarkerEdgeColor', 'k', 'LineWidth', 2);

legend('Current Estimate (EKF)', 'Actual Position', 'Estimated Landmarks', 'Actual Landmarks');
axis([-3.2 1 -2 2]);
grid on;
title(['State Space (t=' num2str(t) ')']);
xlabel('X1');
ylabel('X2');

%% Orienta玢o
figure(2);
clf;
hold on

title(['Drone Orientation (t=' num2str(t) ')'])
quiver3(0,0,0,ref_real(1,1),ref_real(2,1),ref_real(3,1), 'b');
quiver3(0,0,0,ref_ekf(1,1),ref_ekf(2,1),ref_ekf(3,1), 'r');
quiver3(0,0,0,ref_real(1,2),ref_real(2,2),ref_real(3,2), 'b');
quiver3(0,0,0,ref_real(1,3),ref_real(2,3),ref_real(3,3), 'b');
quiver3(0,0,0,ref_ekf(1,2),ref_ekf(2,2),ref_ekf(3,2), 'r');
quiver3(0,0,0,ref_ekf(1,3),ref_ekf(2,3),ref_ekf(3,3), 'r');
text(ref_real(1,1),ref_real(2,1),ref_real(3,1), 'x');
text(ref_real(1,2),ref_real(2,2),ref_real(3,2), 'y');
text(ref_real(1,3),ref_real(2,3),ref_real(3,3), 'z');
text(ref_ekf(1,1),ref_ekf(2,1),ref_ekf(3,1), 'x"');
text(ref_ekf(1,2),ref_ekf(2,2),ref_ekf(3,2), 'y"');
text(ref_ekf(1,3),ref_ekf(2,3),ref_ekf(3,3), 'z"');
view(135,45);

legend('Actual Orientation','Estimated Orientation');
grid on;
set(gca,'XTick',[], 'YTick', [], 'ZTick', []);
pause(0.0001);