---
title: Tightly-Coupled Iterated EKF
url: https://www.emergentmind.com/topics/tightly-coupled-iterated-kalman-filter-iekf
type: topic
---

# Tightly-Coupled Iterated EKF

A tightly-coupled Iterated Extended Kalman Filter (iEKF) generalizes the conventional Extended Kalman Filter by applying multiple linearizations and Gauss–Newton-style updates within each measurement update, maintaining full cross-modal consistency between state-prediction and measurement models. In the tightly-coupled setting—as in direct visual-inertial, LiDAR-inertial, radar-visual-inertial, or multibody estimation applications—sensor streams are fused at the raw measurement or residual level, leveraging the filter’s iterated structure for maximal information transfer and robustness. This approach strengthens estimation performance in highly nonlinear and sensor-rich environments, with computational and algorithmic optimizations that enable real-time applications.

## 1. State and Process Model Structure

Tightly-coupled iEKF frameworks parameterize the state vector to reflect all minimal quantities essential for the target environment and estimation task. This includes kinematic, calibration, and geometric entities; sensor biases; and often map or feature elements. Example structures include:

- **Direct visual-inertial ego-motion (planar scene):**
  $$
  x = [\alpha\;\; \theta\;\; \mu_s\;\; g_s\;\; b_a\;\; b_\omega]
  $$
  where $\alpha=1/d$ (inverse distance to a local plane), $\theta=v/d$, $\mu_s$ (SO(3) parameterization of plane normal), $g_s$ (gravity direction), $b_a$, $b_\omega$ (IMU biases) [2001.05215].

- **LiDAR–IMU odometry:**
  $$
  x = [\;R_{I},\; p_{I},\; v_{I},\; b_\omega,\; b_a,\; g \;]
  $$
  $R_{I}\in SO(3)$, $p_{I},v_{I},g,b_\omega,b_a\in \mathbb{R}^3$ [2010.08196, 2603.15471].

- **Radar-visual-inertial odometry:**
  $$
  x = [\,T_{IB}^\top,\; v_{IB}^\top,\; q_G^\top,\; \text{calibration parameters},\; \text{feature set}\,]^\top
  $$
  $T_{IB}$ (IMU position), $v_{IB}$ (velocity), $q_G$ (attitude quaternion), extrinsics, per-feature bearings and depths [2603.23052].

- **Error-state representation:** compact increments in tangent spaces for manifold-valued components (e.g., SO(3), SE(2,3)), essential for tightly-coupled IMU integration and efficient linearization [2010.08196, 2603.15471, 2604.15449].

IMU-driven, process models implement continuous-time rigid-body dynamics, bias-random walks, and platform-specific kinematic equations, discretized (by Euler, midpoint, or Lie-group flows) to propagate the prior mean and covariance.

## 2. Raw Measurement Models and Coupling

In tightly-coupled designs, measurement models operate directly on sensor outputs, using geometric or physics-derived constraints:

- **Direct photometric feedback:** The pixel-residual vector, of the form $z = h(x) + \eta$, where $h(x)$ samples the warped image at reprojected coordinates under a homography defined by $x$ [2001.05215].
- **LiDAR scan-to-map or point-to-plane residual:** $d_i = \mathbf{n}_i^T[{}^G \mathbf{p}_i - \mathbf{q}_i]$, mapping deskewed LiDAR points to global frame, matching against local probabilistic planes in a VoxelMap [2010.08196, 2603.15471].
- **Radar Doppler and range:** Direct mapping from state and calibration to radar radial speed and range; used both as measurement updates and to initialize visual feature depths [2603.23052].
- **Proprioceptive/kinematic constraints:** Foot-velocity in quadruped odometry or multibody joint/torque constraints, encoded as invariant or pseudo-measurements [2604.15449, 2505.08193].

The key aspect of tight coupling is that measurement residuals “see” the entire fused state vector and are not staged through intermediate mapping/bundling (no two-step estimation, e.g., pose then landmarks), thereby directly inducing posterior cross-correlations in the filter.

## 3. Iterated Update: Gauss–Newton Linearization and Correction

Unlike the standard EKF’s single linearization per measurement epoch, the iEKF employs one or more inner iterations, refining both the measurement and (optionally) the process model linearization. The Kalman-type update at the $j$-th inner iteration (for prediction state $x_k^-$, measurement $z_k$, and Jacobians $H_j, F$) is:

$$
K_j = P^- H_j^T (H_j P^- H_j^T + R)^{-1}
$$
$$
\delta x_j = K_j \left[ z_k - h(x_j) - H_j(x^- - x_j) \right]
$$
$$
x_{j+1} = x_j + \delta x_j
$$

Convergence is declared if $\|\delta x_j\| < \epsilon$ or after a small fixed number of iterations (typically $3$–$5$) [2001.05215, 2406.06427, 2010.08196]. Posterior covariance is updated using the last computed Jacobian. When formulated in information-matrix notation or using backtracking/damping, numerical robustness is improved, especially in highly nonlinear regimes [2404.15359, 2302.13871].

- **Error-state (IESKF) variants:** operate on tangent-space deltas and manifold retractions, maintaining accuracy under large rotations and extended feature sets [2010.08196, 2603.15471].
- **Dynamic (DIEKF/“full Gauss–Newton”) variants:** relinearize both process and measurement models at every inner iteration, yielding systematically improved convergence and stability, especially when process models are strongly nonlinear [2404.15359, 2302.13871].

## 4. Algorithmic Workflow and Computational Aspects

The typical workflow of a tightly-coupled iEKF is as follows:

| Step                        | Operation                                                                 | Complexity         |
|-----------------------------|---------------------------------------------------------------------------|--------------------|
| Prediction                  | Propagate mean and covariance using process model                         | $O(n^3)$           |
| Deskew/Motion Compensation  | Reproject all raw measurements (e.g., LiDAR points, image pixels)         | $O(mn)$            |
| Iterated Update             | For each iteration, compute residuals, linearize, update state/covariance | $O(m n^2)+O(n^3)$  |

- $n$ is state dimension, $m$ is number of raw measurements (e.g., LiDAR features, image pixels).
- Efficient gain computation often employs the matrix-inverse lemma to confine matrix inversions to $n\times n$ size, avoiding inverting $m\times m$ matrices when $m\gg n$ [2010.08196].
- Real-time rates: e.g., in “FAST-LIO” $>1200$ LiDAR features fused in $<25$ ms per iEKF scan update; direct photometric visual-inertial iEKF achieves 30 Hz updates with state-of-the-art accuracy at $1.4$ ms per update [2001.05215, 2010.08196].
- For state spaces on matrix Lie groups (SO(3), SE(3), SE(2,3)), right-invariant error representations and retraction operations are crucial for consistency and convergence [2604.15449].

## 5. Robustness, Accuracy, and Benchmarks

Tightly-coupled iEKF architectures consistently deliver superior accuracy and robustness relative to non-iterated and loosely-coupled strategies:

- **Visual-inertial:** Altitude RMSE $2–6$ cm and velocity RMSE $1–7$ cm/s achieved in planar flight using direct photometric iEKF—substantially outperforming feature-based EKF (RMSE an order of magnitude higher and 5x computational cost) [2001.05215].
- **LiDAR-inertial:** Velocity drift $<0.3\%$ over long UAV flights; update time $<25$ ms (vs $>100$ ms with standard gain), robust operation under $>1200$ features per update [2010.08196].
- **Radar-visual-inertial:** Robust, low-drift odometry under visual deprivation/darkness or radar occlusion by fully fusing radar, vision, and IMU streams at the filter core [2603.23052].
- **Multibody MoCap:** Joint-angle RMSD $3.24^\circ$ (iEKF-IMU), 30–50% lower than OpenSense (loosely-coupled), and torque RMSD $2$–$4$ Nm depending on scenario [2505.08193].
- **Quadruped robot odometry:** Iterated invariant EKF delivers up to $60\%$ lower base velocity and attitude error than non-iterated or loosely-coupled alternatives, particularly salient under large initialization uncertainty or when using only proprioceptive constraints [2604.15449].

Empirically, the iterated structure also reduces NEES (normalized estimation error squared), implying improved consistency and well-calibrated uncertainty under strong nonlinearity and cross-sensor constraints.

## 6. Theoretical Properties, Variants, and Extensions

- **MAP and Gauss–Newton solution:** The iEKF update is equivalent to minimizing the joint negative log-posterior of prior and measurement, solved by stacked Gauss–Newton (normal) equations [2406.06427, 2404.15359, 2001.05215].
- **Dynamically iterated extensions (DIEKF):** By relinearizing both the process and measurement functions at every inner loop, the DIEKF becomes locally equivalent to a single-step iterated Rauch–Tung–Striebel smoother, yielding robustness to transition-model nonlinearities [2302.13871, 2404.15359].
- **Invariant formulations:** On $\mathrm{SE}_2(3)$ or similar Lie groups, iterated invariant EKF preserves autonomy and alignment between propagation and observation; the iterated update strictly corresponds to a MAP estimator on the group manifold, with linearization and retraction in the group structure [2604.15449].
- **Real-time practicality:** State dimensions in tightly-coupled iEKF designs typically remain $<30$; major computational savings arise from block-diagonal measurement covariances, sparse Jacobians, and algorithmic avoidance of large measurement-matrix inverses [2010.08196].
- **Convergence and damping:** Quadratic convergence is achievable near the true state; Levenberg–Marquardt or Armijo-type line search can be embedded for global robustness if severe nonlinearity or ill-conditioning occurs [2404.15359].

## 7. Applications and Impact

The tightly-coupled iEKF paradigm enables real-time, robust, and accurate estimation in a wide range of robotic, navigation, and motion-capture domains:

| Application Domain       | Sensors Fused                   | Filter Structure      | Noted Papers         |
|-------------------------|----------------------------------|----------------------|----------------------|
| Direct visual-inertial  | IMU + monocular camera           | 14-state iEKF        | [2001.05215]         |
| LiDAR-inertial odometry | IMU + LiDAR (edge/plane/voxel)   | iEKF/IESKF, n=18     | [2010.08196, 2603.15471] |
| Radar-visual-inertial   | IMU + radar + camera             | ROVIO-type iEKF      | [2603.23052]         |
| Quadruped odometry      | IMU + kinematics (feet)          | Iterated invariant EKF | [2604.15449]         |
| Multibody/Kinetics      | IMUs (+markers/torque opt.)      | IEKF with constraints | [2505.08193]         |

Tightly-coupled iEKF approaches are favored in settings with high sensor count, strong geometric constraints, and mission- or safety-critical consistency requirements. The design principles underlying these filters are established as a generalized best practice for next-generation state estimation across robotic perception, inertial navigation, and sensor fusion research [2001.05215, 2010.08196, 2603.23052, 2604.15449, 2505.08193, 2603.15471, 2406.06427, 2404.15359, 2302.13871].

Source: https://www.emergentmind.com/topics/tightly-coupled-iterated-kalman-filter-iekf