iterated iekf

This work addresses the challenge of achieving accurate, consistent, and convergent pose (orientation, velocity, and position) estimation in multi-IMU articulated rigid-body systems, where incorporating joint kinematic constraints without compromising the convergence and consistency of invariant filtering remains difficult. The authors propose a Lie group representation of relative L-extended poses, formulate a group-affine dynamic model, and embed kinematic constraints as noise-free pseudo-observations within an iterative invariant extended Kalman filter (IterIEKF). This approach introduces, for the first time in an invariant filtering framework, a Lie group–based state representation and constraint fusion mechanism that preserves both convergence and consistency for articulated systems. Evaluated on a UR5e robotic arm and a human leg, the method demonstrates faster convergence, lower inter-run variability, and at least 50% reduction in RMSE compared to various EKF and IterIEKF baselines, consistently achieving state-of-the-art estimation accuracy.

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.

A long-standing debate in robotics concerns whether left- and right-invariant extended Kalman filters (IEKFs) are equivalent and whether their chirality must match the measurement model’s. Method: This paper proves that, on Lie groups, left- and right-IEKFs are mathematically equivalent when a proper state reset step is incorporated—grounded in group-affine system theory and validated via simulations in a GNSS-aided inertial navigation framework. Results: The reset mechanism eliminates chirality-induced discrepancies and substantially improves the asymptotic estimation performance of IEKF. Crucially, the work refutes the common misconception that IEKF chirality must be selected based on the measurement model, establishing the reset step as an essential design element for ensuring consistency and robustness of invariant filtering. This provides both a unifying theoretical perspective and practical guidance for state estimation on Lie groups.

Long-standing inconsistencies in symmetry modeling hinder unified filter design for inertial navigation systems (INS). Method: This work proposes a systematic reconstruction framework grounded in equivariant symmetry. It introduces, for the first time in INS, two novel classes of Lie group equivariant symmetries, establishes a comprehensive taxonomy encompassing all admissible symmetry choices, and integrates equivariant filtering (EqF), matrix Lie group theory, and nonlinear stochastic observer design to achieve a unified modeling framework for both the extended Kalman filter (EKF) and its modern Lie group variants. Contribution/Results: The framework reveals the fundamental mechanisms underlying performance differences among existing filters, precisely delineates their applicability boundaries and intrinsic accuracy limits, and delivers an interpretable, scalable paradigm for designing high-precision, real-time navigation filters—thereby laying a rigorous theoretical foundation for enhancing robustness and consistency in IMU/GNSS integrated navigation.

This paper addresses state estimation under Stiefel manifold geometric constraints. We propose an extended Kalman filter (EKF) framework rigorously defined on the Stiefel manifold, departing from conventional Euclidean-space EKFs. Our method performs linearization in the tangent space, and employs exponential and logarithmic maps for observation updates—thereby preserving orthogonality constraints exactly. Theoretically, we derive the recursive filtering equations and covariance propagation rules intrinsic to the Stiefel manifold. Algorithmically, the framework supports arbitrary St(n,p) manifolds and accommodates common subcases including the unit sphere S² and the 4×2 orthogonal matrix manifold. Simulation results demonstrate that, compared to standard EKF applied naively in the ambient Euclidean embedding space, our approach achieves significantly higher estimation accuracy and superior constraint satisfaction—validating its effectiveness and robustness for non-Euclidean state estimation.

This work addresses the limitations of conventional local navigation algorithms, which neglect Earth’s curvature, rotation, and gravity variations, thereby failing to meet the demands of high-precision state estimation at a global scale. By leveraging Lie group symmetry and invariant Kalman filtering theory, the paper systematically derives and unifies the global navigation dynamics for four classes of error-state Kalman filters—including standard, left-invariant, and right-invariant formulations. It presents the first comprehensive comparison of ESKF equations under different error-state representations, clarifying their respective applicability conditions and performance characteristics in global scenarios. The proposed framework accommodates complex sensor configurations and dynamic environments, delivering a directly implementable, high-precision, and robust state estimation algorithm that advances the practical deployment of trajectory-independent error propagation theory.

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 work addresses the longstanding disconnect between IMU preintegration and propagation, which has hindered code reuse, complicated error-state transformations, and impeded consistency verification. For the first time, we establish their mathematical equivalence and propose a unified framework that is agnostic to error-state conventions. By encapsulating any IMU propagation routine—such as RK4—the framework automatically generates corresponding preintegrated measurements, bias Jacobians, and covariances, and vice versa. This formulation enables seamless migration across different error-state definitions and incorporates a built-in mechanism for consistency validation. Experiments demonstrate that an RK4-based propagation implementation achieves high agreement with GTSAM’s tangent-space and manifold preintegration modules in terms of Jacobians, covariances, and state transition matrices, confirming the correctness and practical utility of the proposed approach.

This work addresses the degraded accuracy and limited scalability of traditional ensemble Kalman filters (EnKF) in discrete-time nonlinear filtering under model misspecification or poor initialization. To overcome these limitations, the paper proposes the Exact Ensemble Kalman Filter (ExEnKF), which, for the first time within the EnKF framework, replaces Dirac measures with Gaussian approximations to more effectively explore the state space while preserving computational efficiency. Theoretical analysis establishes that ExEnKF converges to the optimal filter at a rate of $1/\sqrt{N}$, where $N$ is the ensemble size. Numerical experiments demonstrate its superior performance over standard EnKF and sequential Monte Carlo methods in highly stochastic, model-mismatched, and multiscale Lorenz-96 systems, particularly in robustly tracking unobservable hidden state components. This approach thus offers a practical, scalable, and accurate solution for high-dimensional nonlinear filtering.