Graph-Based vs. Error State Kalman Filter-Based Fusion Of 5G And Inertial Data For MAV Indoor Pose Estimation

TL;DR

This work tackles indoor MAV pose estimation by fusing 5G ToA measurements with onboard IMU data via two model-based approaches: an Error State Kalman Filter (ESKF) and a Pose Graph Optimization (PGO) framework. It augments EuRoC MAV sequences with realistic 5G ToA data generated in QuaDRiGa, evaluating how base-station count and bandwidth affect localization. The study finds that the graph-based PGO consistently yields higher accuracy (around $0.15~\mathrm{m}$ ATE with five BSs) than the ESKF (around $0.34~\mathrm{m}$), while both methods run in real time, underscoring the potential of 5G ToA for robust indoor MAV localization. The results highlight the value of leveraging historical measurements in a graph-based formulation and point to future enhancements through sensor fusion with cameras and barometers to address vertical errors and non-line-of-sight scenarios.

Abstract

5G New Radio Time of Arrival (ToA) data has the potential to revolutionize indoor localization for micro aerial vehicles (MAVs). However, its performance under varying network setups, especially when combined with IMU data for real-time localization, has not been fully explored so far. In this study, we develop an error state Kalman filter (ESKF) and a pose graph optimization (PGO) approach to address this gap. We systematically evaluate the performance of the derived approaches for real-time MAV localization in realistic scenarios with 5G base stations in Line-Of-Sight (LOS), demonstrating the potential of 5G technologies in this domain. In order to experimentally test and compare our localization approaches, we augment the EuRoC MAV benchmark dataset for visual-inertial odometry with simulated yet highly realistic 5G ToA measurements. Our experimental results comprehensively assess the impact of varying network setups, including varying base station numbers and network configurations, on ToA-based MAV localization performance. The findings show promising results for seamless and robust localization using 5G ToA measurements, achieving an accuracy of 15 cm throughout the entire trajectory within a graph-based framework with five 5G base stations, and an accuracy of up to 34 cm in the case of ESKF-based localization. Additionally, we measure the run time of both algorithms and show that they are both fast enough for real-time implementation.

Figures (8)

• Figure 1: Illustration of a MAV indoor localization example scenario using several 5G base stations.
• Figure 2: 5G Frame Structure
• Figure 3: Visualization of the 5G Resource Grid Structure, highlighting Resource Blocks (RBs) and Resource Elements (REs), showcasing the allocation of resources in Resource Grid.
• Figure 4: PRS distribution in a physical resource block in 5G NR with two BSs using a comb-6 structure
• Figure 5: The figure visualizes the structure of the factor graph used to optimize the variables, represented by circles, by relating them through factors, represented by squares. The nodes $\mathtt{T}_t$ incorporate the 6DoF pose variables, $\mathtt{v}_t$ nodes encapsulate the velocity variables, and $\mathtt{b}_t$ nodes denote the bias variables, encompassing biases from both gyroscopes and accelerometers. IMU pre-integration factors connect all of these nodes. ToA measurements create range factors between robot pose nodes and BSs position nodes, with a single instance, $\mathbf{L}_1$, visualized here to enhance graph clarity. Prior factors, namely prior pose, prior velocity, and prior bias, are connected to the respective nodes $\mathtt{T}_1$, $\mathtt{v}_1$, and $\mathtt{b}_{1}$ to constrain them with their initial values in the trajectory.