This paper presents a framework for the real-time initialization of unknown Ultra-Wideband (UWB) anchors in UWB-aided navigation systems. The method is designed for localization solutions where UWB modules act as supplementary sensors. Our approach enables the automatic detection and calibration of previously unknown anchors during operation, removing the need for manual setup. By combining an online Positional Dilution of Precision (PDOP) estimation, a lightweight outlier detection method, and an adaptive robust kernel for non-linear optimization, our approach significantly improves robustness and suitability for real-world applications compared to state-of-the-art. In particular, we show that our metric which triggers an initialization decision is more conservative than current ones commonly based on initial linear or non-linear initialization guesses. This allows for better initialization geometry and subsequently lower initialization errors. We demonstrate the proposed approach on two different mobile robots: an autonomous forklift and a quadcopter equipped with a UWB-aided Visual-Inertial Odometry (VIO) framework. The results highlight the effectiveness of the proposed method with robust initialization and low positioning error. We open-source our code in a C++library including a ROS wrapper.

This paper introduces a generic filter-based state estimation framework that supports two state-decoupling strategies based on cross-covariance factorization. These strategies reduce the computational complexity and inherently support true modularity – a perquisite for handling and processing meshed range measurements among a time-varying set of devices. In order to utilize these measurements in the estimation framework, positions of newly detected stationary devices (anchors) and the pairwise biases between the ranging devices are required. In this work an autonomous calibration procedure for new anchors is presented, that utilizes range measurements from multiple tags as well as already known anchors. To improve the robustness, an outlier rejection method is introduced. After the calibration is performed, the sensor fusion framework obtains initial beliefs of the anchor positions and dictionaries of pairwise biases, in order to fuse range measurements obtained from new anchors tightly-coupled. The effectiveness of the filter and calibration framework has been validated through evaluations on a recorded dataset and real-world experiments.

In this paper, we present a Radar-Inertial Odometry (RIO) approach that utilizes performance improving modules, enhanced for the sparse and noisy radar signals, from the vision community in order to estimate the full 6DoF pose and 3D velocity of a robot in an unprepared environment. Our method leverages a multi-state approach in which we make use of several past robot poses and trails of measurements from a lightweight and inexpensive Frequency Modulated Continuous Wave (FMCW) radar sensor. Furthermore, in our estimation framework we include a method for promoting measurement trails to persistent landmarks which correspond to salient features in the environment. In an Extended Kalman Filter (EKF) framework, we fuse the range measurements to the persistent landmarks, trails, and the velocity measurements of the detected 3D points together with the Inertial Measurement Unit (IMU) readings. Our method is particularly relevant for (but not limited to) Unmanned Aerial Vehicles (UAV), enabling them to localize while performing missions in Global Navigation Satellite System (GNSS)-denied environments and, thanks to the properties of the radar sensor, in environments generally challenging for robot perception due to external factors such as smoke or extreme illumination. We show in real flight experiments the effectiveness of our estimator and compare it to the state-of-the-art.

Collaboratively estimating the state of two robots under communication constraints is challenging regarding computational complexity and statistical optimality. Previous work only achieves practical solutions by either disregarding parts of the measurements or imposing a communication overhead, being non-optimal or not entirely distributed, respectively. In this work, we present a centralized-equivalent but dis-tributed approach for pairwise state estimation where two agents only communicate when they meet. Our approach utilizes elements from wave scattering theory to efficiently and consistently summarize (pre-compute) past estimator information (i.e., state evolution and uncertainty) between encounters of two agents. This summarized information is then used in a joint correction step taking into account all past information of each agent in a statistically correct way. This novel approach enables us to distribute the pre-computations of both state evolution and uncertainties on the agents and reconstruct the centralized-equivalent system estimate with very few computations once the agents meet again while still applying all measurements from both agents on both estimates upon encounter. We compare our approach on a real-world dataset against a state of the art collaborative state estimation approach.

Navigating accurately in potentially GPS-denied environments is a perquisite of autonomous systems. Relative localization based on ultra-wideband (UWB) is - especially indoors - a promising technology. In this paper, we present a probabilistic filter based Modular Multi-Sensor Fusion (MMSF) approach with the capability of using efficiently all information in a fully meshed UWB ranging network. This allows an accurate mobile agent state estimation and the calibration of the ranging network's spatial constellation. We advocate a new paradigm that includes elements from Collaborative State Estimation (CSE) and allows us considering all stationary UWB anchors and the mobile agent as a decentralized set of estimtors/filters. With this, our method can include all meshed (inter-)sensor observations tightly coupled in a modular estimator. We show that the application of our CSE-inspired method in such a context breaks the computational barrier. Otherwise, it would, for the sakeof complexity-reduction, prohibit the use of all available information or would lead to significant estimator inconsistencies due to coarse approximations. We compare the proposed approach against different MMSF strategies in terms of execution time, accuracy, and filter credibility on both synthetic data and on a dataset from real Unmanned Aerial Vehicles (UAVs).

Multicopter Unmanned Aerial Vehicles (UAV) are small and agile robots with the potential to become prominent in performing autonomous tasks in various Global Navigation Satellite System (GNSS)-denied environments. These environments can potentially be rendered even more challenging due to external factors impairing the robot's perception, such as low or too bright light, permeation with aerosols or smoke. A precondition of autonomous operation, though, is the ability of a robot to accurately localize itself in the surrounding environment. Millimeter-wave Frequency Modulated Continuous Wave (FMCW) radar sensors are resilient to the aforementioned factors while being lightweight, inexpensive and highly accurate. In this paper, we present a Radar-Inertial Odometry (RIO) method for estimating the full 6DoF pose and 3D velocity of a UAV. In an Extended Kalman Filter (EKF) framework, we fuse range measurements and velocity measurements of 3D points detected by an FMCW radar sensor together with Inertial Measurement Unit (IMU) readings. In real experiments we show that our approach enables accurate state estimation of a UAV and that it exhibits improvements over similar existing state-of-the-art method.

The research community presented significant advances in many different Visual-Inertial Navigation System (VINS) algorithms to localize mobile robots or hand-held devices in a 3D environment. While authors of the algorithms of-ten do compare to, at that time, existing competing approaches, their comparison methods, rigor, depth, and repeatability at later points in time have a large spread. Further, with existing simulators and photo-realistic frameworks, the user is not able to easily test the sensitivity of the algorithm under examination with respect to specific environmental conditions and sensor specifications. Rather, tests often include unwillingly many polluting effects falsifying the analysis and interpretations. In addition, edge cases and corresponding failure modes often remain undiscovered due to the limited breadth of the test sequences. Our unified evaluation framework allows, in a fully automated fashion, a reproducible analysis of different VINS methods with respect to specific environmental and sensor parameters. The analyses per parameter are done over a multitude of test sets to obtain both statistically valid results and an average over other, potentially polluting effects with respect to the one parameter under test to mitigate biased interpretations. The automated performance results per method over all tested parameters are then summarized in unified radar charts for a fair comparison across authors and institutions.

This paper presents a novel approach to recover outdated cross-covariance between correlated agents at the moment they perform joint observations. This allows to render Collaborative State Estimation (CSE) fully distributed, with communication only required for the moment of joint observation and most importantly, it significantly reduces the maintenance effort in case of high frequent propagation sensors. These properties make the approach suitable to a wide range of multi-robot applications. In our evaluation on a Quaternion-based Error-State Extended Kalman Filter (Q-ESEKF) using an Inertial Measurement Unit (IMU) as propagation sensor at a rate of 200Hz, we showed a significant speedup against our previous approach for maintaining a couple of interdependence. We compared the approach in total against four different approaches on both, a simulation and on a real-world dataset for Micro Aerial Vehicles (MAVs). Video: https://youtu.be/xkljfwbhMP0

In this paper, we present a Quaternion-based Error-State Extended Kalman Filter (Q-ESEKF) based on IMU propagation with an extension for Collaborative State Estimation (CSE) and a communication complexity of O(1) (in terms of required communication links). Our approach combines a versatile filter formulation with the concept of CSE, allowing independent state estimation on each of the agents and at the same time leveraging and statistically maintaining interdependencies between agents, after joint measurements and communication (i.e. relative position measurements) occur. We discuss the development of the overall framework and the probabilistic (re-)initialization of the agent's states upon initial or recurring joint observations. Our approach is evaluated in a simulation framework on two prominent benchmark datasets in 3D.

Delay compensation in filter based sensor fusion frameworks for multiple sensors with varying delays and different rates quickly results in large computational overhead should the delayed measurements be incorporated in a statistically meaningful way. Even more so if high rate propagation sensors (e.g. IMU) are used. This work presents an approach to implement such frameworks with significant complexity reduction compared to standard implementations. We set particular focus on the state covariance propagation as this chain of re-computations (i.e. FPFT+ Q per propagation step) upon a delayed update is the dominant bottleneck. We draw our inspiration from the scattering theory and propose a method which projects the idea of wave propagation to an efficient concatenation of covariance propagation steps between filter updates. Through this approach, we reach a speed-up of more than a factor of 10 for the covariance propagation and render the computational complexity independent of the number of propagation steps between filter updates. We evaluated our method in simulation and with real data.