Cooperative Localization and Mapping Based on UWB/IMU Fusion Using Factor Graphs

In complex and unknown environments, achieving precise localization and real-time mapping stands as a critical requirement for agent navigation and scene comprehension. However, conventional localization methods, which rely on single sensors such as inertial measurement units (IMUs) or ultra-wideband (UWB) sensors, often face challenges in maintaining high precision within these intricate settings. Consequently, the task of achieving accurate self-localization and constructing topological maps becomes increasingly daunting. To tackle this challenge, we introduce a collaborative localization and mapping approach that harnesses data from both IMU and UWB sensors. We employ factor graphs as the representation model, treating observations, states, and constraints as factors within the graph. The IMU provides vital attitude and acceleration information, while the UWB sensor contributes valuable distance observations. Through the maximization of posterior probability, we estimate the agent's position and create the map. Our comprehensive evaluations conducted in physical environments conclusively demonstrate the effectiveness of our method in achieving accurate localization and mapping.