The Iterative Equivariant Filter

1 Introduction

• Mobile robots need reliable estimates of states such as position, velocity, and attitude to execute tasks including inspection, mapping, and material handling.Source: §1 · p. 1
• Prior work represents robotic states with Lie-group elements and develops deterministic observers and EKF variants, including MEKFs and Lie-group MEKFs [15, § 10].Source: §1 · p. 1
• The EqF extends MEKF methods to homogeneous spaces with Lie-group state symmetry, while InEKF convergence properties rely on group-affine dynamics and invariant measurements.Source: §1 · p. 1
• Monte-Carlo range-based localisation simulations show IterEqF has superior transient performance in both estimation error and covariance relative to the standard EqF.The paper derives the reset step as a natural consequence of the iterative correction.Source: §1 · p. 2
• IterEqF combines equivariant filtering with a Gauss–Newton iterative correction that solves a nonlinear weighted least-squares problem.The introduction positions this design as an extension of earlier iterative filters.Source: §1 · p. 2

2 Preliminaries

• The preliminaries introduce tangent spaces and tangent bundles, and describe differentials of smooth maps between manifolds.Source: §2 · p. 2
• A Lie group is a smooth manifold with a smooth group structure, with associated Lie algebra, exponential and logarithm maps, and adjoint actions.Source: §2 · p. 2
• A group action is transitive when it can reach every point of the state space from any chosen point; such a space is homogeneous.Source: §2 · p. 2

3 Equivariant Filtering

• The system model has continuous dynamics and discrete measurements, with state, input, and output maps defined on the state manifold.Source: §3 · pp. 2–3
• EqF design begins with a transitive right group action and a lift that replicates system trajectories on the group.For a free action, the lift is uniquely determined by the lift condition.Source: §3 · p. 3
• The observer evolves on the group from a fixed origin, and the state estimate is obtained by applying the group action to that origin.Source: §3 · p. 3
• The filter estimates local equivariant error coordinates as a zero-mean Gaussian with covariance, using a normal chart around the origin.The paper assumes a normal chart and considers only right group actions.Source: §3 · pp. 2–3
• The covariance dynamics are obtained by linearizing the error-coordinate dynamics around zero error and the mean input, with input-measurement noise represented by Q_t.Source: §3 · p. 3

4 Iterative Updates for Equivariant Filters

• The cost combines prior-filter information and the new measurement on the state space, then lifts the residual and cost to the symmetry group for optimization.The prior and measurement have separate covariance information, with R representing measurement-noise covariance.Source: §4 · p. 4
• Starting from the prior estimate and covariance, the filter performs Gauss–Newton iterations until convergence or a maximum iteration count, with a positive step size that may use line search.The iteration matrices are formed from derivatives of the chart, group action, and measurement function.Source: §4 · p. 4
• Perturbations along the stabilizer leave the residual unchanged, so the update is restricted to the image of Θ; the method also guards against degenerate lifted-cost directions causing instability.The group perturbation is designed to be a descent direction at each step.Source: §4 · pp. 4, 6
• Unlike prior iterative methods that optimize costs directly on Lie groups, the IterEqF lifts a homogeneous-space cost and computes final covariance in manifold-local coordinates.The nonlinear change of coordinates with the changing posterior modifies prior covariance, yielding a reset as a natural consequence of the Gauss–Newton update.Source: §4 · p. 6

5 Example: Range-based Localisation

• The robot state is represented as an SE(2) pose, with angular and forward velocity inputs and beacon distances as measurements.The state comprises planar angle and position, and each beacon defines a range measurement function.Source: §5 · p. 6

5.1 EqF Construction

• The EqF construction selects a transitive SE(2) action and a lift Λ that is independent of its state argument, simplifying computation of the EqF matrices.The construction also chooses normal coordinates on SE(2).Source: §5.1 · p. 6

5.2 Simulation Results

• Across 100 trials, IterEqF converges faster and is more statistically consistent during the transient, while both filters match accuracy after about 30 s.Position and heading errors converge more quickly for IterEqF; it uses many iterations during the initial 5 s and incurs greater computational cost.Source: Figure 1 · p. 7, §5.2 · p. 8
• The simulation compared IterEqF with standard EqF as a mobile robot followed a figure-eight Lissajous curve, using three landmarks and range measurements at 2 Hz corrupted by zero-mean Gaussian noise with 0.1 m standard deviation.Source: §5.2 · pp. 7–8

• Monte Carlo simulations show that IterEqF has superior convergence properties in both estimation error and covariance relative to the standard EqF.Source: §6 · p. 8
• The IterEqF correction step solves a weighted nonlinear least-squares problem using the Gauss–Newton algorithm.Source: §6 · p. 8
• The iterative correction naturally produces the covariance reset, without requiring a separate justification for whether the reset is needed.Source: §6 · p. 8