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

[1]Meisam<EMAIL_ADDRESS>
1]Claudio<EMAIL_ADDRESS>
1]Hriday<EMAIL_ADDRESS>1]Jose Luis<EMAIL_ADDRESS>1,2]Holger<EMAIL_ADDRESS>
*[1]Interdisciplinary Center for Security Reliability and Trust (SnT), University of Luxembourg,   Luxembourg

[2]Faculty of Science, Technology, and Medicine (FSTM), Department of Engineering, University of Luxembourg,  Luxembourg

5G New Radio Time of Arrival () 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 measurements. Our experimental results comprehensively assess the impact of varying network setups, including varying base station numbers and network configurations, on -based MAV localization performance. The findings show promising results for seamless and robust localization using 5G 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.

§ INTRODUCTION

Small drones, so-called Micro Aerial Vehicles (MAVs), are increasingly being used in indoor environments due to their versatility across various applications, such as surveillance tasks or monitoring in intralogistics, to mention only a few. Precise positioning and orientation are paramount for these applications. For instance, MAVs must adeptly navigate through constrained passageways and enclosed spaces in search-and-rescue missions to pinpoint and assist distressed individuals. Similarly, in warehouse scenarios, MAVs must navigate accurately to efficiently carry out tasks like item retrieval and delivery.

While Global Navigation Satellite Systems (GNSS) stand as the predominant positioning technology for outdoor drone applications, their effectiveness diminishes in indoor settings due to the challenges posed by signal attenuation and multipath effects. Consequently, they struggle to provide accurate localization within enclosed spaces. Inertial navigation systems (INSs) present an alternative method for indoor localization, yet they are susceptible to accumulating noise over time, leading to significant deviations in positional accuracy if left uncorrected.

Recent advances in sensor technology have made LiDAR and image-based methods promising alternatives for indoor MAV localization due to their high accuracy and robustness. However, challenges such as computational complexity, the need for sophisticated algorithms, the high cost and still considerable size and weight of LiDAR sensors, and the difficulty of image-based methods in poor-texture or poorly illuminated environments hinder their widespread adoption.

Indoor localization alternatives based on wireless communication, including WLAN, Bluetooth, and Ultra-Wideband (UWB), exhibit limitations in accuracy, scalability, energy efficiency, and cost, as highlighted by various studies [1, 2]. For instance, WLAN is notably susceptible to noise interference, Bluetooth faces constraints related to range and precision, and UWB has seen slow progress in standardization. Furthermore, with its focus on low-power communication and constrained range, also Zigbee has limited potential for accurate indoor positioning. These difficulties highlight the need for other MAV indoor positioning technologies to offer sufficient precision and dependability without relying on GNSS signals.

However, recent advances in mobile communications have paved the way for developing location-based services and applications that rely on accurate localization [3]. In particular, the deployment of fifth-generation cellular networks (5G New Radio, or 5G for short) has opened up new possibilities for indoor localization due to their characteristics, such as high bandwidth, low latency, advanced beamforming techniques, and enhanced coverage [4, 5, 6]. Small cell technologies like femtocells and picocells further facilitate comprehensive indoor coverage. Moreover, 5G has been tailored to cater to the diverse requirements of various industries, encompassing fields like industrial automation, asset tracking, and virtual and augmented reality (VR&AR). The positioning prerequisites outlined by the 3rd Generation Partnership Project (3GPP) span from meter-level accuracy to sub-decimeter precision for vehicle-to-everything (V2X) scenarios (as detailed in [7] and related references). In the mm-wave frequency range, 5G benefits from an increased likelihood of Line-Of-Sight (LOS) connections and broader bandwidth, contributing to an elevated positioning accuracy level. 5G employs a dedicated pilot signal termed the Positioning Reference Signal (PRS) for downlink positioning. This signal is utilized to gauge signal delay by cross-correlating the received PRS with a locally generated counterpart at the transmitter. The resulting delay, the so-called Time of Arrival (), is determined by identifying the peak correlation value between the two signals.

Therefore, while accomplishing precise and reliable MAV indoor localization continues to be challenging, primarily attributed to indoor environments’ intricate and dynamic nature as well as the highly dynamic motion of the MAV, our work proposes a novel approach to this problem capitalizing on 5G measurements. However, relying solely on 5G data may not yield adequate information for ensuring dependable indoor localization. To enhance reliability and precision, the integration of supplementary sensors becomes imperative. Therefore, we fuse 5G measurements with data obtained from an on-board inertial measurement unit (IMU), providing inertial accelerations and angular velocities of the MAV during flight. By integrating these two types of measurement using advanced optimization techniques, we can achieve accurate and robust real-time pose (i.e., position and orientation) estimations, even when the MAV is maneuvering through complex indoor environments. Fig. <ref> illustrates a typical example scenario in which a MAV has to navigate within a warehouse environment equipped with 5G base stations (BSs).

Thus, our research aims to fuse 5G with IMU measurements, enhancing real-time pose estimation for a flying MAV, specifically focusing on improvements in localization accuracy, scalability and adaptability, real-time performance, and integration with sensor fusion frameworks. This emphasis on drone localization distinguishes our work from studies primarily focused on the 5G side aspects, highlighting our contribution to advancing indoor drone localization techniques. Building upon our prior work  [8], this extended version proposes two novel algorithmic approaches to this sensor fusion problem, namely an Error State Kalman Filter (ESKF) and Pose Graph Optimization (PGO) using a factor graph. Within the ESKF framework, we establish models for the IMU error states and their corresponding covariances. These models facilitate the precise representation of error states as we acquire high-frequency IMU measurements. In the subsequent update stage, we then enhance the estimation process by seamlessly integrating the state estimate and error covariance with lower frequency 5G measurements. This integration mitigates errors and counteracts the intrinsic drift tendencies commonly associated with IMU-based estimations. In the PGO approach, we introduce a novel factor related to the 5G measurements and also apply the idea of IMU preintegration  [9]. We apply the GTSAM [10] framework to solve the resulting PGO problem.

Illustration of a MAV indoor localization example scenario using several 5G base stations.

To validate our solutions, we apply them to the widely used EuRoC MAV benchmark dataset [11], which comprises onboard visual-inertial sensor measurements and related ground truth pose data collected during a number of experimental MAV indoor flight sequences. Extending our previous work  [8], we augment six distinct sequences of these flight data (where we use the inertial measurements only and discard the vision data) with related simulated yet highly realistic 5G ToA measurements generated with the help of the QuaDRiGa (quasi-deterministic radio channel generator) channel simulator [12]. Herein, our simulations include three 5G indoor network configurations with a varying number of base stations (BSs) and assumed Line-Of-Sight (LOS) communication. To ensure precise channel modeling during the simulations, we incorporated the ground truth 6 Degrees-of-Freedom (DoF) pose data from the related EuRoC MAV datasets.

To summarize, the novel contributions provided by this paper are the following:

* To the best of our knowledge, this is the first derivation of two novel approaches where 5G and IMU measurements are fused for real-time pose estimation of a MAV during indoor flights: (1) an ESKF-based approach and (2) an approach based on PGO using factor graphs.

* We augment a part of the well-known EuRoC MAV benchmark dataset with related simulated 5G measurements. In order to obtain highly realistic simulated data, we use the ground truth pose data of six selected EuRoC MAV flight sequences in a simulation of a 5G indoor network with different configurations and number of BSs, and a very detailed communication channel model provided by the QuaDRiGa simulator.

* We present a thorough assessment and comparison of the two novel proposed approaches for MAV indoor pose estimation using the augmented EuRoC MAV benchmark dataset. Herein, we also systematically compare various 5G networks and communication settings across indoor environments. For the assessment, we utilize the two most popular metrics in Simultaneous Localization and Mapping (SLAM), i.e., the Absolute Trajectory Error (ATE) and the Relative Pose Error (RPE) [13].

* We finally show that both proposed approaches for MAV indoor pose estimation achieve high accuracy while the PGO-based approach considerably outperforms the ESKF-based approach in terms of accuracy.

§ RELATED WORKS

The literature on localization using 5G is relatively limited, especially when considering a dynamic target, i.e., the MAV in our case, and the integration of sensor fusion frameworks. The existing literature often relies on simplistic methodologies and scenarios. Ferre et al [14] compared localization accuracy for different combinations of the 5G network configurations (center frequency, sub-carrier spacing, and PRS comb size) in terms of the Root Mean Square Error (RMSE). Also, their study considered a stationary target and employed multilateration based on PRS-derived data from multiple BSs.
A study by del Peral-Rosado et al [15] explored the impact of positioning performance using a 5G network when BSs are linearly placed along a straight roadside. They utilized Gauss-Newton optimization and simulated a vehicle traveling at a constant velocity of 100 km/h on a highway. The study revealed an accuracy of less than 20-25 cm for a communication bandwidth of 50-100 MHz. Additionally, the researchers calculated the by determining the first correlation peak between the PRS and the received signal. Saleh et al [16] proposed a time-based position estimation by combining vehicle velocity information and 5G measurements. They evaluated their approach in a simulated urban canyon using Siradel's S_5GChannel simulator [17] and employed an Extended Kalman Filter (EKF) with a constant velocity model for sensor fusion. The study also analyzed the impact of the 5G geometrical setup on EKF position estimation. Another EKF-based positioning framework is proposed by Menta et al [18]. The authors leveraged the 5G Angle of Arrival () extracted from the communication signal of BSs equipped with multi-array antennas. By utilizing this information, they achieve sub-meter accuracy in localization.
Sun et al [19] studied localization by combining estimates from 5G BSs with measurements from GNSS satellites. The authors utilized the Taylor series to linearize the mathematical model. As post-processing, they applied a moving averaging to the raw position estimates to minimize errors. Klus et al [20] explored the fusion of beamformed RSS information with GNSS data using Neural Networks (NN), achieving meter accuracy. In a study by Talvitie
et al [21], a simulation-based system was proposed for positioning high-speed trains using 5G technology. The system utilized measurements from both the Angle of Departure () and from beamformed 5G synchronization signals. Using an EKF to track the train's position, the study found that -based positioning was more accurate than -based. However, combining both measurements improved accuracy, i.e., submeter accuracy could be achieved for more than 75% of the tracking time.

There are also works whose primary focus is on the communication side, emphasizing the analysis of signal characteristics, like channel estimation, or computing and using MUSIC (Multiple Signal Classification) algorithms, and then primarily considering a simplified multilateration scenario for localization.
For instance, Pan et al [22] estimated the and based on the channel state information (CSI) for indoor positioning. Then, using multilateration, they investigated the position accuracy. Shamaei and Kassas [23] proposed an opportunistic estimation approach that exploits 5G synchronization signals (SS) and the physical broadcast channel (PBCH), with broadcast in the downlink channel and without requiring any communication with the network. This block is transmitted periodically and is always on, even when the user is not connected to the network. The authors developed a software-defined receiver (SDR) to extract ranging information from these signals and derived the statistics of the code phase error in a multipath-free environment and in the presence of multipath. They also conducted experiments to evaluate the ranging performance of the proposed SDR with real 5G signals, achieving a ranging error standard deviation of 1.19 m. In a fingerprinting-based method, Zhang et al [24] utilized , , and multi-path effects to improve positioning accuracy. They created a fingerprint dataset using 5G and amplitude information for all paths obtained from channel state data. This dataset was used to train a deep neural network (DNN) as a position estimator. The authors reported an accuracy of approximately one meter, even in non-line-of-sight (NLOS) environments.
In a more advanced theoretical approach, Chu et al [25], Mendrzik et al [26] investigated both localization and radio mapping, where the multi-path information is used to estimate the positions of the vehicle as well as the reflectors (obstacles) in the environment simultaneously using factor graphs. Their work, however, lacks network simulations and fusion schemes.

An Error State Kalman Filter (ESKF) is typically used for highly nonlinear systems, and it can improve the accuracy and robustness of state estimation [27, 28, 29].
Yin et al [30] used the ESKF and Rauch–Tung–Striebel (RTS) smoother to fuse GNSS and IMU data for localization and achieved good results even in challenging environments. Similarly, Marković et al [31] proposed an ESKF-based multi-sensor fusion algorithm for UAV localization in indoor environments, which was able to accurately track the UAV's position using measurements from IMU, LiDAR, visual odometry, and UWB sensors. Mourikis and Roumeliotis [32] introduced the Multi-State Constraint Kalman Filter (MSCKF) as an algorithm for estimating the state of a vision-aided inertial navigation (VINS) system. The MSCKF is based on the ESKF and fuses visual and inertial measurements to estimate the vehicle's position, orientation, and velocity. Its performance has been evaluated using several datasets, including the EuRoC Mav dataset [11], and the results show that it outperforms previous VINS algorithms in terms of accuracy and robustness.

While filtering is a sequential approach to pose estimation, Pose Graph Optimization (PGO) is a batch-based approach that considers more information over time, leading to more accurate and robust pose estimates [33]. For example, Mascaro et al [34] proposed a PGO-based multi-sensor fusion approach for drone localization. It fuses visual-inertial odometry poses and GPS measurement to infer the 6 DoF pose of the robot in real-time. ORB-SLAM 2 [35] and ORB-SLAM 3 [36] are popular examples of PGO-based mobile robot localization systems that fuse images and IMU data. ORB-SLAM 2 is a real-time visual SLAM system that uses PGO to estimate the pose of a camera as it moves through an environment. ORB-SLAM 3 is a recent version of ORB-SLAM that includes several improvements, including a more efficient PGO implementation.

Unlike previous approaches that consider localization based on 5G data, we address indoor localization using a factor graph to model the relation among non-homogeneous sensor measurements. We also leverage the advanced IMU preintegration factor [9] to propagate the MAV's 6 DoF pose between two lower frequency measurements, obtaining 6 DoF pose estimates at a higher frequency. Furthermore, to establish a comprehensive benchmark for our graph-based method, we have also implemented an ESKF in this paper. This incorporation enables a direct and insightful comparison between the graph-based approach and the sequential ESKF technique, providing a well-rounded evaluation and comparison of the efficacy and performance of both localization methodologies.

To identify the key research gaps within the literature, we can summarize them as follows:

* Limited scope:

* Most existing methods focus on static targets or vehicles with constant velocity, while MAVs are dynamic with complex motion patterns.

* Existing studies often consider simplistic scenarios lacking real-world complexity.

* There's a lack of research on sensor fusion frameworks specifically designed for 5G-based MAV localization.

* Methodological limitations:

* Many studies rely on basic multilateration using signals from multiple base stations, neglecting advanced algorithms and sensor fusion techniques.

* Some approaches focus primarily on communication aspects, analyzing signals and estimating angles/times of arrival without fully addressing localization challenges.

* Existing works often use simplified mathematical models that don't accurately capture the complexities of real-world environments and MAV dynamics

* Accuracy and robustness limitations:

* Many methods achieve sub-meter accuracy, which might not be sufficient for precise indoor MAV operations.

* Limited research exists on methods that address the drift inherent in inertial measurement units (IMUs), crucial for MAV localization.

* Lack of benchmarking and comparison:

* Existing studies often use different datasets, metrics, and network configurations, making comparisons challenging.

* Few works directly compare different localization approaches, leaving unclear which methods perform best in specific scenarios.

Table <ref> provides a concise overview of existing research on 5G-based localization for MAVs, elucidating the principal gaps and limitations in prior approaches. Each study is outlined with details including the primary gap identified, utilization of 5G data, experimental/simulation approach, integration of sensor fusion, localization technique employed, and consideration of fixed or dynamic MAV targets.

Comparison of Related Works in 5G-based MAV Localization

References     Primary gap     5G data     Simulation/Experiment     Sensor Fusion     Technique     Dynamic Target

mygray [14]     Oversimplified scenario with fixed target         Simulation (Qaudriga)           Lateration

[15]      Simplicity of Approach, with constant velocity assumption            Simulation (Qaudriga)          Least square

mygray [16]     Lacks robustness and complexity for real-world MAV scenarios         Simulation (S-5GChannel)           EKF

[18]      Lacks sensor fusion, limiting accuracy and robustness           Experiment           EKF

mygray [19]     Simplicity of Approach, Lacks robustness and complexity         Simulation (Qaudriga)            Least Square

[20]      Achieves low accuracy (meter-level),  insufficient for many MAV applications           Simulation (ray tracing)           NN

mygray [21]     Simplicity of Approach, excluding real MAV data         Simulation           EKF

[22]      Focuses on communication aspects, neglecting sensor fusion and real-world complexities           Experiment           Triangulation

mygray [23]     Primarily communication-centric, lacking comprehensive sensor fusion for robust MAV localization         Experiment           Lateration

[24]      Limited to communication data without sensor fusion           Simulation           DNN

mygray [26, 25]     Exclude sensor fusion and real MAV data         Not network simulation, theoretical analysis           PGO

[30]      No 5G data, not relevant to the scope of 5G-based MAV localization           Simulation (non-5G data)            ESKF

mygray [31]     No 5G data, not relevant to the scope of 5G-based MAV localization        Experiment (non-5G data)           ESKF

[32]      No 5G data, not relevant to the scope of 5G-based MAV localization           Experiment (non-5G data)           ESKF

mygray [34]     No 5G data, not relevant to the scope of 5G-based MAV localization         Simulation (non-5G data)           PGO

[35, 36]     No 5G data, not relevant to the scope of 5G-based MAV localization         Dataset-based evaluation (non-5G data)           PGO

§ MATERIALS AND METHODS

§.§ 5G Signal Structure and PRS Fundamentals for Localization

This section introduces some preliminary concepts related to 5G that are essential for understanding the configurations and other related topics discussed in Section <ref>.

5G employs Orthogonal Frequency Division Multiplexing (OFDM) with the inclusion of a cyclic prefix, which is a guard band added to the beginning of each OFDM symbol to reduce inter-symbol interference. In the time domain, OFDM breaks down a high-speed stream of digital bits into multiple parallel slower-moving streams. In the frequency domain, OFDM divides a wideband signal into multiple narrowband subcarriers, each of which carries a small portion of the data. Subcarrier spacing, referring to the separation or interval between adjacent subcarriers within the frequency domain, is a crucial parameter in 5G Localization accuracy. 5G offers flexible options for subcarrier spacing, ranging from 15 kHz to 240 kHz, influencing the number of subcarriers within each narrowband subcarrier.

5G Frame Structure

In 5G's downlink transmission, a frame has a duration of 10 milliseconds, which is further divided into ten subframes, each lasting 1 millisecond. Fig. <ref> shows the time-domain structure of the 5G frame, illustrating its reliance on the subcarrier spacing. In the time domain, the choice of subcarrier spacing determines the number of slots in a subframe, each slot contains 14 OFDM symbols. As illustrated in this figure, subframes are subdivided into 1, 2, 4, 8, or 16 slots, depending on the chosen subcarrier spacing, all utilizing a standard cyclic prefix.

The resource grid is a two-dimensional structure where time is represented along one axis, and frequency is represented along the other (matrix of subcarriers and OFDM symbols). A resource grid is characterized by one subframe in the time domain and full carrier bandwidth in the frequency domain. This grid is used to allocate resources for communication between the base station (gNodeB) and user devices (User Equipment UE). Resource grids are further divided into resource blocks (RBs). Each RB is composed of a group of subcarriers. The number of subcarriers in an RB depends on the subcarrier spacing. For example, with 15 kHz subcarrier spacing, an RB contains 12 subcarriers. A Physical resource block (PRB) is the smallest unit of resource that can be allocated to a user in 5G. A PRB is defined as a group of consecutive subcarriers in the frequency domain and a group of consecutive OFDM symbols in the time domain. Finally, a resource element (RE) is the smallest unit of resource allocation in the 5G resource grid. Each RE contains one subcarrier and one OFDM symbol. Fig. <ref> shows the resource grid, PRBs, Rbs, RE, as well as the distribution of the reference signal PRS which will be explained in more detail in the next section.

Visualization of the 5G Resource Grid Structure, highlighting Resource Blocks (RBs) and Resource Elements (REs), showcasing the allocation of resources in Resource Grid.

For precise positioning 5G supports Positioning Reference Signals (PRS) in downlink. PRS are generated using Gold sequences, which are a type of pseudo-random code, enabling easy reproduction of the PRS at the receiver and resistance to interference. Gold sequences also have good cross-correlation properties, meaning that they have a low correlation with all other sequences in the same set of Gold sequences. PRS symbols then are generated using Quadrature Phase Shift Keying (QPSK) modulation and distributed across time and frequency in the resource grid. The PRS  pattern can be configured with parameters like starting resource element, periodicity, and density. The configuration depends on the specific requirements of the positioning application. One critical configuration that significantly impacts the estimation accuracy is the PRS bandwidth. A wider PRS bandwidth will result in more accurate and robust localization but at the cost of increased interference.

Downlink PRS utilizes a comb-like frequency structure by transmitting PRS within a subset of the sub-bands derived from the divided PRS bandwidth. The comb size N in 5G PRS dictates that every Nth subcarrier is used by PRS symbols, with configurable values of N (2, 4, 6, or 12). This effectively diminishes interference from neighboring base stations. It also helps to improve the correlation properties of the PRS signal, which leads to enhanced accuracy and robustness of estimation. PRS mutation is another feature in 5G PRS which can also decrease the interference of PRS from neighboring BSs. Fig. <ref> illustrates a Physical Resource Block (PRB) featuring PRS symbols configured with a comb-6 structure for two BSs. For more detailed information, we refer to [7]. Finally, before transmission, the PRS and data signals are OFDM modulated and a cyclic prefix is added to the beginning of each OFDM symbol.

At the receiver, the UE receives the signal, eliminates the cyclic prefix, and applies Fast Fourier Transform (FFT) to reconstruct all OFDM symbols in the frame, including PRSs. Assuming tight synchronization between the receiver and base station, the UE correlates the received PRS signal with the known PRS pattern. The UE identifies the peak of the correlation function, which corresponds to the of the signal from the base station. This process is repeated for all BSs from which signals are received.

PRS distribution in a physical resource block in 5G NR with two BSs using a comb-6 structure

§.§ Problem Definition

In this section, we formulate the MAV localization problem in 5G networks combining IMU data,  angular velocity and linear acceleration, with measurements, which provide the radial distance from multiple BSs. These measurements could then be fused to track the MAV motion by the estimation approaches described in the following sections.

It is worth noting that while recent breakthroughs in machine learning, like Variational Autoencoders (VAEs) and other learning-based approaches, hold promise for localization tasks, this study concentrates on model-based estimation techniques, specifically the ESKF and PGO, for a few key reasons:

* Data Availability, Offline Learning

* Learning-based methods often require large datasets for effective learning, which can be challenging to obtain, especially for unique environments or specific 5G deployments. Although transfer learning could mitigate this issue, it falls outside the scope of our current study due to the additional data needed for fine-tuning and the potential computational burden on resource-constrained drones. As such, we prioritized alternative approaches better suited to our real-time and resource-constrained scenario.

* Interpretability and Safety:

* In safety-critical applications like drone navigation, understanding the decision-making process behind localization is paramount. Model-based methods such as ESKF and PGO provide greater transparency in their calculations compared to learning-based methods, which can be black boxes and difficult to interpret. This transparency aligns better with the safety requirements of our application.

§.§.§ Sensing and State Representation for MAV Localization

We aim to determine the 3D location and orientation of the MAV's body center, which we align with the IMU frame $\Imu$, in the world fixed frame $\World$.  The set of state variables $\States$ contains the pose and velocity of the MAV.  Moreover, we need to estimate the time-variant biases of the IMU's gyroscope $\mathbf{b}^g \in \mathbb{R}^3$ and accelerometer $\bias^a \in \mathbb{R}^3$ to account for the IMU noise drift.

The state vector $\States$ can then be represented as:

\begin{equation}\label{states}
\States =
\begin{bmatrix}
\mathbf{q}^T & (\mathbf{b}^g)^T & \vel^T & (\bias^a)^T & \tran^T
\end{bmatrix}^T.
\end{equation}

Here, each transformation $\T = (\R,\tran) \in \SEthree$ is composed of a rotation $\R \in \SOthree$ and a translation $\tran \in \mathbb{R}^3$. This transformation matrix $\T$ effectively transforms the body frame $\Imu$ to the world frame $\World$, in which the base stations (BSs) are positioned. Additionally, the quaternion form of rotation, denoted as $\mathbf{q}$, is related to the rotation matrix $\R$ in the transformation $\T$.

§.§.§ IMU Measurements

IMU measurements are provided w.r.t. the $\Imu$ frame. Our approach involves a 6-axis IMU that measures the linear acceleration ${}_\imu\bar\acc$ and angular velocity  ${}_\imu\bar\rotvel$. The IMU real motion state $\{ {}_\imu\acc, {}_\imu\rotvel\}$ is altered by additive Gaussian white noise $\{\noise^{a}, \noise^{g}\}$ and slowly time-varying biases $\{\bias^a, \bias^g\}$ affecting respectively the accelerometer and gyroscope as defined by the following IMU measurement model [37, 32]:

\begin{equation}\label{eq:imu-model}
\begin{aligned}
=&\ {}_\imu\rotvel +\R^T \rotvel^G+ \bias^g + \noise^g, \\
= &\ \R^\trnsp\left( {}_\world\acc-\gravity +2\rotvel^G_\times\vel +[\rotvel^G_\times]^2 \tran\right)\\
&+ \bias^a + \noise^a \, %,\\
% \dot\bias^a = & \noise^{wa}\, ,\\
% \dot\bias^g = & \noise^{wg}\, ,
\end{aligned}
\end{equation}

where ${}_\imu\bar\rotvel$ and ${}_\imu\bar\acc$ denote the gyro and accelerometer measurements in the body frame,  and ${}_\world\acc$ is the acceleration of the body frame expressed in the world frame. The terms $\noise^a$ and $\noise^g$ represent the stochastic noise affecting accelerometer and gyroscopic measurement readings, respectively.

The term $\rotvel^G$ incorporates the effects of the earth's rotation, and the accelerometer measurements include the gravitational acceleration $\gravity$. The operator $\rotvel_\times$ denotes a skew-symmetric matrix, and for $\rotvel = (\rotvel_x, \rotvel_y, \rotvel_z)$, it is defined by:

\begin{equation*}
\rotvel_\times =  \begin{bmatrix}
0 & -\rotvel_z & \rotvel_y \\
\rotvel_z & 0 & -\rotvel_x \\
-\rotvel_y & \rotvel_x & 0 \\
\end{bmatrix}.
\end{equation*}

§.§.§ Time of Arrival () Measurements

In this context, we leverage measurements denoted as $\bar\rho = (\bar\rho_{1}, \ldots, \bar\rho_{K})$, where $K$ is the total number of 5G BSs. By multiplying the values with the speed of light $c$, we can deduce the distances to each BS, respectively. Let us represent these distances using the vector:

\[
\bar{\mathbf{d}} = (\bar{\mathbf{d}}_{1}, \ldots, \bar{\mathbf{d}}_{K})^T.
\]

This distance is determined by both the dynamic position of the drone and the fixed positions of the 5G BSs (Landmarks), defined by the equation:

\begin{equation}\label{eq:dist_eq}
\bar{\mathbf{d}}_k  =
\mathbf{d}_{k} + \noise^{\text{Dist}}_k =  \| \tran - \mathbf{L}_k\|_2  + \noise^{\text{Dist}}_k, \quad k \in \{1, ..., K\}.
\end{equation}

Here $\bar{\mathbf{d}}_{k}$ denotes the distance measurement between the drone and the $k$-th BS and  $\mathbf{L}_k \in \mathbb{R}^3$ denotes the 3D position of the $k$-th 5G BS.

Please note that unless explicitly stated, the explicit time index is omitted for brevity and all variables will be interpreted as values at time $t$.

§.§.§ Dynamic MAV model

Assuming the MAV has a rigid frame, we apply the dynamics of a rigid body to characterize its motion. The MAV's attached IMU allows us to use IMU measurements as inputs to the dynamic model. This results in the following dynamic model governing IMU states:

\begin{equation}\label{eq:state-model}
\begin{aligned}
\dot{\mathbf{q}} &= \frac{1}{2} \Omega\left(\rotvel(t)\right) \mathbf{q}(t),\\
\dot \bias^g &= \noise^{wg}(t),\\
\dot \vel &= {}_\world\acc, \\
\dot \bias ^a &= \noise^{wa}(t),\\
\dot \tran &= \vel,
\end{aligned}
\end{equation}

where $\noise^{wa}$ and $\noise^{wg}$ are random walk noise of IMU accelerometer and gyroscope biases, and

\begin{align}
\Omega(\rotvel) = \begin{bmatrix}
-\rotvel_\times & \rotvel \\
-\rotvel^T  & 0
\end{bmatrix}.
\end{align}

Taking into account the IMU measurement model described in  Eq. (<ref>) and applying the expectation operator $\hat{ }$, we obtain the following set of dynamic equations:

\begin{equation}\label{eq:post_error_state}
\begin{aligned}
\dot{\hat{\mathbf{q}}} &= \frac{1}{2} \Omega\left(\hat\rotvel(t)\right) \hat{\mathbf{q}}(t),\\
\dot {\hat\bias}^g &= \bf{0},\\
\dot{\hat\vel} &= \hat\R^T\hat\acc + \gravity - 2[{\rotvel^G}_\times]\hat\vel - [\rotvel^G_\times]^2 \hat\tran,   \\
\dot {\hat\bias} ^a &= \bf{0},\\
\dot {\hat\tran} &= \hat\vel,
\end{aligned}
\end{equation}

with $\hat \acc  = {}_\imu\bar\acc - \hat \bias ^a$ and $\hat \rotvel = {}_\imu\bar\rotvel - \hat \bias_g - \hat\R^T \rotvel^G$.
Note that we consider $\World$ as an inertial frame of reference, neglecting the effect of the Earth's rotation, i.e., $\rotvel^G = \bf{0}$.

§.§ Error State Kalman Filter for MAV Localization (Indirect Method)

The Extended Kalman filter (EKF) is a widely employed technique for estimating and tracking system states, particularly for non-linear system dynamics or measurement models. The EKF operates by linearizing the system dynamics and the observation (measurement) model around the current state estimate. Although the EKF performs accurately enough in many situations, its efficacy may diminish in highly nonlinear systems.

One effective approach is to utilize the error-state Kalman filter (ESKF) method for enhanced precision and confidence in estimations, as elucidated by [28]. ESKF entails approximating the error state, representing the disparity between the true and estimated states, rather than directly estimating the states themselves.  The rationale behind adopting an ESKF, also known as the indirect method, is that errors are typically smaller and exhibit more linearity than the states, making them well-suited for linear Gaussian filtering. This enables the ESKF to refine estimations with greater precision.  This process unfolds in two main steps, namely prediction and update.

During the prediction stage, the system state is forecasted using integration, disregarding minor disturbances and noise. However, this straightforward projection can result in the gradual accumulation of errors due to noise and disturbances. The ESKF simultaneously calculates a Gaussian estimate of the error state's distribution while integrating the nominal state to address these inaccuracies. This dual process refines our real-time understanding of the evolving errors. By identifying small, subtle signals within the system's behavior, the ESKF enhances state estimation accuracy.

The update step is triggered by the reception of the new measurements, a less frequent occurrence than prediction. These measurements serve to expose accumulated errors. Exploiting this new information, the ESKF refines the error state estimate, enhancing the comprehension of its distribution and contributing to higher accuracy. The error state is then augmented with the estimated state, enabling necessary adjustments. While the error state is reset to zero, its uncertainty is updated.

Table <ref> presents the key differences between the EKF and ESKF, highlighting the trade-off between accuracy and computational complexity. In summary, while  EKF directly estimates states and linearizes around the current estimate, leading to lower computational complexity, its accuracy may be compromised in highly non-linear systems. The ESKF, on the other hand, tackles this challenge by estimating the error between true and estimated states and linearizing around this error state. This approach achieves higher accuracy in non-linear scenarios, although at a slightly increased computational cost due to additional calculations and memory requirements. In both filters, the prediction stage utilizes linearized dynamics to forecast the state (ESKF predicts both error and nominal state), while the update stage refines the estimate using new measurements (ESKF updates error before refining the full state).

Comparison of Extended Kalman Filter (EKF) and Error State Kalman Filter (ESKF)

Feature     EKF     ESKF

mygray State Representation     Actual system states     Error between true and estimated states

Linearization Point     Current state estimate      Error state

mygray Suitability for Non-linearity     Less accurate     More accurate

Computational Complexity     Lower     Slightly higher

mygray Prediction Stage     Predicts system state based on linearized dynamics     Predicts error state evolution & nominal state

Update Stage     Updates state estimate directly	     Updates error state estimate & refines state through error augmentation

§.§.§ Prediction Step

Having the states $\States$ defined in  Eq. (<ref>), the error state is defined as per [32]:

\begin{gather}\label{eq:err-state}
\delta\States =
\begin{bmatrix}
\delta \theta^T & (\delta\bias^g)^T & \delta\vel ^T & (\delta\bias^a )^T & \delta\tran^T
\end{bmatrix}^T.
\end{gather}

Herein, $\delta x = x- \hat x$, where $x$ represents the true value, and $\hat x$ is the estimated value. However, the quaternion error is defined as $ \mathbf{q} = \delta \mathbf{q} \otimes \hat {\mathbf{q}}$, with $\mathbf{q}$ being a real value and $\hat{\mathbf{q}}$ as the estimated one. To effectively represent slight rotational errors, we adopt the following approximation:

\begin{gather}
\delta \mathbf{q} \simeq \begin{pmatrix}
\frac{1}{2}\delta \theta^T & 1
\end{pmatrix}^T.
\end{gather}

This approximation serves to simplify the representation of minor rotational errors within quaternion-based state estimation.

Note that we explicitly estimate the IMU biases states within the overall state vector($\delta\States$). This allows the filter to learn and compensate for these constant offsets over time. In the error state equation, bias state dynamics would be modeled as:
$\dot{\delta\bias^g} \approx 0$ and $\dot{\delta\bias^a} \approx 0$ (assuming random walk noise is negligible).

Employing the minimal representation provided by $\delta \theta$, it reduces computational complexity while simultaneously maintaining accuracy.

Based on  Eq. (<ref>) and  Eq. (<ref>), the error state equation can be expressed in the following form:

\begin{gather}\label{eq:dynamic_error_state}
\delta\dot {\States} = \F \delta \States +  \G \noise^{\text{IMU}},
\end{gather}

where $\noise^{\text{IMU}} = \begin{bmatrix}
(\noise^g)^T & (\noise^{wg})^T & (\noise^a)^T & (\noise^{wa})^T
\end{bmatrix}^T$, $\F$, and $\G$ are given as follows:

\begin{gather*}
\F = \begin{bmatrix}
-[\hat\rotvel_\times] & -I_3  & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\
-\hat\R^T[\hat\acc _\times] & 0_{3\times3} & -2[\rotvel^G_\times] & -\hat\R^T & -[\rotvel^G _\times]^2 \\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\
0_{3\times3} & 0_{3\times3} & I_3 & 0_{3\times3} & 0_{3\times3} \\
\end{bmatrix},
\end{gather*}

\begin{gather*}
\G = \begin{bmatrix}
-I_3 & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\
0_{3\times3} & I_3 & 0_{3\times3} & 0_{3\times3} \\
0_{3\times3} & 0_{3\times3} & -\hat\R^T & 0_{3\times3} \\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & I_3 \\
0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 0_{3\times3} \\
\end{bmatrix},
\end{gather*}

where $I_3 \in \mathbb{R}^{3 \times 3}$ denotes the Identity matrix.

In the discrete implementation, the IMU captures measurements at discrete time intervals, namely $\bar\rotvel_t$ and $\bar\acc_t$, which are then employed to propagate the state and covariance matrix. The ESKF is initialized with the initial state estimate and covariance matrix. Upon the arrival of a fresh IMU measurement, the IMU-derived state estimate undergoes propagation through a  numerical integration, effectively accounting for the dynamic model of the system as specified by  Eq. (<ref>). The covariance matrix is also propagated using a numerical integration of the Lyapunov equation:

\begin{gather}\label{eq:lyapunov}
\dot \Pcov  = \F \Pcov + \Pcov \F^T +\G \mathbf{Q}^{\text{IMU}}\G^T.
\end{gather}

Note that the IMU noise characteristics are represented by a noise covariance matrix ($\mathbf{Q}^{IMU}$) tailored to the specific sensor used. This matrix captures the variances and correlations of different noise sources (gyroscope noise, accelerometer noise, etc.).
Accurate estimation of $\mathbf{Q}^{IMU}$, whether through empirical measurements or manufacturer specifications, is crucial to precise noise modeling.
The $\mathbf{G}$ matrix in the error state equation (<ref>) acts as a gain matrix, mapping IMU noise directly into the error state dynamics. Each non-zero entry in $\mathbf{G}$ indicates how a specific noise component influences a particular state error. For example, gyroscope noise primarily affects the attitude error, while accelerometer noise impacts both attitude and velocity errors. Furthermore, in the propagation of the covariance matrix, the process noise term ($\G \mathbf{Q}^{IMU}\G^T$) in the Lyapunov equation (<ref>) drives the growth of the state covariance matrix ($\Pcov$).
As the covariance matrix increases, it reflects the increasing uncertainty in the state estimates due to IMU noise accumulation

§.§.§ Update Step

Upon receiving Time of Arrival (ToA) measurements, we proceed with the update stage. Given  Eq. (<ref>), we define the measurement function $h(\States)$ as follows:

\begin{gather*}
h(\States) = \begin{bmatrix}
\| \tran - \mathbf{L}_1\|_2 \\
\vdots\\
\| \tran - \mathbf{L}_K\|_2 \
\end{bmatrix} \in \mathbb{R}^{K \times 1}.
\end{gather*}

Now the residual $\residual^\rho \in \mathbb{R}^{K\times 1}$ is formulated as:

\begin{equation}
\begin{aligned}
\residual^\rho = \: & \bar{\mathbf{d}} - h(\hat\States) =  h(\States) - h(\hat\States)+ \noise^{\text{Dist}}  \\ =\: & h(\hat\States+\delta\States) -h(\hat\States)+ \noise^{\text{Dist}}\\
\approx \: & \mathbf{H} \delta\States+ \noise^{\text{Dist}},
\end{aligned}
\end{equation}

where $\noise^{\text{Dist}}$ represents the vector of noise associated with the distance measurements and the measurement Jacobian matrix $\mathbf{H}$ is defined by:

\begin{equation}
\begin{aligned}
\mathbf{H} = & \frac{\partial \residual^\rho}{\partial(\delta\States)}|_{ \hat\States} \\
= & \begin{bmatrix}
0_{1\times 12} & \frac{\hat\tran_x -L_{1x}}{d_1} & \frac{\hat\tran_y -L_{1y}}{d_1} & \frac{\hat\tran_z -L_{1z}}{d_1}\\
\vdots & \vdots & \vdots & \vdots  \\
0_{1\times 12} & \frac{\hat\tran_x -L_{Kx}}{d_K} & \frac{\hat\tran_y -L_{Ky}}{d_K} & \frac{\hat\tran_z -L_{Kz}}{d_K}
\end{bmatrix}_{k \times 15}
\end{aligned}
\end{equation}

Here  $L_k = (L_{kx}, L_{ky}, L_{kz})$, for $k \in {1, ..., K}$ represent the position of the k$th$ base station.

The update rules at time $t$ will be:

\begin{align}
% \mathbf{S}_t  = & \mathbf{H} \mathbf{P}_{t|t-1} \mathbf{H}^\intercal + \mathbf{R}_t \\
\mathbf{K}_t = & \mathbf{P}_{t|t-1} \mathbf{H}^T  \left(\mathbf{H} \mathbf{P}_{t|t-1} \mathbf{H}^T + \mathbf{R} \right)^{-1},\\
\delta\hat\States_{t|t} =&  \Kgain_t \residual^\rho_t,\\
\Pcov_{t|t} = & (\mathbf{I} - \Kgain_t \mathbf{H}) \Pcov_{t|t-1}.
\end{align}

In these equations, $\mathbf{R}$ represents the covariance matrix of the distance measurement noise. The process involves calculating the Kalman Gain $\mathbf{K}_t$, updating the error state estimate $\delta\hat\States_{t|t}$ and the error covariance matrix. Finally, $\delta\hat\States_{t|t}$ is appropriately incorporated into the predicted state $\hat\States_{t|t-1}$ based on the specific calculations for each component of the state vector. This step refines the estimated state using ToA measurements and their associated uncertainties.

The ESKF algorithm's core steps are outlined in the pseudocode presented in <ref>, serving as an overview of the ESKF implementation, helping to grasp the algorithm's logical flow and key components.

ESKF-based Localization

IMU and ToA measurements

state estimate $\hat{\States}$

Initialize: $\hat{\States}_0$, $\Pcov$

new IMU measurement received

Prediction Stage:

Propagate state using Runge-Kutta integration
Propagate covariance matrix using Runge-Kutta integration

new ToA measurements received

Update Stage:

Obtain distance measurements $\mathbf{\bar{d}}_t$
Calculate residual $\residual^\rho_t \leftarrow \mathbf{\bar{d}}_t - h(\hat{\States}_{t|t-1})$
Calculate Jacobian matrix $\mathbf{H}_t$ for measurement model
Calculate Kalman Gain $\mathbf{K}_t$
Update error state estimate $\delta\hat{\States}_{t|t} \leftarrow \mathbf{K}_t \residual^\rho_t$
Update covariance matrix $\Pcov_{t|t} \leftarrow (\mathbf{I} - \mathbf{K}_t \mathbf{H}_t) \Pcov_{t|t-1}$
Update state estimate $\hat{\States}_{t|t} \leftarrow \hat{\States}_{t|t-1} \oplus \delta\hat{\States}_{t|t}$

§.§ Pose Graph Optimization (PGO)

As a second alternative method to estimate the drone's 6DoF pose we also leverage graph-based optimization techniques. This method models the relationships between the pose variables based on sensor measurements and then performs the estimation using least squares optimization. For that purpose, a factor graph model [33] is created to abstract the problem. Factor graphs are bipartite graphs with two types of nodes, namely variables and factors, where factors represent functions on subsets of the variables. Edges in the factor graph between a factor and a set of variables indicate that the factor depends only on those variables. In our case, the variables are the state variables that should be estimated at certain instants and the factors correspond to the likelihood of the adjacent state variable nodes given the related measurements, see the structure of the resulting factor graph in Fig. <ref>. The figure illustrates a factor graph used for optimizing variables, with circles representing variables and squares representing factors. Nodes $T_t$ represent pose variables, $v_t$ represent velocity, and $b_t$ represent bias. IMU pre-integration factors connect these nodes, while ToA measurements create range factors. Prior factors constrain initial values.

Therefore, the factor graph can be used to specify the joint posterior probability density $p(\mathcal{X}|\mathcal{Z})$ of the whole set of state variables $\mathcal{X}$ and the whole set of measurements $\mathcal{Z}$ as the product of all factors $f_i(\mathcal{X}_i)$ in the graph

\begin{equation}
\label{eq:fg}
p(\mathcal{X}|\mathcal{Z})  \propto \prod_{\forall f_i\in \mathcal{F}} f_i(\mathcal{X}_i) \, ,
\end{equation}

where $\mathcal{X}_i$ is the set of state variable nodes adjacent to the factor $f_i(\mathcal{X}_i)$ and $\mathcal{F}$ is the set of all factors in the graph. Herein, the measurements are no longer explicitly represented but simply become a fixed parameter in the corresponding factor. This factorization will be exploited for pose estimation as described in the following.

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 $\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. 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 $\T_1$, $\mathtt{v}_1$, and $\mathtt{b}_{1}$ to constrain them with their initial values in the trajectory.

§.§.§ IMU Factor

Due to the IMU's higher sampling frequency than other sensors, it typically captures multiple measurements between two instances. The IMU factor is constructed utilizing a  preintegrated measurement [9] constraining the relative motion increments. Especially, we obtain the condensed measurements $\preintRmeas_{ij}$ of rotation, $\preintPmeas_{ij}$ of position, and $\preintVmeas_{ij}$ of velocity by integrating multiple IMU readings $\{{}_\imu\bar\acc_t, {}_\imu\bar\rotvel_t : \forall t \in [t_i, t_j]\}$. So, we can define the residual terms $\residual$ for the states $\{\R_{ij}, \tran_{ij}, \vel_{ij}\}$:

\begin{align}
\residual_{ij}^{\R}
= &\ \textstyle \logmap\left( \preintRmeas_{ij}^\trnsp\
\R_i^\trnsp \R_j  \right)\, ,\\
\residual_{ij}^{\tran}
= &\ \textstyle  \R_i^\trnsp \big( \tran_j-\tran_i - \vel_i\Delta t_{ij}
- \frac{1}{2}\gravity \Delta t_{ij}^2\big)  -\preintPmeas_{ij}\, ,\\
\residual_{ij}^{\vel}
= &\ \textstyle \R_i^\trnsp \left(\vel_j-\vel_i - \gravity\Delta t_{ij}\right)-\preintVmeas_{ij} \, ,
\end{align}

where $\Delta t_{ij} = t_j - t_i$ is the total time interval with $t_i < t_j$. Also, $\logmap : \SOthree \rightarrow \Real^3$ defines the logarithm map that associates elements of the rotation manifold $\SOthree$ to vectors on the Euclidean tangent space $\Real^3$ representing rotation increments.

Regarding the biases, the total residual $\residual_{ij}^{\bias}$ between time $t_i$ and $t_j$ is set as follows:

\begin{equation}
\residual_{ij}^{\bias} =\ \bias^{g}_j - \bias^{g}_i + \bias^{a}_j - \bias^{a}_i  \, .
\end{equation}

§.§.§ Range Factor

By multiplying the estimated values $\rho_{sk}$ by the speed of light $c$,  $d_{sk} = \rho_{sk} \cdot c$,

we obtain $\mathrm{K}$ metric distance measurements $d_{sk}\in \Real$ of the drone to the $k$-th BS at location $\mathbf{L}_k$ at time $t_s$. Notably, we explicitly express the possibility of having fewer measurements than the number of tracked poses. The residual $\residual_{sk}^{\rho}$ of the factor at time $t_s$ with the BS $\mathbf{L}_k$ is defined as:

\begin{equation}
\residual_{sk}^{\rho} = d_{sk} - \| \tran_s  - \mathbf{L}_k\|_2 \, .
\end{equation}

§.§.§ Optimization

The pose graph optimization problem is formulated as Maximum a Posteriori (MAP) estimation that involves finding the state $\mathcal{X}^*$ that maximizes the posterior:

\begin{equation}
\label{eq:map0}
\mathcal{X}^* = \argmaxA_{\mathcal{X}}  p(\mathcal{X}|\mathcal{Z}) .
\end{equation}

Considering the proportional relationship in  Eq. (<ref>),  Eq. (<ref>) is equivalent to the maximization of the product of all factors in the factor graph:

\begin{equation}
\label{eq:map}
\mathcal{X}^* = \argmaxA_{\mathcal{X}} \prod_{\forall f_i\in \mathcal{F}} f_i(\mathcal{X}_i)\, .
% \argmaxA_{\mathcal{X}} p(\mathcal{X}_0) \prod^{\mathrm{N}}_{t=1}\prod^{}_{\mathbf{z}_t^f \in \mathcal{Z}_t^f} p(\mathbf{z}_t^f|\mathbf{x}_t) \, ,
\end{equation}

In our application, the factor graph evolves with time, where we consider $t=\{1,\ldots,N\}$ instants and $\mathcal{X}_t$ is the state at instant $t$. Therefore, the overall set of states is $\mathcal{X} = \{\mathcal{X}_1,\ldots, \mathcal{X}_N\}$ starting with a given initial state $\mathcal{X}_0$. The factors are likelihoods derived from the respective previously described measurements, assumed to be corrupted by zero-mean, normally distributed noise. Now we denote with $f_{ti}$ the factors between state $\mathcal{X}_t$ and the state $\mathcal{X}_{t+1}$ and, if measurements are available, the factors between state $\mathcal{X}_t$ and the respective BSs. Taking the negative log of  Eq. (<ref>) finally leads to the minimization of the sum of the respective residuals in the following form:

\begin{equation}
\mathcal{X}^* =\ \argminA_{\mathcal{X}} \left\|\residual_0\right\|^2_{ \mathbf{\covprior}_0} + \sum^\mathrm{N-1}_{t=1} \sum^{}_{\forall f_{ti}\in{\mathcal{F}}}
\left\|\residual_{f_{ti}}\right\|^2_{ \mathbf{\covprior}_{f_{ti}}} \, ,
\end{equation}

where $\left \| \mathbf{r} \right\|_\mathbf{\covprior}^2 = \mathbf{r}^\trnsp \mathbf{\covprior}^{-1}{\mathbf{r}}$ is the squared Mahalanobis norm, and $\residual_{f_{ti}}$ are the residual functions related to the aforementioned factors $f_{ti}$ with covariance matrix $\mathbf{\covprior}_{f_{ti}}$. We denote with $\residual_0$ the residual derived from the prior on the initial pose with $\mathbf{\covprior}_0$ being its covariance matrix.

To efficiently solve the MAP optimization problem, we utilize the iSAM2 (Incremental Smoothing and Mapping 2) iterative optimization algorithm [38] implemented in GTSAM [10]. This algorithm can automatically identify the variables that require linearization at each step, and it enables us to keep our graph solution updated while adding new nodes without experiencing memory overload.

iSAM2 effectively leverages a Bayesian tree structure to incorporate historical data during optimization, but its performance can be impacted by the growing tree depth for extended temporal horizons. In scenarios where both long-term memory retention and real-time performance are critical, [39] offers a promising solution that is based on a spanning tree-based hierarchy.  This method leverages a simplified spanning tree representation of the pose graph, reducing complexity. It also employs a coarse-to-fine optimization strategy, achieving faster convergence by first optimizing on supernodes formed by connected components and then refining within each supernode using the original measurements. While this approach introduces a slight accuracy approximation, its reduced complexity and potential for real-time performance make it a valuable option for specific applications.

Within <ref>, we have encapsulated the essence of graph-based state estimation in a streamlined manner.

IMU and ToA measurements
Optimized state estimates $\mathcal{X}^*$
Initialize: $\mathcal{X}_0^* = \mathcal{X}_0$
each fixed time interval
Accumulate IMU measurements
preintegrate accumulated IMU measurements
Add a new node
Create an IMU factor linking the new node to the previous one
New ToA measurement arrives
Determine the temporally nearest node to the ToA measurement.
Incorporate a range factor between the node and the relevant landmark

Perform optimization using iSAM2
Update the state estimates $\mathcal{X}^*$

Graph-Based Localization

§ EVALUATION AND RESULTS

In order to evaluate the two derived approaches for pose estimation, we intend to apply them as far as possible to real experimental data. For this purpose we selected the EuRoC MAV dataset [11] that comprises vision data and inertial sensor measurements as well as the related ground truth pose data collected during a number of MAV indoor flight sequences. The dataset is widely used in the literature as a benchmark for the evaluation of visual-inertial odometry or SLAM solutions. However, while it provides the inertial measurements for our evaluation, it does so far not include the necessary 5G ToA data. Therefore, we had to augment the EuRoc MAV dataset with such suitable data that could either be generated experimentally or via simulations. However, an experimental generation of these 5G data would not only require an exact repetition of the flights with the same equipment and configurations, but also the setup of a configurable 5G indoor network and an on-board 5G receiver with ToA measurements. While such 5G networks (e.g., 5G femtocells) are foreseen for the near future by many mobile communications equipment suppliers, it is currently still a major challenge to acquire them commercially and install them in an academic lab environment.

Therefore, we decided to generate the missing 5G ToA data via a suitable simulation environment. In the following, we describe in more detail our approach to generate such simulated yet highly realistic data and finally present our assessment based on the augmented EuRoC MAV dataset.

§.§ Augmenting the EuRoC MAV dataset with simulated 5G data

Fig. <ref>  shows the overall structure of the simulation environment with its core components and their interaction for the augmentation of the EuRoC MAV dataset. In addition, it shows the connection of the resulting dataset with the pose estimators for the assessment.

System Architecture for Augmentation of EuRoC MAV Dataset with 5G Simulation.

Herein, the main components are:

* EuRoC MAV dataset: provides inertial measurements and ground truth pose data from real MAV flights.

* Signal generation: Generates the 5G signals as emitted from a number of simulated BSs placed at defined locations.

* QuaDRiGa channel simulation: Simulates the propagation of the 5G signals from the BSs with their respective fixed poses through the environment to a supposed flying MAV with its current pose provided by the EuRoC MAV ground truth data.

* Calculation: Calculates the current measurements as provided by a supposed MAV on-board 5G receiver by analyzing the received 5G signal, and stores them as a dataset.

* Pose estimator: Estimates the pose of the MAV using the stored simulated measurements and the inertial measurement from the EuRoC MAV dataset using one of the two pose estimators (i.e., ESKF or PGO) as derived in Sections <ref> and <ref>, respectively.

The EuRoC MAV dataset [11] was collected by an indoor flying MAV equipped with a stereo-camera module capturing images at 20 Hz and a calibrated IMU providing inertial measurements at a rate of 200 Hz. The dataset also containes the MAV's position and orientation data obtained through the Vicon motion capture system as ground truth, recording the full 6DoF at approximately 100 Hz. The full set of calibrated rigid transformations between sensors and the Vicon system is also given. The EuRoC MAV dataset comprises multiple flight sequences while our study examines all six sequences from the Vicon Room 1 and Vicon Room 2 datasets.

To estimate the distance to 5G BSs, we generate the 5G signal, including PRS and Physical Downlink Shared Channel (PDSCH) resources transmitted by each BS. We use the MATLAB 5G Toolbox to generate resource blocks for these signals, setting the transmit power to 0 dBm (1 mW) and the SNR to 10 dB, which is a conservative value, as real-world scenarios often have higher SNR levels.

We then employ the QuaDRiGa (quasi-deterministic radio channel generator) channel simulator [12] to create an impulse response that emulates the wireless channel characteristics based on specific network configurations, given receiver and transmitter positions, orientations, velocities, and the chosen indoor environment. QuaDRiGa is a realistic channel simulator that has been validated through extensive measurements. For the MAV trajectories we have used the available 6DoF ground truth poses of the MAV provided by the EuRoC MAV dataset, from which we compute the required velocity considering the translation vectors between two time-consecutive poses. We virtually place two up to five fictitious BSs in the room where the trajectory is recorded. The positions of the BSs in the EuRoC MAV Vicon system's coordinate frame are $\mathrm{BS}_1 = (-10, -7, 2)$, $\mathrm{BS}_2 = (7, 13, 3)$, $\mathrm{BS}_3 = (25, -35, 4)$, $\mathrm{BS}_4 = (-6, 9, 5)$, $\mathrm{BS}_5 = (-4, -14, 6)$. These values are used to initialize the corresponding state variables of the optimization problem with a small covariance.

We consider three QuaDRiGa channel simulator scenarios, each operating at different frequencies, to simulate the wireless communication environment realistically: QuaDRiGa-Industrial-LOS for 5 GHz, 3GPP-38.901-Indoor-LOS for 28 GHz, and mmMAGIC-Indoor-LOS for 78 GHz.

* QuaDRiGa_Industrial_LOS [40]: This scenario is designed to replicate a line-of-sight (LOS) environment for industrial applications. The simulation is optimized for frequencies ranging from 2 to 6 GHz and aims to capture the radio propagation behavior typically observed in automation industry halls. The scenario is validated through measurements conducted across five factory halls within Siemens' Nuremberg operational premises.

* 3GPP_38.901_Indoor_LOS [41]: This scenario simulates an indoor environment with a 0.5-100 GHz frequency in LOS conditions. It aims to encompass various indoor deployment scenarios, such as those commonly found in office buildings and shopping centers. These indoor settings typically consist of open cubicle spaces, private enclosed offices, common areas, corridors, etc.

* mmMAGIC_Indoor_LOS [42]: This is designed specifically for frequencies in the range of 6-100 GHz and indoor scenarios with LOS like traditional enclosed offices, semi-closed offices (cubicle areas), and open offices.

We also assumed that both the receiver and all transmitters used omnidirectional antennas. Other configurations are detailed in Table <ref>. It includes information such as frequency band, bandwidth, subcarrier spacing, number of resource blocks (RBs), comb size, signal-to-noise ratio (SNR), and cyclic prefix type for each 5G scenario.

5G system configurations.

5G Sim. Scenario     Freq (GHz)     Bw (MHz)      Subcarrier Spacing (KHz)     number of RBs     comb size     SNR      cyclic prefix

QuaDRiGa-Industrial-LOS     5 (FR1)     100      30     275      6     10 dB     normal

3GPP-38.901-Indoor-LOS     28(FR2)    200      60     275      6     10 dB     normal

mmMAGIC-Indoor-LOS     78(FR2)    400      120      275     6     10 dB     normal

We finally convolve the transmitted signal with the impulse response to replicate the effects of the transmission environment, generating the received signal. We generate the received signal at the receiver every 0.2 seconds, which enables us to calculate the at a frequency of 5 Hz. We then correlate the received signal with the transmitter's PRS pattern and calculate the delay by analyzing the correlation profile. Typically, the initial or highest peak is considered as the response. However, this approach can be compromised by noise or when the LOS coefficient is weaker than the multipath coefficient. To address this, we set a threshold to eliminate values below it and choose the first peak above the threshold as the response.  We have empirically determined that a threshold of 0.2 is optimal for estimation with the generated data. An example of a correlation profile in the simulation is displayed in Fig. <ref>. The response shown in green is neither the first nor the maximum peak. Still, a suitable threshold allowed the selection of the first peak as the response. <ref> gives the statistics of the error in the resulting estimated distance to each BS.

PRS Correlation Profile

Error statistics of the estimated distance to BSs for different scenarios and given in meters.

in 2, 4, ..., 36

Dataset       5G Sim. Scenario       Statistic     #1       #2       #3       #4       #5

6-1V101   2-1QuaDRiGa-Industrial-LOS     Mean       0.129      -0.045      0.006      -0.081      -0.023

Std.     0.568     0.81     0.763     0.872     0.718

2-13GPP-38.901-Indoor-LOS     Mean       -0.024      -0.021      -0.059      0.041      -0.06

Std.     0.344     0.368     0.352     0.394     0.369

2-1mmMAGIC-Indoor-LOS     Mean       0.002      0.01      -0.008      0.003      -0.01

Std.     0.185     0.171     0.173     0.159     0.176

6-1V102   2-1QuaDRiGa-Industrial-LOS     Mean       0.16      -0.033      -0.135      -0.129      -0.156

Std.     0.645     0.874     0.722     0.739     0.677

2-13GPP-38.901-Indoor-LOS     Mean       -0.104      0.104      0.106      -0.122      -0.052

Std.     0.358     0.39     0.404     0.367     0.322

2-1mmMAGIC-Indoor-LOS     Mean       0.037      -0.011      -0.018      0.016      -0.046

Std.     0.174     0.153     0.154     0.16     0.193

6-1V103   2-1QuaDRiGa-Industrial-LOS     Mean       0.043      -0.065      1.232      -0.066      -0.387

Std.     0.775     0.784     1.628     0.772     1.275

2-13GPP-38.901-Indoor-LOS     Mean       -0.042      0.053      0.008      -0.033      -0.022

Std.     0.353     0.382     0.387     0.36     0.369

2-1mmMAGIC-Indoor-LOS     Mean       0.001      -0.011      0.012      0.0      -0.013

Std.     0.176     0.166     0.17     0.18     0.183

6-1V201   2-1QuaDRiGa-Industrial-LOS     Mean       0.059      0.108      -0.182      -0.154      -0.27

Std.     0.751     0.897     0.592     0.986     0.79

2-13GPP-38.901-Indoor-LOS     Mean       0.025      -0.07      -0.045      0.054      0.12

Std.     0.364     0.379     0.392     0.302     0.367

2-1mmMAGIC-Indoor-LOS     Mean       -0.012      0.026      0.015      -0.019      0.015

Std.     0.164     0.177     0.163     0.18     0.192

6-1V202   2-1QuaDRiGa-Industrial-LOS     Mean       0.027      0.141      0.072      0.082      -0.204

Std.     0.716     0.674     0.908     0.933     0.631

2-13GPP-38.901-Indoor-LOS     Mean       0.052      -0.053      -0.039      0.012      0.043

Std.     0.391     0.348     0.427     0.359     0.351

2-1mmMAGIC-Indoor-LOS     Mean       -0.007      0.007      -0.018      0.013      0.011

Std.     0.178     0.168     0.17     0.19     0.192

6-1V203   2-1QuaDRiGa-Industrial-LOS     Mean       0.067      -0.017      0.273      -0.13      -0.045

Std.     0.754     0.73     1.343     0.724     0.684

2-13GPP-38.901-Indoor-LOS     Mean       0.009      -0.022      -0.046      0.043      0.02

Std.     0.376     0.351     0.317     0.37     0.358

2-1mmMAGIC-Indoor-LOS     Mean       0.018      0.017      0.002      0.004      -0.009

Std.     0.17     0.181     0.174     0.178     0.174

§.§ Evaluation Metrics

For the evaluation of our approach, we utilize the two most popular metrics in Simultaneous Localization and Mapping (SLAM): Absolute Trajectory Error (ATE) and Relative Pose Error (RPE) [13]. ATE is a metric for global consistency, defined as the Root Mean Square Error (RMSE) between the absolute distances of the estimated and ground truth trajectories. RPE complements ATE by focusing on local accuracy and is primarily utilized for evaluating odometry systems. It provides a concise measure of the error between consecutive poses in the estimated trajectory compared to the ground truth trajectory. RPE helps to assess the drift or error accumulation in pose estimation over time, specifically at smaller time intervals.

In addition to these metrics, we analyzed the RMSE for each trajectory coordinate axis, denoted as $E_{a}$ where $a$ represents $x$, $y$, or $z$. This calculation was carried out to assess the accuracy of the estimation along specific spatial directions. The $\RMSE$ error for each coordinate axis $a \in \{ x, y, z\}$ was calculated using the following formula:

\begin{equation*}
E_{a} = \sqrt{\frac{1}{N}\sum_{i=1}^{N}(a_{i,\text{est}} - a_{i,\text{gt}})^2}, \quad a \in \{ x, y, z\}
\end{equation*}

In this equation, $N$ represents the total number of poses, while the subscripts $(i, \text{est})$ and $(i, \text{gt})$ correspond to the $i$-th estimated and ground truth poses, respectively. This analysis gave us valuable insights into potential accuracy variations dependent on the spatial direction. By evaluating the error along each axis separately, we could identify any discrepancies in performance for different spatial orientations.

To further assess the suitability of the proposed algorithms for real-time applications, we evaluated their implementation times. For PGO, we calculated the average and standard deviation (std) of the time taken for each optimization process. For the ESKF, we recorded the average and std of the time taken to complete one prediction plus update state cycle.

§.§ Results

The experiments were performed on a Ubuntu 20.04 laptop with an Intel(R) Core(TM) i9-10885H CPU @ 2.40GHz with 16 cores and 32 Gb of RAM. All codes (PGO and ESKF) are implemented in Python, utilizing the relative interface of GTSAM v4.0 for creating and optimizing the factor graphs.
The drone's position and orientation results are obtained from the factor graph based on the final Maximum A Posteriori (MAP) estimate for each node, where nodes are consistently generated at 10 Hz, twice the frequency of measurements. On the other hand, the results for the Error State Kalman Filter (ESKF) approach are acquired at each update stage, coinciding with the reception of measurements. To comprehensively evaluate the performance of the localization algorithm, error metrics are computed by establishing a comparison between the ground truth Vicon pose and the estimated pose that is temporally closest.

The detailed results for the graph-based and ESKF methods are exhaustively documented in <ref> and <ref>, corresponding to Vicon Room 1 and Vicon Room 2, respectively. These tables provide comprehensive information encompassing ATE, RPE, and translation RMSE for each distinct motion direction in various 5G simulation scenarios for both methods.  Additionally, the ATEavg. column presents the average ATE for each method across each scenario with 2, 3, 4, and 5 base stations.

Subsequently, we delve into a detailed analysis of the obtained results, uncovering the underlying factors contributing to the observed performance trends.

* Base Stations and Bandwidth Influence: The results consistently show that the accuracy of -based localization improves when the number of base stations and the available bandwidth (communication scenarios) increase. This is because more base stations provide more reference points for measurements, and higher bandwidth allows for higher resolution measurements. For example, using dataset V101 and a fixed communication scenario (3GPP-38.901-Indoor-LOS), the error decreases from $2.58~m$ to $0.25~m$ under graph-based optimization when the number of base stations increases from 2 to 5. However, adding more than four base stations sometimes does not lead to significant or further improvement. This may indicate a lower bound to the error reduction achieved by adding more antennas. Nevertheless, such redundancy may be helpful in those environments where NLOS conditions are more frequent. The effect of bandwidth on the accuracy is also evident, as higher bandwidth leads to higher resolution measurement. In particular, the third scenario (mmMAGIC-Indoor-LOS), which has the highest bandwidth (400 MHz), also offers the highest accuracy for the same antennas.

* Reduced Accuracy in the Vertical Dimension (z-Axis) Upon analyzing the Root Mean Square Error (RMSE) along each axis ($E_x$, $E_y$, $E_z$), a prominent observation is the relatively larger errors in the $E_z$ component, indicative of the vertical direction estimation challenge. This is attributed to the limited offsets provided by base stations in the vertical direction compared to the $x$ and $y$ directions.

* Limited Impact of Measurements on Attitude Estimation: In assessing attitude estimation, a lack of discernible patterns is apparent, suggesting that the inclusion of measurements does not notably enhance the accuracy of attitude estimation. This is inherently expected as measurements primarily offer information about position rather than attitude.

* Superior Performance of Graph-Based Optimization Compared to ESKF: Notably, the graph-based optimization consistently outperforms ESKF across all datasets, scenarios, and base station configurations, demonstrating the graph-based approach's superior efficacy in enhancing indoor localization accuracy. The main reason behind this advantage lies in the filtering method's practice of marginalizing all older information by limiting itself to the most recent states. In contrast, the graph-based method utilizes the entire historical information, employing all past measurements up to the current one and optimizing the entire trajectory. Notably, graph-based methods tend to perform better when dealing with sparse, low-frequency measurements, such as our implementation utilizing measurements with a frequency of 5 Hz, which is relatively low. This increased susceptibility of the ESKF to sparse measurements can contribute to more significant performance disparities between the two methods.

* Robustness Across Different Trajectories and Datasets: We assessed the robustness of our proposed approaches across datasets with varying levels of complexity, encompassing sequences from two different Vicon rooms: Vicon Room 1 (V101, V102, V103) and Vicon Room 2 (V201, V202, V203). These sequences were categorized into three levels of difficulty: easy (V101, V201), medium (V102, V202), and difficult (V103, V203).

Despite the diversity in trajectory complexity and environmental conditions, we observed that the accuracy of our proposed approaches did not exhibit a clear pattern across the different sequences. Surprisingly, in some instances, our methods yielded better results for the more difficult datasets, challenging conventional expectations. This variability underscores the importance of robustness and adaptability in MAV pose estimation algorithms, especially when operating in diverse and dynamic environments.

Our findings highlight the effectiveness of our proposed methods in handling the complexities inherent in MAV flight scenarios. The lack of a consistent pattern in accuracy underscores the versatility of our approaches, demonstrating their ability to adapt to various challenges encountered during indoor flight missions. This adaptability is crucial for real-world applications, where MAVs must navigate through unpredictable environments with confidence and precision.

Table <ref> presents the execution times for both PGO and ESKF, showcasing the best results achieved using five base stations. As evident from the table, both approaches are suitable for real-time implementation, with ESKF outperforming PGO, as expected. This observation reinforces ESKF's superior efficiency for real-time applications.
For instance, in the V101 dataset, PGO required an average of 3.08 milliseconds per optimization, while ESKF consumed only 0.39 milliseconds for a prediction-update cycle. Since the codes were implemented in Python (with PGO partially implemented in Python), we expect further improvement in real-time performance by converting the codes entirely to C++. Overall, while the ESKF estimation in general runs faster than the PGO, these results also show that both estimation approaches can be implemented for real-time MAV applications, given a suitable on-board processing unit.

Computation time comparison for PGO and ESKF in Euroc MAV dataset localization. The table showcases the best results for each dataset, including the average time and standard deviation for each PGO optimization and ESKF (both prediction and update steps). All times are reported in milliseconds.

Method      V101                        V102     V103       V201     V202     V203

(lr)1-1 (lr)2-7

Graph-based     3.08$\pm$2.38      1.85$\pm$1.34      2.36$\pm$1.69      2.89$\pm$2.44      2.62$\pm$2.24      2.44$\pm$1.72

ESKF-based     0.39$\pm$0.20      0.35$\pm$0.16      0.37$\pm$0.14      0.39$\pm$0.15      0.43$\pm$0.19      0.35$\pm$0.15

To visually illustrate the distinction between the two algorithms, we focused on a specific scenario for each dataset: mmMAGIC-Indoor-LOS configuration with five base stations. In Fig. <ref>, we present visual representations of the trajectories in the xy-plane (upper row) and the z-coordinate over time (lower row) for different datasets. Each subfigure corresponds to a specific dataset (V101, V102, etc.), and within each subfigure, three trajectories are displayed:
Blue: Ground-truth trajectory, Red: Graph-based estimation, Green: ESKF estimation.

Trajectories in the xy-plane (upper low) and z-coordinate over time (lower row) for different datasets. Blue represents ground-truth trajectories, red represents graph-based estimation, and green represents ESKF estimation.

§.§ Limitations

The approach used in this study currently has several limitations and hence leaves potential for future work. The 3D position is not fully constrained with only two antennas, making convergence difficult without other measurements. Nevertheless, the UAV's rotation errors primarily result from IMU noise, as the 5G measurements only provide distances to the antennas.
The yaw estimation has drift issues because it lacks global measurement to correct it. Integrating other sensors can improve the localization accuracy by observing the rotation around $z$,  employing a magnetometer. Notably, a camera can be incorporated to add other constraints on the 6DoF relative motion based on visual features and loop closures.

Furthermore, the error in the $z$ axis is larger than along $x$ and $y$ axes because of limited offset or variation in the positions of the base stations in the height direction. We foresee possibly fusing the barometer's absolute height measurements to relieve such issues. Additionally, the localization accuracy depends heavily on the quality of the measurements, which can be negatively affected by NLOS conditions. In such cases, correctly setting the measurement uncertainty for each range factor, using Mahalanobis distance to discard outliers, or applying a robust kernel (such as  , a Huber kernel) to the cost function, may be beneficial to alleviate the problem.

Finally, the proposed method assumes that the positions of the base stations are known with high confidence and fixed in the exact location, which may not be the case in real-world scenarios where the stations may be moving or their positions may be completely unknown. However, especially the PGO approach can be adapted to also include an estimation of the unknown position of the BSs, if the measurements can be unambiguously assigned to the sending base station, respectively.

§ CONCLUSION AND FUTURE WORK

In conclusion, our research demonstrates the potential of utilizing 5G Time of Arrival (ToA) range measurements alongside inertial sensor data for precise indoor localization of Micro Aerial Vehicles (MAVs) across various scenarios and network configurations. We accurately determined MAV positions and orientations by employing graph-based optimization and Error State Kalman Filtering (ESKF). We conducted a thorough comparative analysis across different network scenarios, trajectories, and base station setups.

Our comprehensive investigation revealed valuable insights. We observed that increased base stations and bandwidth improved -based localization accuracy. Despite limitations in the vertical dimension due to base station offsets, measurements exhibited potential for maintaining position estimates at a global level. Graph-based optimization consistently outperformed ESKF, especially in high-bandwidth 5G setups.

Our research roadmap involves enhancing localization accuracy by incorporating camera visual data. This integration will improve the robustness of our methodology, particularly in scenarios prone to noise or non-line-of-sight conditions. Combining multiple data sources promises to elevate localization accuracy and enable effective drone operation in complex indoor environments.

Our results highlight the potential of graph-based optimization for accurate indoor localization, setting the stage for future research. The synergy between wireless communication and sensor fusion offers breakthroughs in robot pose estimation. In closing, this research emphasizes interdisciplinary collaboration and paves the way for enhancing MAV localization algorithms.

This current approach's limitations invite further research, particularly to elevate localization accuracy. Future work could explore fusing 5G measurements with additional sensors like magnetometers or cameras, potentially mitigating UAV rotation errors and imposing constraints for 6 Degrees of Freedom (6DoF) motion. Additionally, incorporating barometers for absolute height measurements might alleviate z-axis errors arising from limited base station positioning. Strategies like improved uncertainty estimation, outlier rejection, and robust cost functions hold promise for managing noisy Time of Arrival (ToA) measurements, especially in non-line-of-sight scenarios. Finally, adapting localization methods like Pose Graph Optimization to handle unknown or dynamic base station positions could enhance the approach's real-world robustness.

Author contribution

All authors actively contributed to the study's conception. Meisam Kabiri led the coding, design, and initial draft, with valuable assistance from Claudio Cimarelli. All other authors offered essential input and support, shaping the conceptual framework, providing insights on methodology and the evaluation system, and contributing to manuscript preparation. Additionally, they played a crucial role in editing and reviewing the manuscript. All authors have thoroughly read and given their approval for the final manuscript.

This research was funded in whole, or in part, by the Luxembourg National Research
Fund (FNR), 5G-Sky Project, ref. C19/IS/13713801/5G-Sky. For the purpose of open access, and in fulfilment of the obligations arising from the grant agreement, the authors have applied a Creative Commons Attribution 4.0 International (CC BY 4.0) license to any  Author Accepted Manuscript version arising from this submission.

Availability of data and materials Not applicable.

Code availability Not applicable.

§ DECLARATIONS

Conflicts of interest No Conflicts of interest
Ethics approval Not applicable
Consent to participate Not applicable
Consent for publication Not applicable

[1]
B. Yang and E. Yang, “A survey on radio frequency based precise localisation technology for uav in gps-denied environment,” Journal of Intelligent & Robotic Systems, vol. 103, pp. 1–30, 2021.

[2]
F. Zafari, A. Gkelias, and K. K. Leung, “A survey of indoor localization systems and technologies,” IEEE Communications Surveys & Tutorials, vol. 21, no. 3, pp. 2568–2599, 2019.

[3]
A. Gupta and U. Singh, “Optimal performance evaluation of localization of sensor nodes in wireless sensor networks,” Wireless Personal Communications, pp. 1–14, 2023.

[4]
M. Kabiri, C. Cimarelli, H. Bavle, J. L. Sanchez-Lopez, and H. Voos, “A review of radio frequency based localisation for aerial and ground robots with 5g future perspectives,” Sensors, vol. 23, no. 1, p. 188, 2022.

[5]
P. S. Shrivastava, U. K. Malviya, M. Meshram, and U. S. Dewangan, “Efficiency of ultra-dense multi-tier future cellular networks for 5g: A survey,” Wireless Personal Communications, vol. 122, no. 4, pp. 3269–3291, 2022.

[6]
R. Dilli, “Hybrid beamforming in 5g nr networks using multi user massive mimo at fr2 frequency bands,” Wireless Personal Communications, vol. 127, no. 4, pp. 3677–3709, 2022.

[7]
S. Dwivedi, R. Shreevastav, F. Munier, J. Nygren, I. Siomina, Y. Lyazidi, D. Shrestha, G. Lindmark, P. Ernström, E. Stare, et al., “Positioning in 5g networks,” IEEE Communications Magazine, vol. 59, no. 11, pp. 38–44, 2021.

[8]
M. Kabiri, C. Cimarelli, H. Bavle, J. L. Sanchez-Lopez, and H. Voos, “Pose graph optimization for a mav indoor localization fusing 5gnr toa with an imu,” arXiv preprint arXiv:2306.09826, 2023.

[9]
C. Forster, L. Carlone, F. Dellaert, and D. Scaramuzza, IMU preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation.

[10]
F. Dellaert and G. Contributors, “borglab/gtsam,” May 2022.

[11]
M. Burri, J. Nikolic, P. Gohl, T. Schneider, J. Rehder, S. Omari, M. W. Achtelik, and R. Siegwart, “The euroc micro aerial vehicle datasets,” The International Journal of Robotics Research, 2016.

[12]
S. Jaeckel, L. Raschkowski, K. Börner, L. Thiele, F. Burkhardt, and E. Eberlein, “Quadriga-quasi deterministic radio channel generator, user manual and documentation,” Fraunhofer Heinrich Hertz Institute, Tech. Rep. v2. 0.0, 2017.

[13]
D. Prokhorov, D. Zhukov, O. Barinova, K. Anton, and A. Vorontsova, “Measuring robustness of visual slam,” in 2019 16th International Conference on Machine Vision Applications (MVA), pp. 1–6, IEEE, 2019.

[14]
R. M. Ferre, G. Seco-Granados, and E. S. Lohan, “Positioning reference signal design for positioning via 5g,” National Committee for Radiology in Finland, 2019.

[15]
J. A. del Peral-Rosado, J. A. López-Salcedo, S. Kim, and G. Seco-Granados, “Feasibility study of 5g-based localization for assisted driving,” in 2016 International conference on localization and GNSS (ICL-GNSS), pp. 1–6, IEEE, 2016.

[16]
S. Saleh, A. S. El-Wakeel, and A. Noureldin, “5g-enabled vehicle positioning using ekf with dynamic covariance matrix tuning,” IEEE Canadian Journal of Electrical and Computer Engineering, vol. 45, no. 3, pp. 192–198, 2022.

[17]
Siradel, “S_5gchannel for advanced 5g mmw propagation modeling,” Siradel website, 2023.

[18]
E. Y. Menta, N. Malm, R. Jäntti, K. Ruttik, M. Costa, and K. Leppänen, “On the performance of aoa–based localization in 5g ultra–dense networks,” IEEE Access, vol. 7, pp. 33870–33880, 2019.

[19]
C. Sun, H. Zhao, L. Bai, J. W. Cheong, A. G. Dempster, and W. Feng, “Gnss-5g hybrid positioning based on toa/aoa measurements,” in China Satellite Navigation Conference (CSNC) 2020 Proceedings: Volume III, pp. 527–537, Springer, 2020.

[20]
R. Klus, J. Talvitie, and M. Valkama, “Neural network fingerprinting and gnss data fusion for improved localization in 5g,” in 2021 International Conference on Localization and GNSS (ICL-GNSS), pp. 1–6, IEEE, 2021.

[21]
J. Talvitie, T. Levanen, M. Koivisto, K. Pajukoski, M. Renfors, and M. Valkama, “Positioning of high-speed trains using 5g new radio synchronization signals,” in 2018 IEEE Wireless Communications and Networking Conference (WCNC), pp. 1–6, IEEE, 2018.

[22]
M. Pan, P. Liu, S. Liu, W. Qi, Y. Huang, X. You, X. Jia, and X. Li, “Efficient joint doa and toa estimation for indoor positioning with 5g picocell base stations,” IEEE Transactions on Instrumentation and Measurement, vol. 71, pp. 1–19, 2022.

[23]
K. Shamaei and Z. M. Kassas, “Receiver design and time of arrival estimation for opportunistic localization with 5g signals,” IEEE Transactions on Wireless Communications, vol. 20, no. 7, pp. 4716–4731, 2021.

[24]
Z. Zhang, L. Wu, Z. Zhang, J. Dang, B. Zhu, and L. Wang, “Aoa-and-amplitude fingerprint based indoor intelligent localization scheme for 5g wireless communications,” in 2021 13th International Conference on Wireless Communications and Signal Processing (WCSP), pp. 1–5, IEEE, 2021.

[25]
X. Chu, Z. Lu, D. Gesbert, L. Wang, and X. Wen, “Vehicle localization via cooperative channel mapping,” IEEE Transactions on Vehicular Technology, vol. 70, no. 6, pp. 5719–5733, 2021.

[26]
R. Mendrzik, H. Wymeersch, and G. Bauch, “Joint localization and mapping through millimeter wave mimo in 5g systems,” in 2018 IEEE Global Communications Conference (GLOBECOM), pp. 1–6, IEEE, 2018.

[27]
R. V. Vitali, R. S. McGinnis, and N. C. Perkins, “Robust error-state kalman filter for estimating imu orientation,” IEEE Sensors Journal, vol. 21, no. 3, pp. 3561–3569, 2021.

[28]
S. Panich, “Indirect kalman filter in mobile robot application,” J. Math. Stat, vol. 6, no. 381384.43, 2010.

[29]
J. L. Sanchez-Lopez, V. Arellano-Quintana, M. Tognon, P. Campoy, and A. Franchi, “Visual marker based multi-sensor fusion state estimation**during this work jose luis sanchez-lopez has been funded by the eiffel excellence scholarship program of the french ministry of foreign affairs and international development and victor arellano-quintana has been funded by a scholarship from conacyt for studies abroad.this work has been partially funded by the european unions horizon 2020 research and innovation programme under grant agreement no 644271 aeroarms,” IFAC-PapersOnLine, vol. 50, no. 1, pp. 16003–16008, 2017.
20th IFAC World Congress.

[30]
Y. Yin, J. Zhang, M. Guo, X. Ning, Y. Wang, and J. Lu, “Sensor fusion of gnss and imu data for robust localization via smoothed error state kalman filter,” Sensors, vol. 23, p. 3676, Apr 2023.

[31]
L. Marković, M. Kovač, R. Milijas, M. Car, and S. Bogdan, “Error state extended kalman filter multi-sensor fusion for unmanned aerial vehicle localization in gps and magnetometer denied indoor environments,” in 2022 International Conference on Unmanned Aircraft Systems (ICUAS), pp. 184–190, 2022.

[32]
A. I. Mourikis and S. I. Roumeliotis, “A multi-state constraint kalman filter for vision-aided inertial navigation,” in Proceedings 2007 IEEE International Conference on Robotics and Automation, pp. 3565–3572, 2007.

[33]
F. Dellaert and M. Kaess, Factor Graphs for Robot Perception.
Foundations and Trends in Robotics, Vol. 6, 2017.

[34]
R. Mascaro, L. Teixeira, T. Hinzmann, R. Siegwart, and M. Chli, “Gomsf: Graph-optimization based multi-sensor fusion for robust uav pose estimation,” in 2018 IEEE International Conference on Robotics and Automation (ICRA), pp. 1421–1428, 2018.

[35]
R. Mur-Artal and J. D. Tardós, “Orb-slam2: An open-source slam system for monocular, stereo, and rgb-d cameras,” IEEE transactions on robotics, vol. 33, no. 5, pp. 1255–1262, 2017.

[36]
C. Campos, R. Elvira, J. J. G. Rodríguez, J. M. Montiel, and J. D. Tardós, “Orb-slam3: An accurate open-source library for visual, visual–inertial, and multimap slam,” IEEE Transactions on Robotics, vol. 37, no. 6, pp. 1874–1890, 2021.

[37]
A. B. Chatfield, Fundamentals of high accuracy inertial navigation, vol. 174.
Aiaa, 1997.

[38]
M. Kaess, H. Johannsson, R. Roberts, V. Ila, J. Leonard, and F. Dellaert, “isam2: Incremental smoothing and mapping with fluid relinearization and incremental variable reordering,” in 2011 IEEE International Conference on Robotics and Automation, pp. 3281–3288, 2011.

[39]
Y. Tazaki, “A spanning tree-based multi-resolution approach for pose-graph optimization,” IEEE Robotics and Automation Letters, vol. 7, no. 4, pp. 10033–10040, 2022.

[40]
S. Jaeckel, N. Turay, L. Raschkowski, L. Thiele, R. Vuohtoniemi, M. Sonkki, V. Hovinen, F. Burkhardt, P. Karunakaran, and T. Heyn, “Industrial indoor measurements from 2-6 ghz for the 3gpp-nr and quadriga channel model,” in 2019 IEEE 90th Vehicular Technology Conference (VTC2019-Fall), pp. 1–7, IEEE, 2019.

[41]
3GPP, “Study on channel model for frequencies from 0.5 to 100 ghz,” 2018.

[42]
L. Carneiro de Souza, C. H. de Souza Lopes, R. de Cassia Carlleti dos Santos, A. Cerqueira Sodré Junior, and L. L. Mendes, “A study on propagation models for 60 ghz signals in indoor environments,” Frontiers in Communications and Networks, vol. 2, p. 757842, 2022.