Iterated Extended Kalman Filter (iEKF)

• iEKF is a nonlinear Bayesian estimation method that iteratively refines state estimates via repeated Gauss–Newton linearizations for improved accuracy in strongly nonlinear systems.
• It is widely applied in inertial navigation, visual-inertial fusion, and SLAM, balancing computational efficiency with robust, real-time performance.
• Specialized variants like IterIEKF exploit Lie group symmetries to enforce hard measurement constraints, ensuring fast error collapse and enhanced consistency.

The Iterated Extended Kalman Filter (iEKF) is a nonlinear Bayesian estimation technique that extends the standard Extended Kalman Filter (EKF) by performing multiple Gauss–Newton iterations in the measurement update step, each time re-linearizing the observation model around the latest state estimate. This iterative process yields improved accuracy and local consistency in regimes with strong nonlinearities, especially when measurement noise is low and the likelihood is sharply peaked. When the error structure and nonlinearities exhibit additional symmetry (such as group-affinity on Lie groups), specialized variants like the Iterated Invariant Extended Kalman Filter (IterIEKF) can achieve further theoretical guarantees, including hard encoding of noise-free measurements, consistency, and fast error collapse. Modern applications include inertial navigation, robot odometry, visual-inertial fusion, SLAM, and high-contrast astronomical imaging. The iEKF has become a standard algorithmic primitive in robotics and estimation, often serving as a computationally efficient online alternative to nonlinear batch optimization or sliding-window factor graph methods.

1. Mathematical Foundations and Algorithmic Structure

The iEKF addresses the discrete-time nonlinear estimation problem:

$$\begin{align*} x_k &= f(x_{k-1}, u_{k-1}, w_{k-1}), \quad w_{k-1} \sim \mathcal{N}(0, Q_{k-1}) \ z_k &= h(x_k) + n_k, \quad n_k \sim \mathcal{N}(0, R_k) \end{align*}$$

At each time step, the EKF produces a Gaussian approximation to the posterior $\mathcal{N}(x_k; \hat{x}_k, P_k)$. The update step is derived by minimizing the maximum a posteriori (MAP) objective: $$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$ The EKF linearizes $h(x)$ at the predicted mean and applies a single Kalman update. The iEKF instead finds the posterior mode using Gauss–Newton iterations:

• At iteration $j$, linearize $h$ at $x_{k|k}^{(j)}$, obtain Jacobian $H^{(j)}$.
• Compute the innovation $$r^{(j)} = z_k - h(x_{k|k}^{(j)}) - H^{(j)}(\hat{x}_k^- - x_{k|k}^{(j)})$$.
• Kalman gain: $$K^{(j)} = P_k^-{H^{(j)}}^T (H^{(j)} P_k^- {H^{(j)}}^T + R_k)^{-1}$$.
• Update: $\mathcal{N}(x_k; \hat{x}_k, P_k)$0.
• Iterate until $\mathcal{N}(x_k; \hat{x}_k, P_k)$1 falls below a threshold.

On manifolds or Lie groups, the update uses group exponentials/logarithms and $\mathcal{N}(x_k; \hat{x}_k, P_k)$2 operations, with tangent-space error coordinates and associated Jacobians (Huai et al., 2023).

2. Theoretical Properties and Consistency

The iEKF approximates the MAP estimate for nonlinear models using iterated local linearizations. Drawing on a Gauss–Newton interpretation, it achieves quadratic convergence near the true mode when the measurement is sufficiently informative and $\mathcal{N}(x_k; \hat{x}_k, P_k)$3 is twice differentiable with a well-conditioned Hessian (Kent et al., 2019, Im, 2024).

In the limit of perfect measurement (noise-free or $\mathcal{N}(x_k; \hat{x}_k, P_k)$4), the iEKF projects the posterior onto the observed manifold $\mathcal{N}(x_k; \hat{x}_k, P_k)$5, and collapses covariance in the measured subspace, mirroring the canonical projection property of the linear Kalman filter (Goffin et al., 2024, Santana et al., 16 Apr 2026). For invariant models on Lie groups, the IterIEKF achieves this property exactly, ensuring hard encoding of measurement constraints and persistence of zeroed covariance directions through later updates. For general nonlinear models, the result is approximate but converges to the same property locally (Goffin et al., 2024).

3. Algorithmic Implementations and Variants

The iEKF core pattern consists of:

• Prediction via first-order Jacobian approximation of $\mathcal{N}(x_k; \hat{x}_k, P_k)$6, propagating the mean and covariance.
• An inner measurement-update loop, re-linearizing the residual and Jacobian at each iteration.
• Kalman-gain computation, leveraging either the classical form (measurement-dimensional inversion) or a state-dimensional form using the matrix-inversion lemma, critical for real-time and large-scale systems (Xu et al., 2020, Yu et al., 2024).
• Stopping when the increment is below a prescribed norm or the cost function increases.

On matrix Lie groups (e.g., $\mathcal{N}(x_k; \hat{x}_k, P_k)$7, $\mathcal{N}(x_k; \hat{x}_k, P_k)$8, $\mathcal{N}(x_k; \hat{x}_k, P_k)$9), $$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$0 is implemented with group exponentials, and invariant errors are employed for consistent error propagation (Huai et al., 2023, Goffin et al., 2024). For quasi-group-affine systems with additional error states (e.g., inertial biases, lever arms), the state is embedded in a higher dimensional algebra (e.g., Clifford algebra-based iEKF) to preserve near-autonomy in error dynamics (Ouyang et al., 2023).

Dual-iteration schemes (e.g., I²EKF) add an outer loop to also rectify distortion in measurement data (e.g., LiDAR de-skewing), resulting in further accuracy improvements in fast or highly-dynamic environments (Yu et al., 2024).

4. Practical Applications

The iEKF and its manifold/invariant variants underpin state-of-the-art systems in:

• Legged robot odometry and inertial-kinematic state estimation: IterIEKF on $$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$1 fuses IMU with proprioceptive kinematics for robust, environmental-insensitive base-state estimation. Empirically, IterIEKF outperforms standard IEKF and SO(3)-based approaches in velocity, orientation, and pitch accuracy, and exhibits superior convergence and consistency under large initialization error (Santana et al., 16 Apr 2026).
• LiDAR-inertial odometry: Real-time iEKF enables tightly-coupled fusion of thousands of points per scan at high frequency (e.g., >1,200 features, 25 ms/scan in FAST-LIO), even on embedded hardware (Xu et al., 2020). Dual-iteration iEKF variants further mitigate motion distortion and adapt process noise in aggressive scenarios (Yu et al., 2024).
• GNSS/INS: Iterated Clifford algebra-based EKF handles large initial attitude errors and significant MEMS IMU biases, achieving convergence properties superior to classical one-shot EKF (Ouyang et al., 2023).
• Visual-inertial fusion and SLAM: Direct photo-geometric iEKF variants enable robust and fast ego-motion estimation, outperforming feature-based and unscented filters in highly nonlinear, resource-constrained micro-aerial vehicle contexts (Zhong et al., 2020).
• Astrodynamics: IEKF within orbital tracking stabilizes estimation in high-eccentricity, angle-only measurement scenarios where one-step EKF/UKF can fail catastrophically (Kent et al., 2019).
• High-contrast imaging: IEKF formalism permits joint recursive estimation of starlight bias and incoherent intensity, accelerating convergence in coronagraphy applications (Riggs et al., 2016).

5. Comparison to Related Methodologies

The iEKF is closely related to several broader families of nonlinear optimization and filtering methods:

• EKF: Performs a single linearization at the predicted mean; iEKF generally outperforms EKF in strongly nonlinear regimes, particularly when measurement noise is small compared to prior uncertainty (Kent et al., 2019, Im, 2024).
• Unscented Kalman Filter (UKF) and iterated UKF (IUKF): The latter uses sigma-points to propagate nonlinearities; iterated versions (IUKF, OCUKF) mirror the Gauss–Newton structure of iEKF, and in practice achieve similar performance when properly tuned (Kent et al., 2019).
• Observation-centered EKF (OCEKF): Provides a closed-form, single-pass approximation to the mode found by iEKF, closely matching its accuracy in low-noise or highly nonlinear measurement settings (Kent et al., 2019, Kent et al., 2019).
• Factor Graph Optimization (FGO): Under a size-1 window, the recursive Gauss–Newton update of FGO is algebraically equivalent to iEKF under Gaussian/L2 loss and repeated local linearization, highlighting the deep connection between iterative filtering and batch optimization (Song et al., 31 Oct 2025).
• Dynamically Iterated Filters (DIF): Generalize iEKF by iterating both the measurement and dynamic models, leading to further improvement in mean-squared error and robustness to step-size/tuning parameters, especially in highly nonlinear dynamical systems (Kullberg et al., 2024).

6. Computational and Implementation Considerations

Each iEKF iteration incurs costs analogous to a standard EKF update (Jacobians, measurement innovation, and a matrix inversion of state or measurement dimension as appropriate) (Im, 2024). For high-dimensional or high-rate applications (e.g., dense LiDAR/IMU odometry), exploiting diagonal structure in measurement covariances and efficient matrix-inversion-lattice techniques (state-dimensional gain calculation) is crucial for real-time performance (Xu et al., 2020, Yu et al., 2024).

IterIEKF and iEKF exploit modern manifold-optimization constructs ($$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$2, tangent-space propagation) to ensure consistency and closed-form formulae for state-covariance correction on Lie groups (Huai et al., 2023, Goffin et al., 2024, Santana et al., 16 Apr 2026). Practical implementations stabilize iteration count (typically 2–6), use analytic Jacobians where feasible, include heuristic outlier rejection, and, in real-time vehicle applications, maintain per-scan execution <25 ms for $$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$3 states and $$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$4 residuals (Xu et al., 2020, Yu et al., 2024).

7. Impact, Limitations, and Future Directions

The iEKF has established itself as a standard, robust, and efficient solver for nonlinear Gaussian filtering in robotics, navigation, and sensor fusion. It bridges the gap between online recursive filtering and local nonlinear optimization, striking a favorable balance between computational tractability and estimator accuracy.

Limitations:

• The iEKF remains locally convergent; poor initializations or severe nonlinearity may require damping, line search, or more global optimization methods (Kullberg et al., 2024, Im, 2024).
• In time-correlated (multi-epoch) or non-Gaussian noise regimes, batch or sliding-window FGO methods provide quantitatively tighter approximations at greater computational cost (Song et al., 31 Oct 2025).

Ongoing research explores multi-loop or dynamically iterated extensions (DIF), robustified variants (RIEKF), and principled connections to full batch-graph inference, with adaptive noise tuning and damping to further enhance convergence and stability. Manifold and invariant formulations continue to generalize the theoretical guarantees, especially in high-dimensional legged robotics, multi-sensor fusion, and SLAM (Goffin et al., 2024, Ouyang et al., 2023).

---

Key references:

• Iterated Invariant EKF for quadruped odometry (Santana et al., 16 Apr 2026)
• Iterated Invariant Extended Kalman Filter (IterIEKF) (Goffin et al., 2024)
• FAST-LIO: iEKF for LiDAR-inertial fusion (Xu et al., 2020)
• I$$J(x) = \frac{1}{2}\|x - \hat{x}_k^-\|^2_{P_k^-} + \frac{1}{2}\|z_k - h(x)\|^2_{R_k}$$5EKF-LO: Dual-iteration LiDAR odometry (Yu et al., 2024)
• Clifford-algebra iEKF for INS/GNSS (Ouyang et al., 2023)
• Dynamically Iterated Filters (Kullberg et al., 2024)
• Factor Graph—iEKF connections (Song et al., 31 Oct 2025)
• Comprehensive iEKF reviews (Huai et al., 2023, Im, 2024)
• Astrodynamics/Observation-centered filtering (Kent et al., 2019, Kent et al., 2019)