Symbolic Error-State Iterated KF on Manifolds

• Symbolic Error-State IKFoM is a framework for nonlinear state estimation on differentiable manifolds using minimal-parameter error-state representations.
• It employs iterated Gauss-Newton updates in the tangent space to mitigate linearization errors and handle non-Euclidean geometries such as SO(3) and SE(3).
• The method combines analytical retraction and covariance propagation to deliver efficient, robust performance in applications like SLAM, navigation, and sensor fusion.

The Symbolic Error-State Iterated Kalman Filter on Manifolds (IKFoM) is an advanced statistical estimation framework for recursively estimating the state of a nonlinear dynamic system whose state evolves on a differentiable manifold. IKFoM generalizes the classical Kalman Filter by combining a symbolic, minimal-parameter error-state formulation with the robustness of iterated least-squares updates, thereby enabling efficient and accurate filtering for systems exhibiting non-Euclidean geometry, such as those involving rotations or constrained vector spaces (Im, 2024, Huai et al., 2023, Solà, 2017, He et al., 2021).

1. Mathematical Foundations and System Representation

IKFoM targets systems whose state $x \in \mathcal{M}$, evolves on a smooth manifold $\mathcal{M}$ (e.g., $\mathbb{R}^n$, $SO(3)$, $SE(3)$, $S^2$). At any time, the filter maintains a nominal estimate $x$ and a small error-state vector $\delta x \in T_x\mathcal{M}$ in the tangent space at $x$, where the true state is expressed as $x_\text{true} = x \oplus \delta x$ (Im, 2024, Huai et al., 2023, He et al., 2021).

Two fundamental manifold operations are central:

• Retraction ($\mathcal{M}$0) or boxplus ($\mathcal{M}$1): $\mathcal{M}$2 maps a small tangent-vector error to the manifold, typically approximated locally by the exponential map.
• Difference ($\mathcal{M}$3) or boxminus ($\mathcal{M}$4): $\mathcal{M}$5 computes the local tangent-vector difference between two nearby manifold points, usually via the logarithm map.

IKFoM operates by “lifting” distributions and uncertainty to the tangent space, performing linear-Gaussian Kalman updates there, and then “retracting” the updated state back onto the manifold, thereby respecting manifold structure at all times (Im, 2024, He et al., 2021). This is exemplified for $\mathcal{M}$6 in quaternion and rotation matrix coordinates (Solà, 2017).

2. Time Update and Error-State Propagation

Let $\mathcal{M}$7 be the (possibly nonlinear) system dynamics, with $\mathcal{M}$8 the control input and $\mathcal{M}$9 process noise. The propagation step consists of:

1. Nominal state propagation:

$\mathbb{R}^n$0

The nominal is advanced by one timestep using the zero-noise model.

2. Linear error-state propagation:

$\mathbb{R}^n$1

The error dynamics are linearized around the nominal. Jacobians are given by

$\mathbb{R}^n$2

$\mathbb{R}^n$3

(Im, 2024, Huai et al., 2023)

3. Covariance prediction:

$\mathbb{R}^n$4

The covariance is evolved in the tangent space at the newly predicted nominal.

This formulation enables leveraging standard linear-Gaussian prediction machinery, regardless of the manifold’s topology (He et al., 2021).

3. Measurement Update and Iterated Correction

The measurement model is $\mathbb{R}^n$5, with $\mathbb{R}^n$6 and $\mathbb{R}^n$7 possibly highly nonlinear. The critical IKFoM innovation is the iterative Gauss-Newton update strategy, implemented in the tangent space:

1. Linearize measurement at current iterate: For each iteration $\mathbb{R}^n$8,

$\mathbb{R}^n$9

where $SO(3)$0 (Im, 2024).

2. Residual evaluation:

$SO(3)$1

(Im, 2024, Huai et al., 2023)

3. Gauss-Newton step: Solve

$SO(3)$2

(Im, 2024), and update

$SO(3)$3

$SO(3)$4

1. Convergence check: Repeat until $SO(3)$5.
2. Posterior update: At the final iterate $SO(3)$6, set

$SO(3)$7

$SO(3)$8

with $SO(3)$9 (Im, 2024).

This procedure refines the state estimate by repeatedly relinearizing the measurement function at the current best guess, yielding improved results over a single-shot linearization (as in EKF), particularly in the presence of strong nonlinearity or large uncertainty (Huai et al., 2023, Im, 2024).

4. Symbolic Structure and Manifold-Aware Implementation

IKFoM is fully symbolic: all update operations, including state, error, Jacobians, and retractions, are specified in terms of symbolic manifold calculus. The approach is generic; each manifold (e.g., $SE(3)$0, $SE(3)$1) requires only explicit definitions for $SE(3)$2, $SE(3)$3, and oplus, along with their associated Jacobians (He et al., 2021, Huai et al., 2023). For instance, on $SE(3)$4, $SE(3)$5 involves the matrix exponential and $SE(3)$6 the matrix logarithm, and linearizations can be computed using closed-form series for the right Jacobian $SE(3)$7 (He et al., 2021). The covariance is always expressed in the local coordinates of the tangent space, and is aligned by appropriate pushforward/pullback transforms during state retractions.

A key feature is that after each state correction, both the nominal state and the covariance are “retracted” (or “reset”) onto the updated manifold reference, ensuring global consistency and numerical stability. This is critical for, e.g., attitude estimation, where naive representations may suffer from parameterization singularities (Solà, 2017). All compound states (products of manifold and Euclidean primitives) are handled by block-diagonal composition of the respective manifold operators (He et al., 2021).

5. Algorithmic Summary and Computational Workflow

A standard IKFoM iteration, as specified in the literature (Huai et al., 2023, He et al., 2021, Im, 2024), proceeds as follows:

Step	Operation	Coordinate System
State Prediction	$SE(3)$8	Manifold
Covariance Propagate	$SE(3)$9	Tangent at $S^2$0
Initialize Update	$S^2$1, $S^2$2	Tangent at $S^2$3
Iterative Correction	For $S^2$4:	Tangent at $S^2$5
	Residual: $S^2$6	Measurement space
	Jacobian: $S^2$7	Linear in $S^2$8
	Gauss-Newton: $S^2$9 as above	Tangent space
	Retract: $x$0, $x$1
Terminate	When $x$2
Posterior State/Cov	$x$3, $x$4	Tan. at $x$5

A canonical C++-style pseudocode reflecting these symbolic operations is provided in (He et al., 2021).

6. Advantages and Applications

By leveraging error-states and iterated relinearization on the manifold, IKFoM significantly attenuates local linearization errors inherent to EKF and single-linearization ESKF, especially in the presence of high measurement nonlinearity or large a priori uncertainty (Im, 2024, Huai et al., 2023). Gauss-Newton iteration drives the estimate toward the exact local Maximum A Posteriori (MAP) solution, with quadratic convergence under regularity and small-residual conditions (Im, 2024).

The use of tangent-space error vectors and manifold retraction/difference operators enables minimal, non-singular parameterizations and seamless mixing of Euclidean and non-Euclidean components. This improves both accuracy and computational efficiency for systems such as Visual/IMU-based navigation, tightly-coupled lidar-inertial systems, and general Simultaneous Localization and Mapping (SLAM) pipelines, as demonstrated in practical open-source implementations (He et al., 2021).

7. Implementation and Numerical Considerations

Proper implementation requires accurate definitions of the manifold operations ($x$6, $x$7), their derivatives, and careful covariance transformation and normalization after each update (He et al., 2021, Solà, 2017). For compound manifolds, block-diagonalization of Jacobians is standard.

Numerical stability is enhanced by regular covariance resets, careful normalization of unit-norm quantities (e.g., quaternions), and condition number checks. Four to six Gauss-Newton iterations usually suffice for practical convergence; excessive iterations are discouraged due to diminishing returns and potential numerical issues (He et al., 2021).

The C++ toolkit referenced in (He et al., 2021) encapsulates the symbolic machinery for general differentiable manifolds, requiring the user only to supply system- and measurement-specific functions and Jacobians. New manifold types are incorporated by defining their primitive operations and associated Jacobians.

---

References:

• (Im, 2024) Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)
• (Huai et al., 2023) A Quick Guide for the Iterated Extended Kalman Filter on Manifolds
• (Solà, 2017) Quaternion kinematics for the error-state Kalman filter
• (He et al., 2021) Kalman Filters on Differentiable Manifolds