CS Paper Radar

The Iterative Equivariant Filter

Pieter van Goor, James Richard Forbes

eess.SY cs.RO ·submitted 2026-09-11 01:22 UTC · v1

Abstract

This paper presents the iterative equivariant filter (IterEqF). The iterative extended Kalman filter (IterEKF) replaces the standard EKF correction 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.

Comment
9 pages, 1 figure, submitted

Closest papers

Nearest neighbours by meaning across the whole index.

Accuracy Evaluation of INS/ZUPT Filtering Methods Based on Different Geometric Error Definitions

cs.RO ·2026-09-25 ·Wei Ouyang, Jiale Han, Yarong Luo, Maoran Zhu
similarity 0.68
Abstract

Geometric filters have recently been introduced to improve the accuracy and consistency of inertial-based integrated navigation systems. Error states were defined through specific group operations, introducing state correlations in error definition, which were lacked in the additive error used by a conventional indirect Kalman filter. The desirable consistent filtering models can be obtained based on specific geometric errors. For zero-velocity measurements expressed in the reference frame, this paper derives left-error process and measurement models from invariant filtering, two-frame-group filtering, and equivariant filtering. Importantly, a new group operation is introduced for the left tangent-group equivariant error. The analysis shows that the two-frame-group invariant extended Kalman filter (TFG-IEKF) and the tangent-group equivariant filter (TG-EqF) do not offer a significant consistency advantage over the invariant extended Kalman filter (IEKF). Experiments with an INS/ZUPT measurement system show that, under small initial attitude errors, the conventional indirect extended Kalman filter (EKF) achieves loop-closure position errors below $0.1\%$ of the traveled distance, while the three geometric filters achieve comparable positioning accuracy.

0.13

Equivariant Filter Design for Acoustic and Depth Aided Inertial Navigation Systems

cs.RO ·2026-09-17 ·Arihant Lunawat, Pieter van Goor, Frank Dellaert, Stefan B. Williams
benchmark similarity 0.64
Abstract · 12 pages, 6 figures, submitted to ICRA 2027

Autonomous Underwater Vehicles (AUVs) navigating without GPS typically fuse inertial measurements with acoustic Doppler Velocity Log (DVL) velocities and pressure-derived depth. Posing the navigation state on a Lie group improves accuracy and consistency. However, state-of-the-art filters based on the Invariant Extended Kalman Filter (IEKF) append the Inertial Measurement Unit (IMU) biases as a Euclidean extension, which breaks the group-affine structure required for exact log-linear error dynamics, causing the reported covariance to degrade alongside the estimate. We apply the Tangent-Group (TG) symmetry, which carries the biases within the geometry of the state space, to derive an Equivariant Filter (EqF) for this system, leaving zero linearization error in the navigation states and second-order error only in the biases. We develop an equivariant output model for the DVL, whose update incurs only third-order linearization error, together with a direct pressure output. Monte Carlo simulations benchmark the TG-EqF against a Two-Frame-Group IEKF and a Multiplicative EKF. The TG-EqF reduces error by 18--25\% against both alternatives in each of attitude, velocity, and position. The main benefit is in the covariance it estimates: its Average Normalized Estimation Error Squared (ANEES) stays closer to its nominal value of one than that of the others. Offline analysis on AUV field data corroborates the findings of the simulations, demonstrating reduced position drift.

0.19

WRAP: Wasserstein-Robust Adaptive Plug-in for Robot Localization

cs.RO eess.SY ·2026-08-10 ·Minhyuk Jang, Astghik Hakobyan, Jungjin Lee, Naira Hovakimyan, Insoon Yang
similarity 0.62
Abstract

Robotic localization under changing sensing conditions can suffer from biased errors and miscalibrated covariances. We present WRAP, an adapter-agnostic Wasserstein-robust plug-in for nonlinear extended Kalman filter (EKF) and error-state Kalman filter (ESKF) stacks. A causal module supplies time-varying effective process and measurement statistics; a mean-preserving Wasserstein local update then computes least-favorable covariances and a robust gain without changing the propagation model, residual, or retraction. This separates mean adaptation from covariance robustification and uses distinct radii for propagation and sensing. On 18 UWB--IMU sequences held out from adapter training, adapter-only and WRAP reduce mean 3-D position RMSE by $19.8\%$ and $27.4\%$ relative to the nominal ESKF; an isotropic ablation reaches $19.5\%$, linking the incremental gain to directional process-covariance redistribution. An in-sample GNSS--INS study shows that mean adaptation provides most of the accuracy gain, while DR improves consistency and mitigates over-tightened classical covariance estimates. The robust solve takes 0.05 ms for UWB and 2.92 ms for GNSS on a Jetson Orin Nano.

0.04

Consistent Distributed Invariant EKF SLAM with Point and Object Features

cs.RO ·2026-10-22 ·Mengya Xu, Xianjia Yu, Ha Sier, Shoudong Huang, Tomi Westerlund
similarity 0.60
Abstract · Submitted to IEEE Robotics and Automation Letters (RA-L)

Multi-robot SLAM allows robots to improve their estimates by exchanging map information, but repeated exchanges create unknown correlations between local estimators. Covariance intersection (CI) can fuse estimates without tracking these correlations. In right-invariant extended Kalman filter (RI-EKF) SLAM, however, point features obtain their invariant errors through the robot rotation, so a neighbor's map estimate is expressed in its own error coordinates and is correlated with its pose, and cannot be fused by CI directly. This paper presents Joint-CI, a unified fusion method for distributed RI-EKF SLAM with point, object, and mixed point--object maps. Using a neighboring robot's local posterior and an inter-robot observation, a joint estimate of the receiving robot's pose and the common features is constructed in the receiving robot's error coordinates, retaining the robot--feature and feature--feature correlations. Partial-state CI then fuses this estimate with the receiving robot's local posterior. We show that the unified formulation and the fusion preserve the six unobservable directions associated with global rotation and translation, and that communication and fusion preserve first-order covariance consistency given consistent local posteriors. Monte Carlo simulations and a real-world dataset-derived experiment show that Joint-CI improves estimation accuracy while maintaining consistent, mildly conservative uncertainty.

0.23

Probe Sets for Nonlinear Kalman Filtering: Multi-Center Gains and Covariance Recalibration

eess.SY eess.SP ·2026-09-13 ·Shida Jiang, Shengyu Tao, Scott Moura
code/data similarity 0.60
Abstract

Kalman filters (KFs) choose the gain as the product of the state-measurement cross-covariance and the inverse of the innovation covariance. In nonlinear systems, the predictive distribution generally changes shape, and these covariances require approximation. Conventional nonlinear KFs approximate them around a single center, the predicted state. They can produce inaccurate gains and overconfident covariance estimates when one local approximation does not represent the measurement geometry across the uncertainty region. To address this issue, we introduce probe sets, small covariance-scaled collections of states at which the filter repeats its local approximation. We then select the gain that minimizes the average covariance reported by these local approximations. The same construction is used in our previously proposed covariance recalibration step to improve covariance consistency. The construction applies to different KF variants without requiring a specific approximation rule. For quadratic measurements, we derive conditions under which probing yields lower normalized true error variance than the corresponding single-center gain when prediction uncertainty is sufficiently understated. We apply the framework to four KF variants and compare the resulting filters with multiple baselines across 300 randomized setups for each of two systems. The results show that probing preserves typical accuracy while substantially reducing covariance inconsistency and the rare large-error trajectories that dominate the root mean square of per-run errors. The code is available at https://github.com/Shida-Jiang/Probe_KF.

0.35

An Equivariant Filter for Spacecraft Nonlinear Relative Navigation with Range and Bearing Measurements

eess.SY ·2026-09-11 ·Gil Serrano, Bruno J. Guerreiro, Pedro Lourenço, Rita Cunha
unusual similarity 0.59
Abstract · Extended version of a manuscript submitted to IEEE for possible publication

Relative navigation is a fundamental task in space proximity operations and autonomous rendezvous. The nonlinear relative orbital dynamics are equivariant and admit a semidirect product Lie group symmetry. This property is used to design an Equivariant Filter (EqF). The filter estimates the relative position and velocity between a chaser spacecraft and a target by lifting the filter dynamics to the group, while respecting the underlying geometry of the problem. Simulations demonstrate the filter's performance and effectiveness.

0.35

Belief-Adaptive Online Autonomy for Quadrotor UAV Navigation under GNSS Degradation in Urban Environments

cs.RO eess.SY ·2026-09-13 ·Deepak Kumar Panda, Weisi Guo
similarity 0.59
Abstract

Reliable online autonomy is critical for quadrotor operation in urban airspaces, where global navigation satellite systems (GNSS) measurements suffer from multipath, blockage, and latency issues, introducing non-stationary, temporally correlated errors that degrade conventional GNSS-IMU fusion. This paper presents a belief-adaptive online autonomy framework that augments an extended Kalman filter (EKF) with explicit GNSS trust modelling, second-order online belief adaptation, and latency-aware out-of-sequence measurement handling. GNSS trust is represented as a latent belief state that modulates measurement weighting and multipath bias uncertainty, and is updated online using EKF consistency signals. Unlike reactive covariance tuning, the proposed approach enables proactive and stable sensor trust adaptation without prior environmental knowledge or offline training. Evaluation in simulated urban air mobility scenarios with correlated multipath, stochastic latency, and obstacle constraints demonstrates improved belief convergence, smoother trajectories, and reduced estimation and tracking errors compared to naive, adaptive, and first-order baselines. The framework preserves classical GNSS-IMU fusion structure and can be integrated directly into existing flight control pipelines, supporting robust online autonomy in GNSS degraded environments.

0.16

A simple derivation of the Kalman filter

eess.SP math.OCeess.SYmath.ST+1 ·2026-09-02 ·Marco Chiani, Giovanni Petris, Moe Z. Win
published similarity 0.59
Abstract · To be published in IEEE Signal Processing Magazine

In this lecture note, we present a concise and self-contained derivation of the discrete-time Kalman filter equations that requires only a basic understanding of least squares estimation. The treatment is designed to minimize mathematical overhead while preserving both rigor and generality.

0.33