Tightly-Coupled Fusion

Tightly-coupled fusion operates on raw or minimally processed sensor measurements directly within a unified probabilistic model. Instead of fusing independent pose estimates, it combines fundamental data like:

• Pixel intensities or feature tracks from a camera
• Specific force and angular rate from an IMU
• Raw point cloud returns from a LiDAR This allows the estimator to model the full measurement geometry and noise characteristics at their source, preserving information that is lost in intermediate processing steps.

All sensor data contributes to a single, joint state estimation problem. A common framework is a Bayesian filter (e.g., Extended Kalman Filter, Error-State Kalman Filter, or Particle Filter) or a factor graph-based optimization. The process involves:

• A centralized state vector containing all estimated variables (pose, velocity, sensor biases, map features).
• A unified process model predicting state evolution.
• Individual measurement models for each raw sensor type, which are linearized and used to update the entire state. This holistic update provides optimal statistical weighting of all available information.

By modeling low-level sensor interactions, tightly-coupled fusion can achieve superior accuracy and improve system observability. Key mechanisms include:

• Direct coupling of complementary sensors: An IMU's high-frequency motion prediction is directly corrected by visual feature reprojection errors, constraining drift in unobservable directions.
• Exploitation of weak cues: It can utilize partial or ambiguous measurements (e.g., a single visual feature, a weak GPS signal) that would be rejected by a higher-level tracker.
• Estimation of latent parameters: It can jointly estimate and correct for sensor biases (e.g., IMU bias) and calibration parameters online, as these are directly observable in the raw data relationship.

The primary trade-off for higher accuracy is significantly increased computational cost. Complexity arises from:

• High-dimensional state space: The state must include parameters for sensor biases and often environmental landmarks (e.g., 3D points), leading to large covariance matrices.
• Nonlinear measurement models: Models for cameras and LiDARs are highly nonlinear, requiring iterative linearization (e.g., in an EKF) or nonlinear optimization (e.g., in factor graphs).
• Data association burden: The system must solve the per-measurement data association problem—determining which landmark in the state each raw measurement corresponds to—which is computationally intensive and critical for robustness.

Performance is critically dependent on precise sensor calibration and exact time synchronization. Requirements are stricter than in loosely-coupled approaches:

• Temporal synchronization: Errors of a few milliseconds in timestamp alignment between, for example, an IMU and a camera can cause significant state estimation errors due to the high dynamics involved.
• Spatio-temporal calibration: Accurate knowledge of the extrinsic transformation (rigid body transformation) between sensor frames and their time offsets must be known and often refined online.
• Intrinsic calibration: Camera lens distortion or LiDAR beam model parameters must be accurate, as they are used directly in the measurement models.

Tightly-coupled fusion is the foundation for many high-performance robotic perception systems. Prominent examples include:

• Visual-Inertial Odometry (VIO): Algorithms like MSCKF, OKVIS, and VINS-Mono tightly fuse IMU readings with raw visual features for robust drone and AR/VR tracking.
• LiDAR-Inertial Odometry (LIO): Systems like LIO-SAM and FAST-LIO directly register raw LiDAR points to a local map while propagating motion with an IMU.
• GPS-INS Tight Integration: Using raw GPS pseudorange and carrier-phase measurements within the INS filter, rather than a standalone GPS position solution.
• Factor Graph SLAM: Modern graph-based optimizers (e.g., GTSAM, g2o) inherently implement tightly-coupled fusion by adding factors for each raw sensor measurement.

A powerful side-benefit of tightly-coupled architectures is the ability to perform online sensor calibration within the state estimation framework. Instead of being a separate, offline procedure, calibration parameters are added to the state vector and estimated concurrently. Commonly estimated parameters include:

• IMU intrinsics: Accelerometer and gyroscope biases, scale factors, and non-orthogonalities.
• Spatio-temporal calibration: The precise 3D transformation (extrinsics) and time offset (temporal calibration) between the IMU and camera/LiDAR.
• Camera distortion parameters. By treating calibration as part of the state, the system automatically adjusts to sensor changes over time (e.g., due to temperature or mechanical stress), maintaining optimal fusion performance without manual intervention.

Loosely-coupled fusion is a sensor fusion architecture where each sensor or modality first processes its data into an independent, high-level state estimate (e.g., a pose from a visual odometry module, a position from a GPS receiver). These independent estimates are then fused, typically using a filter like a Kalman filter. This approach offers modularity and simpler debugging but can suffer from information loss and suboptimal accuracy compared to tightly-coupled methods, as early processing discards raw data correlations and uncertainties.

• Example: Fusing the output of a visual SLAM system (providing 6-DOF pose) with GPS coordinates.
• Contrast: Unlike tightly-coupled fusion, it does not fuse raw pixel intensities with raw IMU readings in a single optimization.

A factor graph is a bipartite graphical model that represents the factorization of a complex probability distribution. It is the dominant optimization backend for modern tightly-coupled fusion systems, especially in visual-inertial odometry (VIO) and SLAM. The graph consists of variable nodes (unknown states like robot poses, landmark positions) and factor nodes (probabilistic constraints derived from sensor measurements and motion models).

• Function: Provides a sparse structure to efficiently solve large-scale Maximum A Posteriori (MAP) estimation problems via nonlinear least-squares optimization (e.g., using Gauss-Newton or Levenberg-Marquardt).
• Advantage: Enables the tight coupling of heterogeneous measurements (e.g., a visual reprojection factor and an IMU pre-integration factor) within a unified probabilistic framework.

Visual-Inertial Odometry (VIO) is a premier application of tightly-coupled fusion. It estimates a robot's 3D pose and velocity by fusing raw visual features (from a camera) with raw inertial measurements (from an IMU) in a single estimator. The IMU provides high-frequency motion data and gravity alignment, while the camera provides absolute scale and drift correction.

• Tight Coupling: In a tightly-coupled VIO system, the IMU data is pre-integrated between camera frames to create a motion constraint, which is added alongside visual reprojection constraints in a factor graph or filter.
• Key Benefit: Robustness during periods of poor visual information (e.g., motion blur, low texture) because the IMU model propagates the state estimate.

The Error State Kalman Filter (ESKF) is a sophisticated implementation of the Kalman filter principle particularly well-suited for tightly-coupled inertial fusion. Instead of estimating the full system state (e.g., position, quaternion orientation), it estimates the error state—the small deviation between a nominal state (propagated by the IMU kinematics) and the true state.

• Mechanism: The IMU drives the nominal state. The filter estimates the error state (which operates in a locally linear space), corrects the nominal state, and then resets the error state to zero.
• Advantages: Offers superior numerical stability for attitude estimation (avoiding singularities associated with direct quaternion filtering) and more naturally handles the nonlinearities of 3D rotation.

Preintegration is a critical technique for efficient tightly-coupled visual-inertial or LiDAR-inertial fusion. Instead of integrating IMU measurements at the filter's high frequency (e.g., 100-1000 Hz) and relating them to low-frequency camera frames (e.g., 10-60 Hz), IMU measurements between two camera/keyframe times are preintegrated into a single relative motion constraint.

• Process: Accelerometer and gyroscope readings are integrated in a manifold-aware way to produce a delta position, velocity, and rotation (and their covariance) between two time instances.
• Benefit: This creates a single, compact inertial factor between camera frames, dramatically reducing computational load in graph-based optimizers and enabling real-time performance.

The measurement model (or observation model) is a mathematical function that describes how sensor data is generated from the true state of the system. In tightly-coupled fusion, the measurement model operates on raw or low-level sensor data. It is a core component that defines the "tightness" of the coupling.

• Examples:
  • Visual Model: Projects a 3D landmark point into the 2D image plane, modeling camera intrinsics and lens distortion.
  • IMU Model: Relates specific force and angular rate measurements to the robot's linear acceleration and rotational velocity in the world frame.
• Role: The residual (difference between the predicted measurement and the actual measurement) is minimized during state estimation. Accurate modeling of noise characteristics (covariance) here is essential for optimal fusion.