contact-aided state estimation

This work addresses the significant drift in inertial navigation systems of legged robots caused by noise in consumer-grade IMUs. To mitigate this issue, the paper proposes four progressively advanced proprioceptive state estimation algorithms that leverage intermittent foot contact information to correct IMU drift and jointly estimate robot pose, velocity, and time-varying IMU biases. Building upon a contact-aided invariant extended Kalman filter, the methods incrementally incorporate factor graph optimization and fixed-lag smoothing to substantially enhance estimation accuracy and robustness. All algorithms are implemented using GTSAM and fully integrated with ROS 2, with complete source code publicly released. This open-source framework effectively alleviates IMU drift and promotes reproducible research in proprioceptive odometry for legged locomotion.

Legged robots suffer from severe pose and velocity estimation drift during highly dynamic maneuvers—such as impacts, slips, and rapid rotations. To address this, we propose a tightly coupled visual–inertial–legged odometry framework. Our approach innovatively employs a distributed multi-IMU configuration across robot links, jointly leveraging joint encoders and monocular camera data to model and compensate dominant error sources in proprioceptive odometry. We formulate a sliding-window factor graph optimization that incorporates extended Kalman filter (EKF)-based preintegration of inertial and joint measurements, while unifying visual features, IMU preintegrations, and foot motion constraints as factors for joint optimization. Experimental results demonstrate centimeter-level localization accuracy and significantly reduced drift under high-dynamic tasks, markedly improving state estimation robustness. The corresponding C++ implementation and a large-scale real-world dataset are publicly released.

This work proposes a proprioception-based real-time state estimation method for humanoid robots operating without external sensors or prior knowledge of ground motion. By fusing foot-mounted IMU measurements with kinematic constraints, the authors formulate a right-invariant extended Kalman filter (InEKF) and, for the first time, introduce a right-invariant observation model to non-inertial ground scenarios, thereby ensuring system observability and enabling rapid convergence. Experimental validation on the Digit robot demonstrates a 96% improvement in convergence speed and an 80% reduction in position estimation error compared to existing approaches. Notably, even when walking on a rotating surface with an initial position error as large as one meter, the method maintains an average position estimation error below 9 centimeters.

This paper addresses the problem of high-precision pose and velocity estimation for rigid bodies. We propose a geometric unified observation framework based on the Lie group SE(5). By fusing IMU measurements with generic inertial-frame or body-frame measurements, we construct, for the first time on SE(5), a decoupled geometric error dynamics model—where translational error evolution mimics continuous-time Kalman filtering, enabling Riccati-equation-driven time-varying gain design and guaranteeing almost global asymptotic stability. Our approach overcomes limitations of conventional Euclidean-space modeling, achieving intrinsic decoupling of error dynamics, simplification of observer structure, and enhanced robustness. Extensive simulations—including stereo-camera-aided and GPS-aided inertial navigation systems—demonstrate its effectiveness. The method significantly improves the generality and engineering applicability of nonlinear geometric observers for high-accuracy state estimation.

This work addresses the problem of estimating the relative pose and velocity of an unmanned aerial vehicle (UAV) with respect to an arbitrarily moving planar platform in 3D space, enabling precise approach and landing. The proposed method introduces a tightly coupled estimator that fuses measurements from two inertial measurement units (IMUs) and monocular vision—specifically, the platform’s center-line-of-sight direction and surface normal vector. A novel architecture combines an SO(3) Lie-group complementary filter with a linear Riccati-based cascaded observer; critically, it recovers unobservable attitude angles using only the platform’s linear acceleration under known normal-axis rotational constraints—a first in the literature. Theoretical analysis establishes local exponential convergence and almost-global asymptotic stability of the estimation error. Extensive simulations demonstrate high accuracy and strong robustness against dynamic disturbances, sensor noise, and large initial state errors.

This work addresses the challenges of abrupt outliers, bias drift, and high computational load encountered when deploying large-scale, low-cost MEMS inertial sensor arrays for land navigation. To this end, the authors propose a robust extended Kalman filter (EKF) architecture, termed RISAF, which incorporates dynamic percentile gating and real-time bias tracking prior to the EKF prediction step. This approach effectively suppresses anomalous measurements and compensates for individual sensor drift without expanding the state dimensionality, enabling efficient fusion of data from hundreds of IMUs. Experimental results demonstrate that, in GNSS-denied environments, RISAF significantly improves heading accuracy and mitigates drift accumulation compared to simple averaging, achieving navigation performance approaching that of tactical-grade inertial systems.

This work addresses the challenge of attitude estimation under GNSS-denied and highly dynamic flight conditions, where inertial measurement unit (IMU)-only approaches suffer from ambiguity between gravitational and inertial accelerations. To overcome this limitation, the paper proposes a nonlinear attitude estimation algorithm that fuses barometric altitude measurements. Two novel observers on SO(3) are developed: the first employs a cascaded structure combining a Riccati observer with complementary filtering to achieve almost global asymptotic stability, while the second constructs a unified observer on SO(3)×ℝ² that relaxes observability requirements and ensures local exponential stability. Notably, the method eliminates the need for conventional velocity sensors by leveraging barometric data to enhance vertical motion awareness. Evaluation on both simulated and real flight data demonstrates that the proposed approach delivers lightweight, reliable, and efficient attitude estimation performance.

This work addresses the degraded state estimation accuracy of legged robots under diverse gaits and environments caused by fixed noise parameters in invariant extended Kalman filters (InEKF). To overcome this limitation, we propose an online adaptive strategy that dynamically tunes the observation noise covariance based on filter residuals and innovations. Relying solely on IMU and leg kinematics—without requiring foot contact force sensors or manual parameter tuning—the method significantly enhances estimation robustness and accuracy. Experimental validation on a Unitree Go2 quadruped robot in both indoor and outdoor settings demonstrates a 25% reduction in position estimation error during trotting compared to the standard InEKF with fixed parameters, achieving performance comparable to approaches that leverage foot force measurements.

This study addresses the lack of quantitative criteria for selecting between closed-loop and open-loop Kalman filter architectures in airborne aided inertial navigation. To this end, the authors propose a unified simulation framework to systematically evaluate the performance differences of these two error-state Kalman filtering approaches under varying inertial sensor accuracy levels. The methodology employs standard inertial mechanization in the geodetic frame combined with direct position aiding, enabling a comprehensive analysis of the trade-off between fusion smoothness and long-term stability. The work provides the first quantitative evidence that, with high-accuracy IMUs, the open-loop architecture yields smoother state fusion, whereas the closed-loop configuration offers superior long-term navigation stability, thereby offering empirical guidance for filter architecture selection.

This work addresses the degraded estimation performance in 3D landmark-aided inertial navigation caused by spurious observability in conventional filters. It introduces, for the first time, the Iterated Invariant Extended Kalman Filter (Iterated Invariant EKF) to this setting. By constraining the state to the observation manifold under low-noise conditions and restricting uncertainty to its tangent space, the method effectively preserves the system’s inherent observability structure. The approach integrates Lie group geometry, group-affine system modeling, and a 3D landmark observation model. Numerical simulations demonstrate that it significantly outperforms classical SO(3)-EKF, iterated SO(3)-EKF, and standard IEKF, achieving notable improvements in both estimation accuracy and consistency.