James Richard Forbes

This paper presents the iterative equivariant filter (IterEqF). The iterative extended Kalman filter (IterEKF) replaces the standard EKF cor… (voir plus)rection step with an iterative correction step that is the Gauss-Newton solution to a nonlinear weighted least squares problem. The standard equivariant filter (EqF) exploits and respects the underlying symmetry of state estimation problems posed on homogeneous spaces and Lie groups. The motivation behind the IterEqF is to combine the features of both the IterEKF and the EqF, thus leading to a high-performance state estimation solution that is well suited to navigation problems. This paper derives the iteration procedure for the IterEqF update step and shows how the intrinsic nonlinearity of the approach naturally results in a `reset' of the filter's covariance into new coordinates. Monte-Carlo simulations of range-based localisation for a mobile robot demonstrate the improvement in performance relative to a standard EqF, especially during the transient convergence.

2026-09-10

arXiv (prépublication)

Extending Ground-Constraint LiDAR-IMU Calibration to Tilted Surfaces in a Continuous-Time Framework

Vassili Korotkine

Pierre Chamoun

Mohammed Ayman Shalaby

This paper presents a novel method that extends targetless LiDAR-IMU calibration for ground vehicles to non- flat environments. Calibration … (voir plus)typically necessitates full exci- tation of the sensor rig, a requirement that is not fulfilled by ground vehicles in normal operation. To address the degenerate planar motion, state-of-the-art methods propose residuals that assume the colinearity of the gravity and physical surface normal vectors, restricting usage to cases where the ground is assumed flat. This paper proposes ground-plane residuals that do not require this assumption, and are applicable for planar motion on a tilted surface. Results are demonstrated on a dataset collected from a Husky ground vehicle, on the M2DGR dataset, as well as on an offroad vehicle dataset. Repeatability is shown to be improved both in tilted and flat-ground scenarios, with strong improvement demonstrated for the tilted case. The implementation and experiments are open-sourced at https://github.com/vkorotkine/licalib_tilted_ground.

2026-08-24

arXiv (prépublication)

Koopman Representation of Nonlinear Virtual Environments in Kinesthetic Haptic Systems

Yanting Zhou

József Kövecses

Rendering haptic feedback with nonlinear virtual environments (VEs) is important in many applications that require highly accurate force fee… (voir plus)dback. This paper considers the use of the Koopman operator to represent a nonlinear VE interacting with a haptic system. Simulation and experimental results demonstrated that the proposed method provides an effective representation of the nonlinear dynamics of a Duffing-oscillator VE. A multi-user study further confirmed this conclusion. In addition, a closed-loop (CL) stability analysis is performed leveraging the Koopman representation of the nonlinear VE to access stability of the overall haptic system. This alternative way of representing nonlinear VEs enables a convenient CL stability analysis that is less conservative than traditional passivity-based methods. Since a linear combination of all lifted states is used to represent the nonlinearity, such representation is also more robust to uncertainties in the modeling of the haptic device than a traditional nonlinear model.

2026-08-10

arXiv (prépublication)

A Passivity-Based Analysis of First-Order Momentum-Based Methods

Sepehr Moalemi

This paper presents a discrete-time passivity-based analysis of first-order momentum-based methods for a class of functions whose gradient h… (voir plus)as lower and upper sector bounds of

2026-08-05

arXiv (prépublication)

D-CLIPSE: Distributed Consensus-based Localization with Passive Listening on Shared State Exchange

Kyle Biron-Gricken

Multi-robot localization that is accurate and consistent is imperative for downstream tasks such as planning and control. Centralized filter… (voir plus)ing approaches optimally fuse all available sensor measurements of the team. However, a centralized solution is rarely implementable due to hardware, communication, and computational constraints. Distributed approaches deploy a filter on each robot to estimate their own state and neighbours' states using inter-robot communication. This paper proposes a consistent, communication-efficient, and consensus-based distributed filtering framework that shares both preintegrated odometry and relevant shared states among communicating robots. The proposed method is validated in simulated and experimental scenarios, showing near centralized performance in accuracy, and especially in consistency, compared to the current state-of-the-art decentralized approach.

2026-07-08

arXiv (prépublication)

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… (voir plus)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 (prépublication)

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)… (voir plus) 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 (prépublication)

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… (voir plus)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 (publié)

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… (voir plus) 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 (publié)

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… (voir plus) 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 (prépublication)

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 … (voir plus)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 (prépublication)