Error-State Iterated Kalman Filter (ESIKF)

• ESIKF is a state estimation approach that integrates error-state representation with iterative measurement updates for enhanced accuracy in nonlinear, high-dimensional systems.
• It leverages local linearity and manifold operations to effectively handle sensor fusion challenges in applications such as inertial navigation and SLAM.
• Adaptive covariance weighting and iterated re-linearization significantly improve robustness against rapid dynamics and measurement nonlinearity.

The Error-State Iterated Kalman Filter (ESIKF) is a state estimation methodology that integrates the error-state representation of nonlinear systems with iterated measurement updates, producing a robust estimator particularly well-suited for high-dimensional, strongly nonlinear, and manifold-valued state spaces such as those encountered in inertial navigation, SLAM, and tightly coupled sensor fusion. ESIKF combines the strengths of the Error-State Kalman Filter (ESKF) and the Iterated Extended Kalman Filter (IEKF), leveraging the local linearity of error states and iterative re-linearization to deliver high accuracy even in the presence of highly nonlinear observations or rapid system dynamics (Im, 2024, Cao et al., 19 Sep 2025, Huai et al., 2023, Solà, 2017).

1. Formalism and State Structure

The ESIKF represents the system state as a nominal reference $\hat{x}$ on a (possibly manifold) state space $M$, with local deviations encoded by a small error-state vector $\delta x$ in the tangent space at $\hat{x}$. The true state $x_{true}$ is thus parameterized as

$$x_{true} = \hat{x} \boxplus \delta x, \quad \|\delta x\| \ll 1$$

where $\boxplus$ denotes the manifold-lift operation. For systems on Lie groups, such as $SE(3)$ or $SO(3)$ (e.g., for IMU-driven navigation), orientation components are updated via group operations (typically, quaternion multiplication with small perturbations mapped through the exponential map).

In typical visual-inertial-odometry systems, as in Omni-LIVO, the state is formulated as

$x_k = [q_{WB},\, v_{WB},\, p_{WB},\, b_g,\, b_a]$

with $q_{WB} \in SO(3)$ a body-to-world quaternion, $v_{WB}, p_{WB} \in \mathbb{R}^3$ velocity and position, and $b_g, b_a$ gyroscope and accelerometer biases (Cao et al., 19 Sep 2025, Solà, 2017).

2. Error-State Dynamics and Propagation

The ESIKF adopts the error-state approach for filtering, modeling process (prediction) dynamics in the tangent space about the nominal trajectory. The propagation step consists of:

• Nominal-state propagation: The predicted nominal state is advanced according to the noise-free process model:

  $\hat{x}_k^- = f(\hat{x}_{k-1}^+, u_{k-1}, 0)$

• Error-state covariance propagation: Linearize the process model about the current nominal state,

  $$F_{k-1} = \left. \frac{\partial (f(\hat{x}_{k-1}^+\boxplus \epsilon, u_{k-1}, 0)\boxminus f(\hat{x}_{k-1}^+, u_{k-1}, 0))}{\partial \epsilon} \right|_{\epsilon=0}$$

  $$P_k^- = F_{k-1} P_{k-1}^+ F_{k-1}^\top + G_{k-1} Q_{k-1} G_{k-1}^\top$$

For IMU-driven systems, the continuous-time propagation employs quaternion differential equations and bias random-walk models, with discretization for real-time implementation (Cao et al., 19 Sep 2025, Solà, 2017).

3. Iterated Measurement Update

Unlike the standard ESKF, which linearizes the measurement model only once about the prior, the ESIKF applies a Gauss–Newton-style iterative refinement within each measurement update, reducing linearization error.

• Residual and Jacobian computation: At each iteration $\ell$, compute the measurement model residual and its Jacobian with respect to the error state at the current estimate $x_{k|k-1} \boxplus \delta x^{(\ell-1)}$.
• Normal equations: Solve for the error-state update:

  $$\delta x^{(\ell)} = \delta x^{(\ell-1)} + K^{(\ell)} \big[r^{(\ell)} - H^{(\ell)} \delta x^{(\ell-1)}\big]$$

  where $K^{(\ell)}$ is the Kalman gain computed with the linearized Jacobian and measurement covariance.

• State correction: Upon convergence, inject the total increment into the nominal state:

  $x_{k|k} = x_{k|k-1} \boxplus \delta x^{(L)}$

  Covariance is updated via the first-order or Joseph form, ensuring positive-definiteness and correct uncertainty propagation (Im, 2024, Cao et al., 19 Sep 2025, Huai et al., 2023, Solà, 2017).

This iterative approach offers increased accuracy in regimes with high nonlinearity, such as vision-based measurements or aggressive vehicle maneuvers.

4. Adaptive Multi-Modal Fusion and Covariance Weighting

In systems with heterogeneous measurements (e.g., multi-camera, LiDAR), ESIKF supports the joint fusion of diverse sensor modalities, balancing their contributions by adaptive covariance weighting. In Omni-LIVO:

• The measurement covariance $R_{\text{multi}}$ is constructed as a block-diagonal matrix with scaling factors per modality or per camera:

  $$R_{\text{multi}} = \mathrm{diag}(\alpha_1 R_{\text{base}}^{(1)}, \ldots, \alpha_C R_{\text{base}}^{(C)}, \alpha_{\text{cross}} R_{\text{cross}})$$

• Scaling factors $\alpha_i$ are computed online as a function of per-view tracking statistics, such as patch coverage and average residuals, down-weighting unreliable measurements and dynamically prioritizing robust sensor streams (Cao et al., 19 Sep 2025).

This mechanism enables ESIKF to automatically favor the most informative and consistent measurements during fusion, increasing estimator robustness across challenging multimodal scenarios.

5. Algorithmic Structure and Implementation Considerations

A canonical ESIKF iteration encompasses the following sequence:

1. Synchronize sensor input frames (IMU, camera(s), LiDAR).
2. Propagate nominal state and covariance.
3. Process sensor residuals, compute analytic Jacobians.
4. Initialize iterative update (with $\delta x^{(0)} = 0$).
5. For each iteration:
   • Re-linearize at the current state hypothesis.
   • Stack all residuals; update measurement covariance adaptively.
   • Compute Kalman gain; update the error-state increment.
   • Validate convergence (e.g., norm of increment below $\epsilon$, or iteration cap $\ell_{\max}$ reached).
6. Inject the converged error-state into the nominal state.
7. Normalize quaternions after updates, if the state includes orientation.
8. Reset and update covariance using a numerically robust scheme (Cao et al., 19 Sep 2025, Im, 2024, Huai et al., 2023).

Tukey weighting or robust loss functions may be employed on visual residuals to mitigate the influence of outliers, as in Omni-LIVO. Analytical Jacobians are recommended for performance and stability. SLAM backends benefit from block-sparsity exploitation in covariance updates.

6. Comparative Properties and Performance

The ESIKF inherits and extends the advantages of both ESKF and IEKF while mitigating their respective weaknesses:

Filter	Update Domain	Linearization Method	Iteration	Key Attributes
ESKF	Error-state	Single (tangent space)	No	Minimal parameterization, efficient
IEKF	Full state	Iterated (full state)	Yes	Robust to nonlinearity, but higher computational cost, can suffer from parameterization singularities
ESIKF	Error-state	Iterated (tangent space)	Yes	Reduces linearization error, robust for strongly nonlinear measurements, preserves minimal parameterization, real-time feasible (2–8 iterations)

Empirically, ESIKF demonstrates superior convergence and numerical stability under high measurement nonlinearity and longer update intervals. In Omni-LIVO, ESIKF enables the system to outperform previous tightly coupled odometry frameworks:

• Successfully processes all 14 sequences in challenging SLAM benchmarks where single-camera methods fail on up to half the cases.
• Delivers approximately 34% lower average RMSE versus FAST-LIVO2, maintaining sub-0.1m errors in degraded visual and dynamic environments.
• Achieves 35 ms processing per packet (four cameras), incurring only ~1.2x overhead relative to single-camera ESKF baselines, and generates 2–4x denser colored point clouds (Cao et al., 19 Sep 2025).

7. Numerical and Practical Issues

Several technical subtleties are crucial for reliable ESIKF deployment:

• Convergence and complexity: Each iteration is $\mathcal{O}(n^3)$, with $n$ the error-state dimension; total iterations are typically capped ($\ell_{\max}$ between 5–8) with a tight convergence threshold ($\epsilon \sim 10^{-5}$).
• Manifold operations: Consistent choice and implementation of $\boxplus$, $\boxminus$ is required for correct Jacobians, especially for rotation groups (Solà, 2017, Huai et al., 2023).
• Covariance management: Ensure positive-definite, symmetric $P$ at each update; Joseph form is preferred for numerical stability.
• Quaternion normalization: After updates in the orientation error-state, the nominal quaternion must be renormalized to avoid drift.
• Block-sparsity: Exploit block-diagonal structure in covariance and measurement models to accelerate high-dimensional updates, especially in multi-view or multi-sensor settings (Huai et al., 2023, Im, 2024).
• Robustification: Incorporating robust error norms (e.g., Tukey loss) enhances resilience to outliers in sensor data (Cao et al., 19 Sep 2025).

These considerations are fundamental for real-time operation and reliable performance, particularly when fusing high-rate heterogeneous sensory streams in SLAM and odometry systems.

---

References:

• "Omni-LIVO: Robust RGB-Colored Multi-Camera Visual-Inertial-LiDAR Odometry via Photometric Migration and ESIKF Fusion" (Cao et al., 19 Sep 2025)
• "Notes on Kalman Filter (KF, EKF, ESKF, IEKF, IESKF)" (Im, 2024)
• "A Quick Guide for the Iterated Extended Kalman Filter on Manifolds" (Huai et al., 2023)
• "Quaternion kinematics for the error-state Kalman filter" (Solà, 2017)