measurement linearization

This work addresses state estimation for discrete-time nonlinear stochastic systems with finite-dimensional states and infinite-dimensional measurements—such as image fields—arising in visual localization and tracking. Conventional methods struggle to rigorously model the relationship between image gradients and the Jacobian of the observation function with respect to the state. To overcome this, we propose an extended Kalman filter (EKF) grounded in infinite-dimensional random field modeling. Crucially, we establish, for the first time from a systems-theoretic perspective, that image gradients correspond precisely to the Fréchet derivative of the observation functional with respect to the state—thereby providing a rigorous mathematical foundation for gradient-based visual state estimation. Evaluated on monocular visual-inertial navigation for unmanned aerial vehicles, our approach reduces root-mean-square error by an order of magnitude compared to VINS-MONO, demonstrating substantial improvements in both estimation accuracy and robustness.

This work addresses the challenge that nonlinear Kalman filters—such as the Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF)—often struggle to balance robustness and accuracy due to a lack of systematic design principles. To this end, the paper introduces a covariance compensation framework that quantifies the deviation from EKF’s covariance prediction and establishes design criteria for performance improvement. It presents, for the first time, the concept of covariance compensation along with three core guidelines: invariance under orthogonal transformations, sufficient compensation relative to the EKF baseline, and a preference for underconfident compensation magnitudes. Through theoretical analysis and numerical experiments, the study demonstrates that adherence to these principles significantly enhances estimation accuracy and reveals that commonly adopted fixed-parameter strategies in the literature are generally suboptimal.

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.

Traditional Kalman filter variants suffer from numerical divergence on synthetic data, reliance on the small-velocity assumption, and accuracy limitations imposed by motion modeling. To address these issues, this paper proposes a manifold-based invertible Kalman filter. By operating directly on the state manifold, the method eliminates the small-velocity assumption, rendering estimation accuracy dependent solely on sensor noise. A numerically stable covariance update mechanism is introduced to suppress filter divergence effectively. Additionally, a heuristic sensor quality detection module is designed to accommodate high-precision multi-sensor fusion—such as 9-axis IMUs and integrated odometry–accelerometer–barometer systems—thereby significantly improving trajectory reconstruction robustness and accuracy in challenging environments (e.g., underwater). Experimental results on both synthetic and real-world datasets demonstrate superior numerical stability and state estimation performance compared to conventional approaches.

This work addresses the issue of state estimation error accumulation on Lie group manifolds caused by linearization of nonlinear observation models in tangent spaces. To circumvent linearization altogether, the authors propose a natural gradient Gaussian approximation filtering framework. The approach reformulates manifold-based filtering as a parameter optimization problem over Gaussian incremental variables, where increments are mapped onto the prior state via the exponential map and iteratively refined using natural gradients. Under invariant observation models, a closed-form covariance update is derived, achieving a favorable balance between accuracy and computational efficiency. Experimental validation on the Unitree GO2 quadruped robot across diverse terrains demonstrates approximately 40% reduction in estimation error compared to existing filters, with comparable computational overhead.

This work investigates the stability of Gaussian inference on smooth manifolds, where marginalization and conditioning typically yield non-Gaussian distributions influenced by underlying geometry, complicating the assessment of linearization-based methods. Focusing on tangent-space linearization, the study establishes the first explicit non-asymptotic Wasserstein-2 (W₂) stability bound, cleanly separating local second-order geometric distortion from non-local tail leakage effects. The proposed closed-form diagnostic depends only on the mean, covariance, and proxies for curvature or injectivity radius, revealing that normal-direction uncertainty dominates error when locality assumptions break down. Experiments on toroidal and planar systems demonstrate a sharp degradation in calibration performance when √|Σ|_op/R ≈ 1/6, providing a practical trigger for switching to multi-chart or sampling-based manifold inference schemes.

In aided inertial navigation systems, the simultaneous identification of unknown constant measurement delays and system states is inherently challenging, significantly degrading estimation accuracy. This work addresses this issue by analyzing the continuous symmetries arising from trajectory geometry and the delayed measurement model, revealing a broader class of degenerate trajectories. For the first time, these degeneracies are attributed to symmetries under the special Galilean group. By integrating Lie group methods, identifiability theory, and Jacobian linearization analysis, the study precisely characterizes the geometric properties of unidentifiable trajectories and establishes a rigorous theoretical link between system symmetries and the loss of identifiability. These insights provide critical guidance for the design of multi-sensor fusion systems operating with time-delayed measurements.

This work addresses the absence of computable state estimation error bounds in learning-based Kazantzis–Kravaris/Luenberger (KKL) observers by proposing a physics-informed neural network (PINN) approach that jointly learns the KKL transformation and its left inverse mapping. For the first time, an explicit error bound is derived that depends solely on verifiable quantities associated with the trained neural networks. This bound applies to nonlinear systems subject to bounded additive measurement noise and enables formal performance guarantees for the observer over a specified region of operation. Experimental evaluations on multiple nonlinear benchmark systems demonstrate that the derived error bound is both tight and effective, significantly enhancing the reliability and certifiability of state estimates in noisy settings.

This work addresses the absence of analytical solutions for Bayesian filtering in nonlinear dynamical systems, where existing Gaussian approximation methods often neglect the underlying geometric structure of probability distributions. From an information-geometric perspective, the authors model prediction and measurement update steps as inference processes on the manifold of Gaussian distributions and propose the geometry-aware NANO filter. This approach iteratively refines the posterior mean and covariance via a single-step natural gradient descent, preserving covariance positive definiteness while exactly recovering the Kalman update in the linear Gaussian case. The method demonstrates superior accuracy and robustness across diverse applications, including satellite attitude estimation, SLAM, and state estimation for quadrupedal and humanoid robots.