We present a method for the unattended gray-box identification of sensor models commonly used by localization algorithms in the field of robotics. The objective is to determine the most likely sensor model for a time series of unknown measurement data, given an extendable catalog of predefined sensor models. Sensor model definitions may require states for rigid-body calibrations and dedicated reference frames to replicate a measurement based on the robot’s localization state. A health metric is introduced, which verifies the outcome of the selection process in order to detect false positives and facilitate reliable decision-making. In the second stage, an initial guess for identified calibration states is generated, and the necessity of sensor world reference frames is evaluated. The identified sensor model with its parameter information is then used to parameterize and initialize a state estimation application, thus ensuring a more accurate and robust integration of new sensor elements. This method is helpful for inexperienced users who want to identify the source and type of a measurement, sensor calibrations, or sensor reference frames. It will also be important in the field of modular multiagent scenarios and modularized robotic platforms that are augmented by sensor modalities during runtime. Overall, this work aims to provide a simplified integration of sensor modalities to downstream applications and circumvent common pitfalls in the usage and development of localization approaches.

Aiming to enhance the consistency and thus long-term accuracy of Extended Kalman Filters for terrestrial vehicle localization, this paper introduces the Manifold Error State Extended Kalman Filter (M-ESEKF). By representing the robot’s pose in a space with reduced dimensionality, the approach ensures feasible estimates on generic smooth surfaces, without introducing artificial constraints or simplifications that may degrade a filter’s performance. The accompanying measurement models are compatible with common loosely- and tightly-coupled sensor modalities and also implicitly account for the ground geometry. We extend the formulation by introducing a novel correction scheme that embeds additional domain knowledge into the sensor data, giving more accurate uncertainty approximations and further enhancing filter consistency. The proposed estimator is seamlessly integrated into a validated modular state estimation framework, demonstrating compatibility with existing implementations. Extensive Monte Carlo simulations across diverse scenarios and dynamic sensor configurations show that the M-ESEKF outperforms classical filter formulations in terms of consistency and stability. Moreover, it eliminates the need for scenario-specific parameter tuning, enabling its application in a variety of real-world settings.

The majority of commercial and open-source autopilot software for uncrewed aerial vehicles rely on the tried and tested extended Kalman filter (EKF) to provide the state estimation solution for the inertial navigation system (INS). While modern implementations achieve remarkable robustness, it is often due to the careful implementation of exception code for a multitude of corner cases along with significant skilled tuning effort. In this paper, we use the data wealth of the ArduPilot community to identify and highlight the most common real-world challenges in INS state estimation, including sensor self-calibration, robustness in static conditions, global navigation satellite system (GNSS) outliers and shifts, and robustness to faulty inertial measurement units (IMUs). We propose a novel equivariant filter (EqF) formulation for the INS solution that exploits a Semi-Direct-Bias symmetry group for multi-sensor fusion with self-calibration capabilities and incorporates equivariant velocity-type measurements. We augment the filter with a simple innovation-covariance inflation strategy that seamlessly handles GNSS outliers and shifts without requiring coding of a whole set of exception cases. We use real-world data from the Ardupilot community to demonstrate the performance of the proposed filter on known cases where existing filters fail without careful exception handling or case-specific tuning and benchmark against the ArduPilot’s EKF3, the most sophisticated EKF implementation currently available.

This paper introduces UVIO, a multi-sensor framework that leverages Ultra Wide Band (UWB) technology and Visual-Inertial Odometry (VIO) to provide robust and low-drift localization. In order to include range measurements in state estimation, the position of the UWB anchors must be known. This study proposes a multi-step initialization procedure to map multiple unknown anchors by an Unmanned Aerial Vehicle (UAV), in a fully autonomous fashion. To address the limitations of initializing UWB anchors via a random trajectory, this paper uses the Geometric Dilution of Precision (GDOP) as a measure of optimality in anchor position estimation, to compute a set of optimal waypoints and synthesize a trajectory that minimizes the mapping uncertainty. After the initialization is complete, the range measurements from multiple anchors, including measurement biases, are tightly integrated into the VIO system. While in range of the initialized anchors, the VIO drift in position and heading is eliminated. The effectiveness of UVIO and our initialization procedure has been validated through a series of simulations and real-world experiments.

Inertial Navigation Systems (INS) are a key technology for autonomous vehicles applications. Recent advances in estimation and filter design for the INS problem have exploited geometry and symmetry to overcome limitations of the classical Extended Kalman Filter (EKF) approach that formed the mainstay of INS systems since the mid-twentieth century. The industry standard INS filter, the Multiplicative Extended Kalman Filter (MEKF), uses a geometric construction for attitude estimation coupled with classical Euclidean construction for position, velocity and bias estimation. The recent Invariant Extended Kalman Filter (IEKF) provides a geometric framework for the full navigation states, integrating attitude, position and velocity, but still uses the classical Euclidean construction to model the bias states. In this paper, we use the recently proposed Equivariant Filter (EqF) framework to derive a novel observer for biased inertial-based navigation in a fully geometric framework. The introduction of virtual velocity inputs with associated virtual bias leads to a full equivariant symmetry on the augmented system. The resulting filter performance is evaluated with both simulated and real-world data, and demonstrates increased robustness to a wide range of erroneous initial conditions, and improved accuracy when compared with the industry standard Multiplicative EKF (MEKF) approach.

We present a novel approach to improve 6 degree-of-freedom state propagation for unmanned aerial vehicles in a classical filter through pre-processing of high-speed inertial data with AI algorithms. We evaluate both an LSTM-based approach as well as a Transformer encoder architecture. Both algorithms take as input short sequences of fixed length N of high-rate inertial data provided by an inertial measurement unit (IMU) and are trained to predict in turn one pre-processed IMU sample that minimizes the state propagation error of a classical filter across M sequences. This setup allows us to provide sufficient temporal history to the networks for good performance while maintaining a high propagation rate of pre-processed IMU samples important for later deployment on real-world systems. In addition, our network architectures are formulated to directly accept input data at variable rates thus minimizing necessary data preprocessing. The results indicate that the LSTM based architecture outperforms the Transformer encoder architecture and significantly improves the propagation error even for long IMU propagation times.

For Ultra-Wide-Band (UWB) based navigation, an accurate initialization of the anchors in a reference coordinate system is crucial for precise subsequent UWB-inertial based pose estimation. This paper presents a strategy based on information theory to initialize such UWB anchors using raw distance measurements from tag to anchor(s) and aerial vehicle poses. We include a linear distance-dependent bias term and an offset in our estimation process in order to achieve unprecedented accuracy in the 3D position estimates of the anchors (error reduction by a factor of about 3.5 compared to current approaches) without the need of prior knowledge. After an initial coarse position triangulation of the anchors using random vehicle positions, a bounding volume is created in the vicinity of the roughly estimated anchor position. In this volume, we calculate points which provide the maximal triangulation related information based on the Fisher Information Theory. Using these information theoretic optimal points, a fine triangulation is done including bias term estimation. We evaluate our approach in simulations with realistic sensor noise as well as with real world experiments. We also fly an aerial vehicle with UWB-inertial based closed loop control demonstrating that precise anchor initialization does improve navigation precision. Our initialization approach is compared to state-of-the-art as well as to an initialization without the simultaneous bias estimation.

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 work presents the Manifold Invariant Extended Kalman Filter, a novel approach for better consistency and accuracy in state estimation on manifolds. The robustness of this filter allows for techniques with high noise potential like ultra-wideband localization to be used for a wider variety of applications like autonomous metal structure inspection. The filter is derived and its performance is evaluated by testing it on two different manifolds: a cylindrical one and a bivariate b-spline representation of a real vessel surface, showing its flexibility to being used on different types of surfaces. Its comparison with a standard EKF that uses virtual, noise-free measurements as manifold constraints proves that it outperforms standard approaches in consistency and accuracy. Further, an experiment using a real magnetic crawler robot on a curved metal surface with ultra-wideband localization shows that the proposed approach is viable in the real world application of autonomous metal structure inspection.

Sensor fusion systems merging (multiple) delayed sensor signals through a statistical approach are challenging setups, particularly for resource constrained platforms. For statistical consistency, one would be required to keep an appropriate history, apply the correcting signal at the given time stamp in the past, and re-apply all information received until the present time. This re-calculation becomes impractical (the bottleneck being the re-propagation of the covariance matrices for estimator consistency) for platforms with multiple sensors/states and low compute power.This work presents a novel approach for consistent covariance pre-integration allowing delayed sensor signals to be incorporated in a statistically consistent fashion with very low complexity. We leverage recent insights in Invariant Extended Kalman Filters (IEKF) and their log-linear, state independent error propagation together with insights from the scattering theory to mimic the re-calculation process as a medium through which we can propagate waves (covariance information in this case) in single operation steps. We support our findings in simulation and with real data.