Tightly-Coupled Iterated EKF

• Tightly-coupled iEKF is a state estimation method that iteratively refines estimates through multiple Gauss–Newton linearizations within each measurement update.
• It fuses raw sensor data from modalities like vision, LiDAR, and radar at the measurement level to maintain cross-modal consistency and enhance robustness.
• Optimized for real-time applications, this approach reduces estimation errors and computational load, making it ideal for complex robotic and navigation tasks.

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) (Zhong et al., 2020).

• 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$ (Xu et al., 2020, Zhan, 16 Mar 2026).

• Radar-visual-inertial odometry:

$\alpha=1/d$0

$\alpha=1/d$1 (IMU position), $\alpha=1/d$2 (velocity), $\alpha=1/d$3 (attitude quaternion), extrinsics, per-feature bearings and depths (Nissov et al., 24 Mar 2026).

• 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 (Xu et al., 2020, Zhan, 16 Mar 2026, Santana et al., 16 Apr 2026).

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 $\alpha=1/d$4, where $\alpha=1/d$5 samples the warped image at reprojected coordinates under a homography defined by $\alpha=1/d$6 (Zhong et al., 2020).
• LiDAR scan-to-map or point-to-plane residual: $\alpha=1/d$7, mapping deskewed LiDAR points to global frame, matching against local probabilistic planes in a VoxelMap (Xu et al., 2020, Zhan, 16 Mar 2026).
• 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 (Nissov et al., 24 Mar 2026).
• Proprioceptive/kinematic constraints: Foot-velocity in quadruped odometry or multibody joint/torque constraints, encoded as invariant or pseudo-measurements (Santana et al., 16 Apr 2026, Osman et al., 13 May 2025).

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 $\alpha=1/d$8-th inner iteration (for prediction state $\alpha=1/d$9, measurement $\theta=v/d$0, and Jacobians $\theta=v/d$1) is:

$\theta=v/d$2

$\theta=v/d$3

$\theta=v/d$4

Convergence is declared if $\theta=v/d$5 or after a small fixed number of iterations (typically $\theta=v/d$6–$\theta=v/d$7) (Zhong et al., 2020, Im, 2024, Xu et al., 2020). 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 (Kullberg et al., 2024, Kullberg et al., 2023).

• Error-state (IESKF) variants: operate on tangent-space deltas and manifold retractions, maintaining accuracy under large rotations and extended feature sets (Xu et al., 2020, Zhan, 16 Mar 2026).
• 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 (Kullberg et al., 2024, Kullberg et al., 2023).

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	$\theta=v/d$8
Deskew/Motion Compensation	Reproject all raw measurements (e.g., LiDAR points, image pixels)	$\theta=v/d$9
Iterated Update	For each iteration, compute residuals, linearize, update state/covariance	$\mu_s$0

• $\mu_s$1 is state dimension, $\mu_s$2 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 $\mu_s$3 size, avoiding inverting $\mu_s$4 matrices when $\mu_s$5 (Xu et al., 2020).
• Real-time rates: e.g., in “FAST-LIO” $\mu_s$6 LiDAR features fused in $\mu_s$7 ms per iEKF scan update; direct photometric visual-inertial iEKF achieves 30 Hz updates with state-of-the-art accuracy at $\mu_s$8 ms per update (Zhong et al., 2020, Xu et al., 2020).
• 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 (Santana et al., 16 Apr 2026).

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 $\mu_s$9 cm and velocity RMSE $g_s$0 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) (Zhong et al., 2020).
• LiDAR-inertial: Velocity drift $g_s$1 over long UAV flights; update time $g_s$2 ms (vs $g_s$3 ms with standard gain), robust operation under $g_s$4 features per update (Xu et al., 2020).
• 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 (Nissov et al., 24 Mar 2026).
• Multibody MoCap: Joint-angle RMSD $g_s$5 (iEKF-IMU), 30–50% lower than OpenSense (loosely-coupled), and torque RMSD $g_s$6–$g_s$7 Nm depending on scenario (Osman et al., 13 May 2025).
• Quadruped robot odometry: Iterated invariant EKF delivers up to $g_s$8 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 (Santana et al., 16 Apr 2026).

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 (Im, 2024, Kullberg et al., 2024, Zhong et al., 2020).
• 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 (Kullberg et al., 2023, Kullberg et al., 2024).
• Invariant formulations: On $g_s$9 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 (Santana et al., 16 Apr 2026).
• Real-time practicality: State dimensions in tightly-coupled iEKF designs typically remain $b_a$0; major computational savings arise from block-diagonal measurement covariances, sparse Jacobians, and algorithmic avoidance of large measurement-matrix inverses (Xu et al., 2020).
• 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 (Kullberg et al., 2024).

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	(Zhong et al., 2020)
LiDAR-inertial odometry	IMU + LiDAR (edge/plane/voxel)	iEKF/IESKF, n=18	(Xu et al., 2020, Zhan, 16 Mar 2026)
Radar-visual-inertial	IMU + radar + camera	ROVIO-type iEKF	(Nissov et al., 24 Mar 2026)
Quadruped odometry	IMU + kinematics (feet)	Iterated invariant EKF	(Santana et al., 16 Apr 2026)
Multibody/Kinetics	IMUs (+markers/torque opt.)	IEKF with constraints	(Osman et al., 13 May 2025)

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 (Zhong et al., 2020, Xu et al., 2020, Nissov et al., 24 Mar 2026, Santana et al., 16 Apr 2026, Osman et al., 13 May 2025, Zhan, 16 Mar 2026, Im, 2024, Kullberg et al., 2024, Kullberg et al., 2023).