James Richard Forbes

Arturo Del Castillo Bernal

DIVO: Continuous-time DVL-Inertial-Visual Odometry for Unmanned Underwater Vehicles

Kyungmin Jung

Angad Bajwa

Junha Yoo

This paper presents a novel acoustic-visual-inertial odometry solution leveraging a continuous-time trajectory estimation framework for unma… (see more)nned underwater vehicles. Underwater environments present unique challenges for visual localization and mapping, such as light attenuation, illumination variance, and the presence of particulate matter. This motivates the use of additional sensing modalities and a visual tracking pipeline that is robust to diverse subsea conditions. The proposed system is the first continuous-time trajectory estimation framework based on Gaussian processes to fuse asynchronous measurements from a Doppler velocity log, a stereo camera, and an inertial measurement unit. Additionally, a novel visual frontend is proposed, incorporating learning-based feature extraction and matching that is robust to the specific challenges that subsea environments present. The proposed framework enables seamless integration of additional sensor modalities in continuous-time and is adaptable to different environments without reconfiguration. The proposed system is extensively tested on real-world underwater inspection datasets, where it outperforms state-of-the-art visual-inertial and acoustic-visual-inertial SLAM algorithms in accuracy, robustness, and trajectory coverage. Notably, the proposed system outperforms the state-of-the-art despite only forming short-term visual data associations.

2026-07-05

arXiv (preprint)

Observability and Consistency Analysis for Visual-Inertial Navigation with Anchored Feature Parameterizations

Mitchell Cohen

Vassili Korotkine

This paper presents an analysis of the observability and consistency properties of filtering-based visual-inertial navigation systems (VINS)… (see more) that utilize anchored feature representations. The unobservable subspace of VINS with anchored landmark parameterizations is shown to be independent of the estimated landmark state, which leads to improved estimator consistency properties without any additional modifications. However, the unobservable subspace is still found to depend on the estimated navigation state, necessitating additional consistency-enforcing techniques. Two methods to improve the consistency of VINS with anchored feature representations are presented. Simulation results showcase that all estimators employing anchored feature paramterizations exhibit improved consistency properties compared to algorithms that estimate features resolved in a global reference frame, especially in scenarios where feature initialization may be poor. Real-world experiments on the TUM-VI dataset showcase that the use of anchored feature representations alone can yield comparable performance to consistency-improved estimators employing a global feature representation, demonstrating the benefit of using anchored feature parameterizations for VINS.

2026-06-16

arXiv (preprint)

Evaluation of data-driven kinematic models for autonomous control of continuum robotic in-situ bioprinters

Swen A.T. Groen

David Brenken

Samuel Smocot

Luc Mongeau

Audrey Sedal

Minimally invasive in-situ bioprinting involves the direct deposition of hydrogels within the body to reconstruct tissue defects. These biop… (see more)rinters use soft robotic printheads to extrude hydrogels through a hollow channel and nozzle. The accurate control of the nozzle tip position is critical for safety and shape fidelity. Due to a lack of sensing integration, existing control strategies are limited to feedforward models and are design-specific, thereby increasing development cost and complexity. This present study systematically compared three different data-driven modeling strategies for autonomous control of a cable-driven continuum in-situ bioprinter: 1) Polynomial regression, 2) Gaussian process regression, and 3) a neural network. Submillimeter accuracy was achieved for both the Gaussian process and the neural network in static measurements. The Polynomial regression model had a 1.67 mm accuracy. Dynamic trajectory tracking indicated that the performance of the neural network was comparable to that of the polynomial regression model and lower than that of the Gaussian process. Printing of different shaped constructs yielded minor visual deviations across all models from the target shapes. These results support the feasibility of real-time autonomous control for minimally invasive in-situ bioprinters and indicate the advantages of the different models during the design process of novel printing strategies.

2026-04-06

IEEE International Conference on Soft Robotics (published)

MILUV: A Multi-UAV Indoor Localization dataset with UWB and Vision

Mohammed Ayman Shalaby

Nicholas Dahdah

Syed Shabbir Ahmed

Charles Champagne Cossette

Jerome Le Ny

This paper introduces MILUV, a Multi-UAV Indoor Localization dataset with UWB and Vision measurements. This dataset comprises 217 minutes of… (see more) flight time over 36 experiments using three quadcopters, collecting ultra-wideband (UWB) ranging data such as the raw timestamps and channel-impulse response data, vision data from a stereo camera and a bottom-facing monocular camera, inertial measurement unit data, height measurements from a laser rangefinder, magnetometer data, and ground-truth poses from a motion-capture system. The UWB data is collected from up to 12 transceivers affixed to mobile robots and static tripods in both line-of-sight and non-line-of-sight conditions. The UAVs fly at a maximum speed of 4.418 m/s in an indoor environment with visual fiducial markers as features. MILUV is versatile and can be used for a wide range of applications beyond localization, but the primary purpose of MILUV is for testing and validating multi-robot UWB- and vision-based localization algorithms. The dataset can be downloaded at https://doi.org/10.25452/figshare.plus.28386041.v1. A development kit is presented alongside the MILUV dataset, which includes benchmarking algorithms such as visual-inertial odometry, UWB-based localization using an extended Kalman filter, and classification of CIR data using machine learning approaches. The development kit can be found at https://github.com/decargroup/miluv, and is supplemented with a website available at https://decargroup.github.io/miluv/.

2026-01-08

International Journal of Robotics Research (published)

KILO-EKF: Koopman-Inspired Learned Observations Extended Kalman Filter

Zi Cong Guo

Timothy D. Barfoot

We present the Koopman-Inspired Learned Observations Extended Kalman Filter (KILO-EKF), which combines a standard EKF prediction step with a… (see more) correction step based on a Koopman-inspired measurement model learned from data. By lifting measurements into a feature space where they are linear in the state, KILO-EKF enables flexible modeling of complex or poorly calibrated sensors while retaining the structure and efficiency of recursive filtering. The resulting linear-Gaussian measurement model is learned in closed form from groundtruth training data, without iterative optimization or reliance on an explicit parametric sensor model. At inference, KILO-EKF performs a standard EKF update using Jacobians obtained via the learned lifting. We validate the approach on a real-world quadrotor localization task using an IMU, ultra-wideband (UWB) sensors, and a downward-facing laser. We compare against multiple EKF baselines with varying levels of sensor calibration. KILO-EKF achieves better accuracy and consistency compared to data-calibrated baselines, and significantly outperforms EKFs that rely on imperfect geometric models, while maintaining real-time inference and fast training. These results demonstrate the effectiveness of Koopman-inspired measurement learning as a scalable alternative to traditional model-based calibration.

2025-12-31

arXiv (preprint)

Nonlinear Observer Design for Visual-Inertial Odometry

Mouaad Boughellaba

Abdelhamid Tayebi

Soulaimane Berkane

This paper addresses the problem of Visual-Inertial Odometry (VIO) for rigid body systems evolving in three-dimensional space. We introduce … (see more)a novel matrix Lie group structure, denoted SE_{3+n}(3), that unifies the pose, gravity, linear velocity, and landmark positions within a consistent geometric framework tailored to the VIO problem. Building upon this formulation, we design an almost globally asymptotically stable nonlinear geometric observer that tightly integrates data from an Inertial Measurement Unit (IMU) and visual sensors. Unlike conventional Extended Kalman Filter (EKF)-based estimators that rely on local linearization and thus ensure only local convergence, the proposed observer achieves almost global stability through the decoupling of the rotational and translational dynamics. A globally exponentially stable Riccati-based translational observer along with an almost global input-to-state stable attitude observer are designed such that the overall cascaded observer enjoys almost global asymptotic stability. This cascaded architecture guarantees robust and consistent estimation of the extended state, including orientation, position, velocity, gravity, and landmark positions, up to the VIO unobservable directions (i.e., a global translation and rotation about gravity). The effectiveness of the proposed scheme is demonstrated through numerical simulations as well as experimental validation on the EuRoC MAV dataset, highlighting its robustness and suitability for real-world VIO applications.

2025-12-31

arXiv (preprint)

The Harmonic Exponential Filter for Nonparametric Estimation on Motion Groups

Miguel Saavedra-Ruiz

Steven A. Parkison

Ria Arora

Liam Paull

Bayesian estimation is a vital tool in robotics as it allows systems to update the robot state belief using incomplete information from nois… (see more)y sensors. To render the state estimation problem tractable, many systems assume that the motion and measurement noise, as well as the state distribution, are unimodal and Gaussian. However, there are numerous scenarios and systems that do not comply with these assumptions. Existing nonparametric filters that are used to model multimodal distributions have drawbacks that limit their ability to represent a diverse set of distributions. This paper introduces a novel approach to nonparametric Bayesian filtering on motion groups, designed to handle multimodal distributions using harmonic exponential distributions. This approach leverages two key insights of harmonic exponential distributions: a) the product of two distributions can be expressed as the element-wise addition of their log-likelihood Fourier coefficients, and b) the convolution of two distributions can be efficiently computed as the tensor product of their Fourier coefficients. These observations enable the development of an efficient and asymptotically exact solution to the Bayes filter up to the band limit of a Fourier transform. We demonstrate our filter's performance compared with established nonparametric filtering methods across simulated and real-world localization tasks.

2025-01-07

IEEE Robotics and Automation Letters (published)

Reducing Two-Way Ranging Variance by Signal-Timing Optimization

Mohammed Ayman Shalaby

Charles Champagne Cossette

Jerome Le Ny

Time-of-flight-based ranging among transceivers with different clocks requires protocols that accommodate varying rates of the clocks. Doubl… (see more)e-sided two-way ranging (DS-TWR) is widely adopted as a standard protocol due to its accuracy; however, the precision of DS-TWR has not been clearly addressed. In this paper, an analytical model of the variance of DS-TWR is derived as a function of the user-programmed response delays, which is then compared to the Cramer-Rao Lower Bound (CRLB). This is then used to formulate an optimization problem over the response delays in order to maximize the information gained from range measurements. The derived analytical variance model and optimized protocol are validated experimentally with 2 ranging UWB transceivers, where 29 million range measurements are collected.

2024-05-31

IEEE Transactions on Aerospace and Electronic Systems (published)

Optimal Robot Formations: Balancing Range-Based Observability and User-Defined Configurations

Syed Shabbir Ahmed

Mohammed Ayman Shalaby

Jerome Le Ny

This paper introduces a set of customizable and novel cost functions that enable the user to easily specify desirable robot formations, such… (see more) as a ``high-coverage'' infrastructure-inspection formation, while maintaining high relative pose estimation accuracy. The overall cost function balances the need for the robots to be close together for good ranging-based relative localization accuracy and the need for the robots to achieve specific tasks, such as minimizing the time taken to inspect a given area. The formations found by minimizing the aggregated cost function are evaluated in a coverage path planning task in simulation and experiment, where the robots localize themselves and unknown landmarks using a simultaneous localization and mapping algorithm based on the extended Kalman filter. Compared to an optimal formation that maximizes ranging-based relative localization accuracy, these formations significantly reduce the time to cover a given area with minimal impact on relative pose estimation accuracy.

2023-12-31

IEEE/RSJ International Conference on Intelligent Robots and Systems (published)

The Invariant Rauch-Tung-Striebel Smoother

Niels van der Laan

Mitchell Cohen

Jonathan Arsenault

This paper presents an invariant Rauch-Tung- Striebel (IRTS) smoother applicable to systems with states that are an element of a matrix Lie … (see more)group. In particular, the extended Rauch-Tung-Striebel (RTS) smoother is adapted to work within a matrix Lie group framework. The main advantage of the invariant RTS (IRTS) smoother is that the linearization of the process and measurement models is independent of the state estimate resulting in state-estimate-independent Jacobians when certain technical requirements are met. A sample problem is considered that involves estimation of the three dimensional pose of a rigid body on SE(3), along with sensor biases. The multiplicative RTS (MRTS) smoother is also reviewed and is used as a direct comparison to the proposed IRTS smoother using experimental data. Both smoothing methods are also compared to invariant and multiplicative versions of the Gauss-Newton approach to solving the batch state estimation problem.

2020-06-24

IEEE Robotics and Automation Letters (published)