Robot State Estimation

Follow this topic Ask about this topic

Robot state estimation combines sensor readings and models to infer quantities such as position, orientation, velocity, contact, and robot shape, which are needed for navigation and control. Common approaches include Kalman and invariant filters, particle filters, factor graphs and smoothing, and continuous-time methods that fuse asynchronous measurements from IMUs, cameras, LiDAR, radar, GNSS, encoders, and contact sensors. Learning-based methods increasingly complement these estimators by predicting motion, sensor reliability, or contact confidence, especially for legged robots and in degraded sensing conditions. A central challenge is maintaining accurate, consistent uncertainty estimates when contacts slip, sensors fail or arrive late, and the environment provides weak geometric information, without exceeding real-time computational limits.

Latest papers

201 papers26 papers in the last four weeks, down 4% on the four weeks before. 0.2% of all new papers.

Apr 13, 2026

3DRO: Lidar-level SE(3) Direct Radar Odometry Using a 2D Imaging Radar and a Gyroscope

Recently, the robotics community has regained interest in radar-based perception and state estimation. A 2D imaging radar provides dense 360deg information about the environment. Despite the radar antenna's cone of emission and reception, the collected data is generally assumed to be limited to the plane orthogonal to the radar's spinning axis. Accordingly, most methods based on 2D imaging radars only perform SE(2) state estimation. This paper presents 3DRO, an extension of the SE(2) Direct Radar Odometry (DRO) framework to perform state estimation in SE(3). While still assuming planarity of the data through DRO's 2D velocity estimates, it integrates 3D gyroscope measurements over SO(3) to estimate SE(3) ego motion. While simple, this approach provides lidar-level odometry accuracy as demonstrated using 643km of data from the Boreas-RT dataset.

Cedric Le Gentil, Daniil Lisus, Timothy D. Barfoot

Robotics Institute, University of Toronto · Mobile Robotics Lab, ETH Zurich

Robot State Estimation Radar-Inertial Odometry

Apr 11, 2026

Natural Gradient Gaussian Approximation Filter on Lie Groups for Robot State Estimation

Accurate state estimation for robotic systems evolving on Lie group manifolds, such as legged robots, is a prerequisite for achieving agile control. However, this task is challenged by nonlinear observation models defined on curved manifolds, where existing filters rely on local linearization in the tangent space to handle such nonlinearity, leading to accumulated estimation errors. To address this limitation, we reformulate manifold filtering as a parameter optimization problem over a Gaussian-distributed increment variable, thereby avoiding linearization. Under this formulation, the increment can be mapped to the Lie group through the exponential operator, where it acts multiplicatively on the prior estimate to yield the posterior state. We further propose a natural gradient optimization scheme for solving this problem, whose iteration process leverages the Fisher information matrix of the increment variable to account for the curvature of the tangent space. This results in an iterative algorithm named the Natural Gradient Gaussian Approximation on Lie Groups (NANO-L) filter. Leveraging the perturbation model in Lie derivative, we prove that for the invariant observation model widely adopted in robotic localization tasks, the covariance update in NANO-L admits an exact closed-form solution, eliminating the need for iterative updates thus improving computational efficiency. Hardware experiments on a Unitree GO2 legged robot operating across different terrains demonstrate that NANO-L achieves approximately 40% lower estimation error than commonly used filters at a comparable computational cost.

Tianyi Zhang, Wenhan Cao, Chang Liu +2

School of Vehicle and Mobility, Tsinghua University, Beijing, China · College of Engineering, Peking University, Beijing, China · School of Vehicle and Mobility and College of Artificial Intelligence, Tsinghua University, Beijing, China

Robot State Estimation Bayesian Filtering Lie Group Methods Natural Gradient Methods

Apr 9, 2026

State and Trajectory Estimation of Tensegrity Robots via Factor Graphs and Chebyshev Polynomials

Tensegrity robots offer compliance and adaptability, but their nonlinear, and underconstrained dynamics make state estimation challenging. Reliable continuous-time estimation of all rigid links is crucial for closed-loop control, system identification, and machine learning; however, conventional methods often fall short. This paper proposes a two-stage approach for robust state or trajectory estimation (i.e., filtering or smoothing) of a cable-driven tensegrity robot. For online state estimation, this work introduces a factor-graph-based method, which fuses measurements from an RGB-D camera with on-board cable length sensors. To the best of the authors' knowledge, this is the first application of factor graphs in this domain. Factor graphs are a natural choice, as they exploit the robot's structural properties and provide effective sensor fusion solutions capable of handling nonlinearities in practice. Both the Mahalanobis distance-based clustering algorithm, used to handle noise, and the Chebyshev polynomial method, used to estimate the most probable velocities and intermediate states, are shown to perform well on simulated and real-world data, compared to an ICP-based algorithm. Results show that the approach provides high fidelity, continuous-time state and trajectory estimates for complex tensegrity robot motions.

Edgar Granados, Patrick Meng, Charles Tang +4

Dept. of Computer Science, Rutgers University, NJ, USA. · Mech. Eng. & Material Sc. Yale University, New Haven, CT, USA.

Tensegrity Robotics Robot State Estimation Robotics Robust Estimation

Apr 9, 2026

Complementary Filtering on SO(3) for Attitude Estimation with Scalar Measurements

This paper proposes a complementary filter on SO(3) for attitude estimation from scalar measurements corresponding to projections of known inertial vectors onto body-frame sensing directions. The observer evolves directly on SO(3) and employs a constant-gain innovation tailored to the scalar-output structure. Under suitable persistence-of-excitation conditions, almost-global asymptotic stability is established when at least three inertial vectors are measured along a common body-frame direction. For configurations involving only two scalar measurements, sufficient conditions for asymptotic convergence are derived together with an explicit characterization of the region of attraction. Numerical simulations illustrate the proposed results.

Alessandro Melis, Soulaimane Berkane, Robert Mahony +1

I3S, CNRS, Universit´e Cˆote d’Azur, Sophia Antipolis, France · Department of Computer Science and Engineering, Universit´e du Qu´ebec en Outaouais (UQO), QC J8X 3X7, Canada · Systems Theory and Robotics Group, School of Engineering, Australian National University, Australia +1

Robot State Estimation

Apr 4, 2026

CT-VoxelMap: Efficient Continuous-Time LiDAR-Inertial Odometry with Probabilistic Adaptive Voxel Mapping

Maintaining stable and accurate localization during fast motion or on rough terrain remains highly challenging for mobile robots with onboard resources. Currently, multi-sensor fusion methods based on continuous-time representation offer a potential and effective solution to this challenge. Among these, spline-based methods provide an efficient and intuitive approach for continuous-time representation. Previous continuous-time odometry works based on B-splines either treat control points as variables to be estimated or perform estimation in quaternion space, which introduces complexity in deriving analytical Jacobians and often overlooks the fitting error between the spline and the true trajectory over time. To address these issues, we first propose representing the increments of control points on matrix Lie groups as variables to be estimated. Leveraging the feature of the cumulative form of B-splines, we derive a more compact formulation that yields simpler analytical Jacobians without requiring additional boundary condition considerations. Second, we utilize forward propagation information from IMU measurements to estimate fitting errors online and further introduce a hybrid feature-based voxel map management strategy, enhancing system accuracy and robustness. Finally, we propose a re-estimation policy that significantly improves system computational efficiency and robustness. The proposed method is evaluated on multiple challenging public datasets, demonstrating superior performance on most sequences. Detailed ablation studies are conducted to analyze the impact of each module on the overall pose estimation system.

Lei Zhao, Xingyi Li, Tianchen Deng +3

B-Spline Trajectory Representation Adaptive Lidar-Inertial Odometry LiDAR Odometry Robot State Estimation Simultaneous Localization and Mapping

Apr 3, 2026

RAGE: A Tightly Coupled Radar-Aided Grip Estimator For Autonomous Race Cars

Real-time estimation of vehicle-tire-road friction is critical for allowing autonomous race cars to safely and effectively operate at their physical limits. Traditional approaches to measure tire grip often depend on costly, specialized sensors that require custom installation, limiting scalability and deployment. In this work, we introduce RAGE, a novel real-time estimator that simultaneously infers the vehicle velocity, slip angles of the tires and the lateral forces that act on them, using only standard sensors, such as IMUs and RADARs, which are commonly available on most of modern autonomous platforms. We validate our approach through both high-fidelity simulations and real-world experiments conducted on the EAV-24 autonomous race car, demonstrating the accuracy and effectiveness of our method in estimating the vehicle lateral dynamics.

Davide Malvezzi, Nicola Musiu, Eugenio Mascaro +2

Vehicle Dynamics Autonomous Racing Robot State Estimation

Apr 3, 2026

An Asynchronous Two-Speed Kalman Filter for Real-Time UUV Cooperative Navigation Under Acoustic Delays

In Global Navigation Satellite System (GNSS)-denied underwater environments, individual unmanned underwater vehicles (UUVs) suffer from unbounded dead-reckoning drift, making collaborative navigation (CN) crucial for accurate state estimation. However, the severe communication delay inherent in underwater acoustic channels poses serious challenges to real-time state estimation. Traditional filters, such as Extended Kalman Filters (EKFs) or Unscented Kalman Filters (UKFs), usually block the main control loop while waiting for delayed data, or effectively discard Out-of-Sequence Measurements (OOSMs), resulting in serious drift. To address this, we propose an Asynchronous Two-Speed Kalman Filter (TSKF) enhanced by a novel projection mechanism, which we term Variational History Distillation (VHD). The proposed architecture decouples the estimation process into two parallel threads: a fast-rate thread that utilizes Gaussian Process (GP) compensated dead reckoning to guarantee high-frequency real-time control, and a slow-rate thread dedicated to processing asynchronously delayed collaborative information. By introducing a Finite-Length Circular State Buffer (FLCSB), the algorithm applies delayed measurements to their corresponding historical states, and utilizes a VHD-based projection to fast-forward the correction to the current time without computationally heavy recalculations. Simulation results demonstrate that the proposed TSKF maintains a trajectory error comparable to computationally intensive batch-optimization methods under severe delays (up to 30,s). Executing in sub-millisecond time, it significantly outperforms standard EKF/UKF. The results demonstrate an effective control, communication, and computing (3C) co-design that significantly enhances the resilience of autonomous marine automation systems.

Shuyue Li, Miguel López-Benítez, Eng Gee Lim +5

School of Advanced Technology, Xi’an Jiaotong-Liverpool University, Suzhou, China · School of Computer Science and Informatics, University of Liverpool, Liverpool, UK · ARIES Research Centre, Universidad Antonio de Nebrija, Madrid, Spain +3

Kalman Filtering GNSS-Denied Localization Cooperative Localization Underwater Robotics Autonomous Underwater Navigation Robot State Estimation+1

Apr 3, 2026

Geometrically-Constrained Radar-Inertial Odometry via Continuous Point-Pose Uncertainty Modeling

Radar odometry is crucial for robust localization in challenging environments; however, the sparsity of reliable returns and distinctive noise characteristics impede its performance. This paper introduces geometrically-constrained radar-inertial odometry and mapping that jointly consolidates point and pose uncertainty. We employ the continuous trajectory model to estimate the pose uncertainty at any arbitrary timestamp by propagating uncertainties of the control points. These pose uncertainties are continuously integrated with heteroscedastic measurement uncertainty during point projection, thereby enabling dynamic evaluation of observation confidence and adaptive down-weighting of uninformative radar points. By leveraging quantified uncertainties in radar mapping, we construct a high-fidelity map that improves odometry accuracy under imprecise radar measurements. Moreover, we reveal the effectiveness of explicit geometrical constraints in radar-inertial odometry when incorporated with the proposed uncertainty-aware mapping framework. Extensive experiments on diverse real-world datasets demonstrate the superiority of our method, yielding substantial performance improvements in both accuracy and efficiency compared to existing baselines. We release our open-source implementation at https://github.com/wooseongY/GeoRIO.

Wooseong Yang, Dongjae Lee, Minwoo Jung +1

Dept. of Mechanical Engineering, SNU, Seoul, S. Korea

Simultaneous Localization and Mapping Inertial Odometry Uncertainty Quantification Radar-Inertial Odometry Robot State Estimation

Apr 2, 2026

Adaptive Learned State Estimation based on KalmanNet

Hybrid state estimators that combine model-based Kalman filtering with learned components have shown promise on simulated data, yet their performance on real-world automotive data remains insufficient. In this work we present Adaptive Multi-modal KalmanNet (AM-KNet), an advancement of KalmanNet tailored to the multi-sensor autonomous driving setting. AM-KNet introduces sensor-specific measurement modules that enable the network to learn the distinct noise characteristics of radar, lidar, and camera independently. A hypernetwork with context modulation conditions the filter on target type, motion state, and relative pose, allowing adaptation to diverse traffic scenarios. We further incorporate a covariance estimation branch based on the Josephs form and supervise it through negative log-likelihood losses on both the estimation error and the innovation. A comprehensive, component-wise loss function encodes physical priors on sensor reliability, target class, motion state, and measurement flow consistency. AM-KNet is trained and evaluated on the nuScenes and View-of-Delft datasets. The results demonstrate improved estimation accuracy and tracking stability compared to the base KalmanNet, narrowing the performance gap with classical Bayesian filters on real-world automotive data.

Arian Mehrfard, Bharanidhar Duraisamy, Stefan Haag +2

Research and Development, Mercedes-Benz AG, Germany · Institute for Autonomous Driving, University of the Bundeswehr Munich, Germany

Robot State Estimation Kalman Filtering Multimodal Sensor Fusion Autonomous Driving

Mar 31, 2026

Communication Outage-Resistant UUV State Estimation: A Variational History Distillation Approach

The reliable operation of Unmanned Underwater Vehicle (UUV) clusters is highly dependent on continuous acoustic communication. However, this communication method is highly susceptible to intermittent interruptions. When communication outages occur, standard state estimators such as the Unscented Kalman Filter (UKF) will be forced to make open-loop predictions. If the environment contains unmodeled dynamic factors, such as unknown ocean currents, this estimation error will grow rapidly, which may eventually lead to mission failure. To address this critical issue, this paper proposes a Variational History Distillation (VHD) approach. VHD regards trajectory prediction as an approximate Bayesian reasoning process, which links a standard motion model based on physics with a pattern extracted directly from the past trajectory of the UUV. This is achieved by synthesizing ``virtual measurements'' distilled from historical trajectories. Recognizing that the reliability of extrapolated historical trends degrades over extended prediction horizons, an adaptive confidence mechanism is introduced. This mechanism allows the filter to gradually reduce the trust of virtual measurements as the communication outage time is extended. Extensive Monte Carlo simulations in a high-fidelity environment demonstrate that the proposed method achieves a 91% reduction in prediction Root Mean Square Error (RMSE), reducing the error from approximately 170 m to 15 m during a 40-second communication outage. These results demonstrate that VHD can maintain robust state estimation performance even under complete communication loss.

Shuyue Li, Miguel López-Benítez, Eng Gee Lim +5

School of Advanced Technology, Xi’an Jiaotong-Liverpool University, Suzhou, China · School of Engineering, University of Liverpool, Liverpool, UK · Jiangsu JITRI Tsingunited Intelligent Control Technology Co., Ltd., Wuxi, China +4

Robot State Estimation Robust Estimation Autonomous Underwater Navigation Unscented Kalman Filtering

Mar 31, 2026

Interacting Multiple Model Proprioceptive Odometry for Legged Robots

State estimation for legged robots remains challenging because legged odometry generally suffers from limited observability and therefore depends critically on measurement constraints to suppress drift. When exteroceptive sensors are unreliable or degraded, such constraints are mainly derived from proprioceptive measurements, particularly contact-related leg kinematics information. However, most existing proprioceptive odometry methods rely on an idealized point-contact assumption, which is often violated during real locomotion. Consequently, the effectiveness of proprioceptive constraints may be significantly reduced, resulting in degraded estimation accuracy. To address these limitations, we propose an interacting multiple model (IMM)-based proprioceptive odometry framework for legged robots. By incorporating multiple contact hypotheses within a unified probabilistic framework, the proposed method enables online mode switching and probabilistic fusion under varying contact conditions. Extensive simulations and real-world experiments demonstrate that the proposed method achieves superior pose estimation accuracy over state-of-the-art methods while maintaining comparable computational efficiency.

Wanlei Li, Zichang Chen, Shilei Li +2

The authors are with the Shenzhen Key Lab for Advanced Motion Control Technology and Modern Automation Equipment and the Guangdong Provincial Key Laboratory of Intelligent Morphing Mechanisms and Adaptive Robots, School of Intelligence Science and Engineering, College of Artificial Intelligence, Harbin Institute of Technology Shenzhen, Shenzhen 518055, China.

Robot State Estimation Legged Robotics

Mar 28, 2026

Predictive Modeling in AUV Navigation: A Perspective from Kalman Filtering

We present a safety-oriented framework for autonomous underwater vehicles (AUVs) that improves localization accuracy, enhances trajectory prediction, and supports efficient search operations during communication loss. Acoustic signals emitted by the AUV are detected by a network of fixed buoys, which compute Time-Difference-of-Arrival (TDOA) range-difference measurements serving as position observations. These observations are subsequently fused with a Kalman-based prediction model to obtain continuous, noise-robust state estimates. The combined method achieves significantly better localization precision and trajectory stability than TDOA-only baselines. Beyond real-time tracking, our framework offers targeted search-and-recovery capability by predicting post-disconnection motion and explicitly modeling uncertainty growth. The search module differentiates between continued navigation and propulsion failure, allowing search resources to be deployed toward the most probable recovery region. Our framework fuses multi-buoy acoustic data with Kalman filtering and uncertainty propagation to maintain navigation accuracy and yield robust search-region definitions during communication loss.

Zizhan Tang, Yao Liu, Jessica Liu

Department of Electrical and Computer Engineering, University of Southern California, Los Angeles, CA, USA · Department of Computer and Information Science, University of Pennsylvania, Philadelphia, PA, USA

Autonomous Underwater Navigation Robot State Estimation Trajectory Prediction Kalman Filtering

Mar 23, 2026

Beyond Matching to Tiles: Bridging Unaligned Aerial and Satellite Views for Vision-Only UAV Navigation

Recent advances in cross-view geo-localization (CVGL) methods have shown strong potential for supporting unmanned aerial vehicle (UAV) navigation in GNSS-denied environments. However, existing work predominantly focuses on matching UAV views to onboard map tiles, which introduces an inherent trade-off between accuracy and storage overhead, and overlooks the importance of the UAV's heading during navigation. Moreover, the substantial discrepancies and varying overlaps in cross-view scenarios have been insufficiently considered, limiting their generalization to real-world scenarios. In this paper, we present Bearing-UAV, a purely vision-driven cross-view navigation method that jointly predicts UAV absolute location and heading from neighboring features, enabling accurate, lightweight, and robust navigation in the wild. Our method leverages global and local structural features and explicitly encodes relative spatial relationships, making it robust to cross-view variations, misalignment, and feature-sparse conditions. We also present Bearing-UAV-90k, a multi-city benchmark for evaluating cross-view localization and navigation. Extensive experiments show encouraging results that Bearing-UAV yields lower localization error than previous matching/retrieval paradigm across diverse terrains. Our code and dataset will be made publicly available.

Kejia Liu, Haoyang Zhou, Ruoyu Xu +3

College of Computer Science and Technology, Zhejiang University · State Key Laboratory of Blockchain and Data Security, Zhejiang University · Hangzhou High-Tech Zone (Binjiang) Institute of Blockchain and Data Security

UAV Navigation Cross-View Geo-Localization GNSS-Denied Localization Robot State Estimation

Mar 20, 2026

Radar-Inertial Odometry with Online Spatio-Temporal Calibration via Continuous-Time IMU Modeling

Radar-Inertial Odometry (RIO) has emerged as a robust alternative to vision- and LiDAR-based odometry in challenging conditions such as low light, fog, featureless environments, or in adverse weather. However, many existing RIO approaches assume known radar-IMU extrinsic calibration or rely on sufficient motion excitation for online extrinsic estimation, while temporal misalignment between sensors is often neglected or treated independently. In this work, we present a RIO framework that performs joint online spatial and temporal calibration within a factor-graph optimization formulation, based on continuous-time modeling of inertial measurements using uniform cubic B-splines. The proposed continuous-time representation of acceleration and angular velocity accurately captures the asynchronous nature of radar-IMU measurements, enabling reliable convergence of both the temporal offset and extrinsic calibration parameters, without relying on scan matching, target tracking, or environment-specific assumptions.

Vlaho-Josip Štironja, Luka Petrović, Juraj Peršić +2

University of Zagreb Faculty of Electrical Engineering and Computing, Laboratory for Autonomous Systems and Mobile Robotics (LAMOR), Unska 3, HR-10000, Zagreb, Croatia · Calirad d.o.o., Augusta Harambašića 4, HR-10000, Zagreb, Croatia

Radar-Inertial Odometry Robot State Estimation

Mar 18, 2026

Proprioceptive-only State Estimation for Legged Robots with Set-Coverage Measurements of Learned Dynamics

Proprioceptive-only state estimation is attractive for legged robots since it is computationally cheaper and is unaffected by perceptually degraded conditions. The history of joint-level measurements contains rich information that can be used to infer the dynamics of the system and subsequently produce navigational measurements. Recent approaches produce these estimates with learned measurement models and fuse with IMU data, under a Gaussian noise assumption. However, this assumption can easily break down with limited training data and render the estimates inconsistent and potentially divergent. In this work, we propose a proprioceptive-only state estimation framework for legged robots that characterizes the measurement noise using set-coverage statements that do not assume any distribution. We develop a practical and computationally inexpensive method to use these set-coverage measurements with a Gaussian filter in a systematic way. We validate the approach in both simulation and two real-world quadrupedal datasets. Comparison with the Gaussian baselines shows that our proposed method remains consistent and is not prone to drift under real noise scenarios.

Abhijeet M. Kulkarni, Ioannis Poulakakis, Guoquan Huang

Department of Mechanical Engineering, University of Delaware, Newark, DE, USA · Robotics Institute, Athena Research Center, Marousi, Greece; School of Mechanical Engineering, National Technical University of Athens, Greece; HERON–Center of Excellence in Robotics, Athens, Greece

Robot State Estimation Legged Robotics

Mar 17, 2026

SE(3)-LIO: Smooth IMU Propagation With Jointly Distributed Poses on SE(3) Manifold for Accurate and Robust LiDAR-Inertial Odometry

In estimating odometry accurately, an inertial measurement unit (IMU) is widely used owing to its high-rate measurements, which can be utilized to obtain motion information through IMU propagation. In this paper, we address the limitations of existing IMU propagation methods in terms of motion prediction and motion compensation. In motion prediction, the existing methods typically represent a 6-DoF pose by separating rotation and translation and propagate them on their respective manifold, so that the rotational variation is not effectively incorporated into translation propagation. During motion compensation, the relative transformation between predicted poses is used to compensate motion-induced distortion in other measurements, while inherent errors in the predicted poses introduce uncertainty in the relative transformation. To tackle these challenges, we represent and propagate the pose on SE(3) manifold, where propagated translation properly accounts for rotational variation. Furthermore, we precisely characterize the relative transformation uncertainty by considering the correlation between predicted poses, and incorporate this uncertainty into the measurement noise during motion compensation. To this end, we propose a LiDAR-inertial odometry (LIO), referred to as SE(3)-LIO, that integrates the proposed IMU propagation and uncertainty-aware motion compensation (UAMC). We validate the effectiveness of SE(3)-LIO on diverse datasets. Our source code and additional material are available at: https://se3-lio.github.io/.

Gunhee Shin, Seungjae Lee, Jei Kong +2

School of Electrical Engineering, KAIST (Korea Advanced Institute of Science and Technology), Daejeon, 34141, South Korea · Hanwha Aerospace, 6, Pangyo-ro 319 beon-gil, Bundang-gu, Seongnam-si, Gyeonggi-do, South Korea.

LiDAR Odometry Robot State Estimation Inertial Navigation

Mar 14, 2026

LoCal-RIO: Radar-Inertial Odometry with Loop-Closure IMU Bias Calibration

Millimeter-wave radar enables robust perception in visually degraded environments, yet radar-inertial estimation remains prone to drift: body-frame velocity measurements do not constrain heading and position, and the gyroscope bias, which governs heading drift, is poorly observable over the short horizons of sliding-window estimators. We propose a hierarchical radar-inertial factor graph that separates estimation into a fixed-lag navigation graph, which fuses IMU preintegration, radar velocities, ZUPT, and ground-plane constraints into smooth, low-latency odometry, and a keyframe mapping graph, which combines this odometry with submap registration and loop closures. Loop closures additionally calibrate the IMU: the part of a loop residual explained by a bias error is estimated through preintegration Jacobians chained over the loop interval and enters the navigation graph as a prior on the bias alone. Since this calibration is irreversible, it uses only loop closures accepted by the mapping graph and a cycle-consistency test. Extensive evaluations demonstrate high accuracy and drift-reduced estimation at real-time speeds.

Ali Alridha Abdulkarim, Mikhail Litvinov, Zein Alabdeen Abdulkarim +1

Intelligent Space Robotics Laboratory, Center for Digital Engineering, Skolkovo Institute of Science and Technology.

Simultaneous Localization and Mapping Factor Graph Optimization Inertial Odometry Radar-Inertial Odometry Robot State Estimation Inertial Navigation

Mar 13, 2026

Consistent and Efficient MSCKF-based LiDAR-Inertial Odometry with Inferred Cluster-to-Plane Constraints for UAVs

Robust and accurate navigation is critical for Unmanned Aerial Vehicles (UAVs) especially for those with stringent Size, Weight, and Power (SWaP) constraints. However, most state-of-the-art (SOTA) LiDAR-Inertial Odometry (LIO) systems still suffer from estimation inconsistency and computational bottlenecks when deployed on such platforms. To address these issues, this paper proposes a consistent and efficient tightly-coupled LIO framework tailored for UAVs. Within the efficient Multi-State Constraint Kalman Filter (MSCKF) framework, we build coplanar constraints inferred from planar features observed across a sliding window. By applying null-space projection to sliding-window coplanar constraints, we eliminate the direct dependency on feature parameters in the state vector, thereby mitigating overconfidence and improving consistency. More importantly, to further boost the efficiency, we introduce a parallel voxel-based data association and a novel compact cluster-to-plane measurement model. This compact measurement model losslessly reduces observation dimensionality and significantly accelerating the update process. Extensive evaluations demonstrate that our method outperforms most state-of-the-art (SOTA) approaches by providing a superior balance of consistency and efficiency. It exhibits improved robustness in degenerate scenarios, achieves the lowest memory usage via its map-free nature, and runs in real-time on resource-constrained embedded platforms (e.g., NVIDIA Jetson TX2).

Jinwen Zhu, Xudong Zhao, Fangcheng Zhu +4

Meituan UAV, Beijing, China · Dept. of Mechanical Engineering, Computer and Information Sciences, University of Delaware, Newark, DE

LiDAR Odometry Robot State Estimation UAV Navigation Kalman Filtering

Mar 10, 2026

NanoBench: A Multi-Task Benchmark Dataset for Nano-Quadrotor System Identification, Control, and State Estimation

Existing aerial-robotics benchmarks target vehicles from hundreds of grams to several kilograms and typically expose only high-level state data. They omit the actuator-level signals required to study nano-scale quadrotors, where low-Reynoldsnumber aerodynamics, coreless DC motor nonlinearities, and severe computational constraints invalidate models and controllers developed for larger vehicles. We introduce NanoBench, an open-source multi-task benchmark collected on the commercially available Crazyflie 2.1 nano-quadrotor (takeoff weight 27 g) in a Vicon motion capture arena. The dataset comprises 172 flight recordings spanning multi-frequency excitation, geometric trajectory tracking at three speed regimes, and long-duration battery-drain hover. Each recording provides synchronized Vicon ground truth, raw IMU data, onboard extended Kalman filter (EKF) estimates, PID controller internals, and motor PWM commands at 100 Hz, alongside battery telemetry at 10 Hz, crossaligned by gyroscope-based correlation on a 1 ms search grid. NanoBench defines standardized evaluation protocols, train/test splits, and open-source baselines for three tasks: nonlinear system identification, closed-loop controller benchmarking, and onboard state estimation assessment. To our knowledge, it is the first public dataset to jointly provide actuator commands, controller internals, and estimator outputs with millimeter-accurate ground truth on a commercially available nano-scale aerial platform.

Syed Izzat Ullah, Jose Baca

Department of Engineering Texas A&M University-Corpus Christi, Corpus Christi, TX, USA

Benchmark Design Nonlinear System Identification Quadrotor Control Robot State Estimation

Mar 10, 2026

Towards Terrain-Aware Safe Locomotion for Quadrupedal Robots Using Proprioceptive Sensing

Achieving safe quadrupedal locomotion in real-world environments has attracted much attention in recent years. When walking over uneven terrain, achieving reliable estimation and realising safety-critical control based on the obtained information is still an open question. To address this challenge, especially for low-cost robots equipped solely with proprioceptive sensors (e.g., IMUs, joint encoders, and contact force sensors), this work first presents an estimation framework that generates a 2.5-D terrain map and extracts support plane parameters, which are then integrated into contact and state estimation. Then, we integrate this estimation framework into a safety-critical control pipeline by formulating control barrier functions that provide rigorous safety guarantees. Experiments demonstrate that the proposed terrain estimation method provides smooth terrain representations. Moreover, the coupled estimation framework of terrain, state, and contact reduces the mean absolute error of base position estimation by 64.8%, decreases the estimation variance by 47.2%, and improves the robustness of contact estimation compared to a decoupled framework. The terrain-informed CBFs integrate historical terrain information and current proprioceptive measurements to ensure global safety by keeping the robot out of hazardous areas and local safety by preventing body-terrain collision, relying solely on proprioceptive sensing.

Peiyu Yang, Jiatao Ding, Wei Pan +2

Dynamic Legged Systems, Istituto Italiano di Tecnologia (IIT), Italy · Department of Industrial Engineering, University of Trento, Italy · Department of Computer Science, University of Manchester, UK +2

Terrain-Aware Robot Locomotion Quadruped Locomotion Robot State Estimation Control Barrier Functions

Mar 10, 2026

3D UAV Trajectory Estimation and Classification from Internet Videos via Language Model

Reliable 3D trajectory estimation of unmanned aerial vehicles (UAVs) is a fundamental requirement for anti-UAV systems, yet the acquisition of large-scale and accurately annotated trajectory data remains prohibitively expensive. In this work, we present a novel framework that derives UAV 3D trajectories and category information directly from Internet-scale UAV videos, without relying on manual annotations. First, language-driven data acquisition is employed to autonomously discover and collect UAV-related videos, while vision-language reasoning progressively filters task-relevant segments. Second, a training-free cross-modal label generation module is introduced to infer 3D trajectory hypotheses and UAV type cues. Third, a physics-informed refinement process is designed to impose temporal smoothness and kinematic consistency on the estimated trajectories. The resulting video clips and trajectory annotations can be readily utilized for downstream anti-UAV tasks. To assess effectiveness and generalization, we conduct zero-shot transfer experiments on a public, well-annotated 3D UAV benchmark. Results reveal a clear data scaling behavior: as the amount of online video data increases, zero-shot transfer performance on the target dataset improves consistently, without any target-domain training. The proposed method closely approaches the current state-of-the-art, highlighting its robustness and applicability to real-world anti-UAV scenarios. Code and datasets will be released upon acceptance.

Haoxiang Lei, Daotong Wang, Shenghai Yuan +1

Robot State Estimation

Mar 6, 2026

Uncertainty-Aware Adaptive Dynamics For Underwater Vehicle-Manipulator Robots

Accurate and adaptive dynamic models are critical for underwater vehicle-manipulator systems where hydrodynamic effects induce time-varying parameters. This paper introduces a novel uncertainty-aware adaptive dynamics model framework that remains linear in lumped vehicle and manipulator parameters, and embeds convex physical consistency constraints during online estimation. Moving horizon estimation is used to stack horizon regressors, enforce realizable inertia, damping, friction, and hydrostatics, and quantify uncertainty from parameter evolution. Experiments on a BlueROV2 Heavy with a 4-DOF manipulator demonstrate rapid convergence and calibrated predictions. Manipulator fits achieve R2 = 0.88 to 0.98 with slopes near unity, while vehicle surge, heave, and roll are reproduced with good fidelity under stronger coupling and noise. Median solver time is approximately 0.023 s per update, confirming online feasibility. A comparison against a fixed parameter model shows consistent reductions in MAE and RMSE across degrees of freedom. Results indicate physically plausible parameters and confidence intervals with near 100% coverage, enabling reliable feedforward control and simulation in underwater environments.

Edward Morgan, Nenyi K Dadson, Corina Barbalata

The authors are with the Department of Mechanical & Industrial Engineering, at Louisiana State University, Baton Rouge, LA 70803, USA

Robot Dynamics Identification Underwater Robotics Robot State Estimation

Mar 5, 2026

Observer Design for Augmented Reality-based Teleoperation of Soft Robots

Although virtual and augmented reality are gaining traction as teleoperation tools for various types of robots, including manipulators and mobile robots, they are not being used for soft robots. The inherent difficulties of modelling soft robots mean that combining accurate and computationally efficient representations is very challenging. This paper presents an augmented reality interface for teleoperating these devices. The developed system consists of Microsoft HoloLens 2 glasses and a central computer responsible for calculations. Validation is performed on PETER, a highly modular pneumatic manipulator. Using data collected from sensors, the computer estimates the robot's position based on the physics of the virtual reality programme. Errors obtained are on the order of 5% of the robot's length, demonstrating that augmented reality facilitates operator interaction with soft manipulators and can be integrated into the control loop.

Jorge Francisco García-Samartín, Iago López Pérez, Emirhan Yolcu +2

Centro de Automática y Robótica, Universidad Politécnica de Madrid, c/ José Gutiérrez Abascal, nº 2, 28006, Madrid, España. · Middle East Technical University, Dumlupinar Blv No 1, 06800 Cankaya, Ankara, Türkiye.

Robot Teleoperation Soft Robotics Robot State Estimation Augmented Reality

Mar 5, 2026

Integrated cooperative localization of heterogeneous measurement swarm: A unified data-driven method

The cooperative localization (CL) problem in heterogeneous robotic systems with different measurement capabilities is investigated in this work. In practice, heterogeneous sensors lead to directed and sparse measurement topologies, whereas most existing CL approaches rely on multilateral localization with restrictive multi-neighbor geometric requirements. To overcome this limitation, we enable pairwise relative localization (RL) between neighboring robots using only mutual measurement and odometry information. A unified data-driven adaptive RL estimator is first developed to handle heterogeneous and unidirectional measurements. Based on the convergent RL estimates, a distributed pose-coupling CL strategy is then designed, which guarantees CL under a weakly connected directed measurement topology, representing the least restrictive condition among existing results. The proposed method is independent of specific control tasks and is validated through a formation control application and real-world experiments.

Kunrui Ze, Wei Wang, Guibin Sun +3

School of Automation Science and Electrical Engineering, Beihang University, Beijing 100191, China · Zhongguancun Laboratory, Beijing 100083, China

Cooperative Localization Robot Localization Robot State Estimation

Mar 4, 2026

Large-Language-Model-Guided State Estimation for Partially Observable Task and Motion Planning

Robot planning in partially observable environments, where not all objects are known or visible, is a challenging problem, as it requires reasoning under uncertainty through partially observable Markov decision processes. During the execution of a computed plan, a robot may unexpectedly observe task-irrelevant objects, which are typically ignored by naive planners. In this work, we propose incorporating two types of common-sense knowledge: (1) certain objects are more likely to be found in specific locations; and (2) similar objects are likely to be co-located, while dissimilar objects are less likely to be found together. Manually engineering such knowledge is complex, so we explore leveraging the powerful common-sense reasoning capabilities of large language models (LLMs). Our planning and execution framework, CoCo-TAMP, introduces a hierarchical state estimation that uses LLM-guided information to shape the belief over task-relevant objects, enabling efficient solutions to long-horizon task and motion planning problems. In experiments, CoCo-TAMP achieves an average reduction of 62.7% in planning and execution time in simulation, and 72.6% in real-world demonstrations, compared to a baseline that does not incorporate either type of common-sense knowledge.

Yoonwoo Kim, Raghav Arora, Roberto Martín-Martín +3

The University of Texas at Austin · Nanyang Technological University

Motion Planning Robot State Estimation Large Language Model-Based Robot Planning Belief-Space Planning

Mar 3, 2026

Real-time tightly coupled GNSS and IMU integration via Factor Graph Optimization

Reliable positioning in dense urban environments remains challenging due to frequent GNSS signal blockage, multipath, and rapidly varying satellite geometry. While factor graph optimization (FGO)-based GNSS-IMU fusion has demonstrated strong robustness and accuracy, most formulations remain offline. In this work, we present a real-time tightly coupled GNSS-IMU FGO method that enables causal state estimation via incremental optimization with fixed-lag marginalization, and we evaluate its performance in a highly urbanized GNSS-degraded environment using the UrbanNav dataset.

Radu-Andrei Cioaca, Paul Irofti, Cristian Rusu +3

University Politehnica of Bucharest, Romania · Three Tensors S.R.L., Romania · University of Bucharest, Romania +2

Factor Graph Optimization Robot State Estimation Inertial Navigation

Mar 3, 2026

Tensegrity Robot Endcap-Ground Contact Estimation with Symmetry-aware Heterogeneous Graph Neural Network

Tensegrity robots possess lightweight and resilient structures but present significant challenges for state estimation due to compliant and distributed ground contacts. This paper introduces a symmetry-aware heterogeneous graph neural network (Sym-HGNN) that infers contact states directly from proprioceptive measurements, including IMU and cable-length histories, without dedicated contact sensors. The network incorporates the robot's dihedral symmetry

D_3

into the message-passing process to enhance sample efficiency and generalization. The predicted contacts are integrated into a state-of-the-art contact-aided invariant extended Kalman filter (InEKF) for improved pose estimation. Simulation results demonstrate that the proposed method achieves up to 15% higher accuracy and 5% higher F1-score using only 20% of the training data compared to the CNN and MI-HGNN baselines, while maintaining low-drift and physically consistent state estimation results comparable to ground truth contacts. This work highlights the potential of fully proprioceptive sensing for accurate and robust state estimation in tensegrity robots. Code available at: https://github.com/Jonathan-Twz/Tensegrity-Sym-HGNN

Wenzhe Tong, Yicheng Jiang, Chi Zhang +2

Robotics Department, University of Michigan, Ann Arbor, MI 48109 USA

Tensegrity Robotics Robotics Robot State Estimation Heterogeneous GNNs Spatiotemporal GNNs

Mar 3, 2026

Scalar-Measurement Attitude Estimation on $\mathbf{SO}(3)$ with Bias Compensation

Attitude estimation methods typically rely on full vector measurements from inertial sensors such as accelerometers and magnetometers. This paper shows that reliable estimation can also be achieved using only scalar measurements, which naturally arise either as components of vector readings or as independent constraints from other sensing modalities. We propose nonlinear deterministic observers on

\mathbf{SO}(3)

that incorporate gyroscope bias compensation and guarantee uniform local exponential stability under suitable observability conditions. A key feature of the framework is its robustness to partial sensing: accurate estimation is maintained even when only a subset of vector components is available. Experimental validation on the BROAD dataset confirms consistent performance across progressively reduced measurement configurations, with estimation errors remaining small even under severe information loss. To the best of our knowledge, this is the first work to establish fundamental observability results showing that two scalar measurements under suitable excitation suffice for attitude estimation, and that three are enough in the static case. These results position scalar-measurement-based observers as a practical and reliable alternative to conventional vector-based approaches.

Alessandro Melis, Tarek Bouazza, Hassan Alnahhal +3

I3S, CNRS, Université Côte d’Azur, Sophia Antipolis, France. · an independent researcher based in Cairo, Egypt · the Department of Technology & Innovation, Capgemini Engineering, Toulouse, 31300, France +2

Robot State Estimation Inertial Navigation

Mar 2, 2026

D-GVIO: A Buffer-Driven and Efficient Decentralized GNSS-Visual-Inertial State Estimator for Multi-Agent Systems

Cooperative localization is essential for swarm applications like collaborative exploration and search-and-rescue missions. However, maintaining real-time capability, robustness, and computational efficiency on resource-constrained platforms presents significant challenges. To address these challenges, we propose D-GVIO, a buffer-driven and fully decentralized GNSS-Visual-Inertial Odometry (GVIO) framework that leverages a novel buffering strategy to support efficient and robust distributed state estimation. The proposed framework is characterized by four core mechanisms. Firstly, through covariance segmentation, covariance intersection and buffering strategy, we modularize propagation and update steps in distributed state estimation, significantly reducing computational and communication burdens. Secondly, the left-invariant extended Kalman filter (L-IEKF) is adopted for information fusion, which exhibits superior state estimation performance over the traditional extended Kalman filter (EKF) since its state transition matrix is independent of the system state. Thirdly, a buffer-based re-propagation strategy is employed to handle delayed measurements efficiently and accurately by leveraging the L-IEKF, eliminating the need for costly re-computation. Finally, an adaptive buffer-driven outlier detection method is proposed to dynamically cull GNSS outliers, enhancing robustness in GNSS-challenged environments.

Yarong Luo, Wentao Lu, Chi Guo +1

School of Robotics, Wuhan University, Wuhan 430072, China. · School of Electronic Information of Wuhan University, Hubei Luojia Laboratory, Wuhan 430072, China.

Robot State Estimation Cooperative Localization Robust Estimation

Mar 1, 2026

Path Integral Particle Filtering for Hybrid Systems via Saltation Matrices

State estimation for hybrid systems that undergo intermittent contact with their environments, such as extraplanetary robots and satellites undergoing docking operations, is difficult due to the discrete uncertainty propagation during contact. To handle this propagation, this paper presents an optimal-control-based particle filtering method that leverages saltation matrices to map out uncertainty propagation during contact events. By exploiting a path integral filtering framework that exploits the duality between smoothing and optimal control, the resulting state estimation algorithm is robust to outlier effects, flexible to non-Gaussian noise distributions, and handles challenging contact dynamics in hybrid systems. To evaluate the validity and consistency of the proposed approach, this paper tests it against strong baselines on the stochastic dynamics generated by a bouncing ball and spring loaded inverted pendulum.

Karthik Shaji, Sreeranj Jayadevan, Bo Yuan +2

Graduate Student, Daniel Guggenheim School of Aerospace Engineering · MathWorks · PlusAI +1

Particle Filtering Robot State Estimation Dynamical Systems