Factor Graph-Based Sensor Fusion

• Factor graph-based sensor fusion is a probabilistic framework that represents sensor and state relationships using modular factorization of joint probability distributions.
• It employs MAP inference through nonlinear least squares to integrate diverse sensor modalities like GNSS, LiDAR, and IMU under Gaussian or robust loss models.
• The approach supports real-time, plug-and-play integration with incremental optimization and outlier mitigation for robust performance in mobile robotics and navigation.

A factor graph is a bipartite probabilistic graphical model that represents the factorization of a joint probability distribution over a set of latent states and sensor measurements. In factor graph-based sensor fusion, each unknown system variable—such as pose, velocity, or bias—is encoded by a variable node, while each sensor measurement, process model, or prior knowledge is encoded by a factor node that connects to relevant variables. The resulting MAP (maximum a posteriori) inference problem is solved by minimizing the joint negative log-likelihood across all factors, typically assuming Gaussian or robustified residual models. This paradigm enables seamless integration of multiple, asynchronous, and heterogeneous sensor modalities for robust and consistent state estimation across a diverse range of mobile robotics, navigation, and infrastructure monitoring tasks.

1. Core Principles of Factor Graph-Based Sensor Fusion

At the heart of the factor graph approach is the decomposition of the fused joint posterior $p(X|Z)$ into products of local factor potentials:

$p(X|Z) \propto \prod_i \phi_i(X_i; z_i)$

where $X = \{x_1,\ldots,x_n\}$ are latent state variables (e.g., poses, velocities, IMU biases), $Z = \{z_1,\ldots,z_m\}$ are noisy measurements, and each factor $\phi_i$ is a function of the subset of variables $X_i$ connected to measurement $z_i$ (Dahal et al., 2023, Hao et al., 2022). Under Gaussian noise, taking the negative log yields a nonlinear least squares objective:

$$X^* = \arg\min_X \sum_{i} \|h_i(X_i) - z_i\|^2_{\Sigma_i^{-1}}$$

where $h_i(\cdot)$ is the measurement function and $\Sigma_i$ is the factor covariance.

By representing each measurement or process constraint as a modular factor, new sensing modalities or auxiliary information (e.g., wheel odometry, GNSS, LiDAR, UWB) can be incorporated by formulating new residual functions $r_i(\cdot)$ and attaching them to the appropriate variable nodes. This architecture enables true “plug-and-play” sensor fusion, supports asynchronous measurements, and provides batch or incremental optimization for high-rate real-time applications (Liu et al., 2024, Dahal et al., 2023).

2. Factor Types and Sensor Models

The factor-graph sensor fusion paradigm supports tight integration of a wide variety of sensor modalities through the definition of specialized factor residuals. Representative models include:

Sensor/Modality	Factor Type / Model	Reference
IMU	Preintegration on $SO(3)\times\mathbb{R}^3$	(Hao et al., 2022)
GNSS (pseudorange)	Global position factor, clock-bias parameter	(Ahmadi et al., 28 Nov 2025)
GNSS (carrier phase/DD)	Double-difference phase factors, local increments	(Beuchert et al., 2022)
LiDAR	ICP registration factors (scan-to-scan, map-to-submap)	(Hao et al., 2022)
Camera (visual SLAM)	Reprojection factors, image-based constraints	(Jia et al., 2021)
UWB/Ultrasonic	TDOA/range factors, dynamic covariance for NLOS	(Zhang et al., 17 Mar 2025)
Odometry/Encoders	Kinematic preintegration factors	(Dahal et al., 2023)
Magnetometer	Vector/scalar magnetic field factors, calibration states	(Lathrop et al., 2023)

For each modality, residuals are formulated in minimal or overparameterized coordinates (e.g., SE(3) on poses, quaternion for orientation), and are typically whitened by sensor noise or online-estimated uncertainty. The flexibility of the factor-graph formalism allows tight/tight coupling (raw GNSS observations rather than navigation fixes), robust fusion (outlier rejection layers), and explicit handling of asynchronicity and multifrequency scheduling across sensors (Zhang et al., 2023, Ahmadi et al., 28 Nov 2025, Nubert et al., 8 Apr 2025).

3. Solvers and Incremental/Sliding-Window Optimization

Minimization over the fused factor graph is achieved by nonlinear least-squares algorithms such as Gauss-Newton or Levenberg–Marquardt, exploiting block sparsity induced by the Markov structure (Hao et al., 2022). Two key solver families are prevalent:

• Batch Solvers: All variables and factors are updated in large nonlinear least-squares problems (QR, Cholesky, or Schur-based), suitable for offline or small-scale problems.
• Incremental/Sliding-Window Solvers: Algorithms such as iSAM2 perform variable relinearization and marginalization incrementally and only update the affected subgraph or a receding window. This yields real-time performance for window sizes up to hundreds of variables/factors per pass (Dahal et al., 2023, Hao et al., 2022, Yin et al., 2024).

Marginalization via the Schur complement preserves information from pruned states as new ones arrive, critical for bounded-memory operation in real-time mobile systems (Ahmadi et al., 28 Nov 2025, Zhang et al., 17 Mar 2025, Song et al., 2023). High performance can be further achieved via hardware acceleration (e.g., FPGA factor-graph accelerators) (Hao et al., 2022).

4. Robustness: Outlier Mitigation and Adaptive Losses

Sensor fusion in adversarial environments demands robustness to outlier measurements, multimodal/misaligned sensors, and changing uncertainty. Contemporary research implements:

• Robust Loss Functions: Adaptive M-estimators such as Barron loss, Huber, or Cauchy are used to suppress the influence of heavy-tailed errors in GNSS, LiDAR, and vision-based factors. Weighting schemes (e.g., Barron loss shape parameter $\alpha$) are tuned via grid search or even self-tuned online for maximum resilience in signal-compromised conditions (Ahmadi et al., 28 Nov 2025, Zhang et al., 17 Mar 2025).
• Dynamic Covariance Estimation: Covariances for UWB, GNSS, ultrasonic, or radar are adjusted in real time based on data quality, such as C/N0, CIR features, or NLOS detection (Zhang et al., 17 Mar 2025, Liu et al., 2024).
• Outlier Detection Frontends: Algorithms may use multi-stage outlier rejection, such as cross-validating GNSS pseudorange measurements with Doppler and INS/odo-predicted increments before accepting as factors (Song et al., 1 Oct 2025).

These approaches yield marked improvements over EKF baselines, especially under GNSS multipath, NLOS, or rapid sensor-loss scenarios.

5. Asynchronous and Modular Sensor Integration

A hallmark of the factor-graph fusion paradigm is its ability to handle arbitrary asynchrony and heterogeneity among sensor streams:

• Time-Centric Graph Construction: State nodes are instantiated at deterministic time intervals, and asynchronous measurements are attached via interpolation or GP-based continuous-time trajectory querying (Zhang et al., 2023).
• Plug-and-Play Modularity: Each sensor provides a factor via a template residual and covariance, transformed to a common reference frame as needed. New sensors can be “onboarded” without architectural redesign, and temporarily failed sensors are seamlessly deactivated by omitting their factors (Dahal et al., 2023, Liu et al., 2024, Nubert et al., 8 Apr 2025).
• Multiple Reference Frames and Online Calibration: Frameworks like Holistic Fusion treat frame transforms and extrinsic calibrations as dynamic random-walk state variables, so all absolute, local, and landmark measurements in arbitrary frames are directly fused (Nubert et al., 8 Apr 2025).

Such strategies underpin robustness to sensor loss, reference-frame drift, and the need to accommodate evolving measurement graphs in the field.

6. Practical Impact and Benchmark Results

Real-world applications across autonomous ground/underwater vehicles, mobile manipulators, infrastructure monitoring, and heavy machinery have demonstrated that factor-graph fusion delivers:

• Centimeter- to decimeter-level global accuracy in urban and GNSS-denied environments (Dahal et al., 2023, Ahmadi et al., 28 Nov 2025, Zhang et al., 17 Mar 2025, Nubert et al., 2022)
• Robustness to outliers and sensor outages with minimal trajectory discontinuities (Beuchert et al., 2022, Yin et al., 2024, Song et al., 2023)
• Computational feasibility for embedded/edge devices via sliding-window solvers and efficient backend implementations (Dahal et al., 2023, Hao et al., 2022)
• Superior latency, consistency, and modularity compared to extended Kalman/filtering approaches (Dahal et al., 2023, Nubert et al., 2022, Liu et al., 2024)

Factor graphs have also been extended to multi-robot decentralized estimation, multitarget tracking with data association uncertainty, and leak localization in distributed infrastructures (Dagan et al., 2022, Gaglione et al., 2021, Irofti et al., 13 Sep 2025).

7. Extensions and Future Directions

Open problems and active research threads in factor graph-based sensor fusion include:

• Development of continuous-time graph representations with GP priors for high-rate, truly asynchronous, and sparse sensor fusion (Zhang et al., 2023)
• Automated and adaptive noise model estimation, either via online EM or reinforcement learning agents for robust dynamic weighting (Liu et al., 2024, Jia et al., 2021)
• Hardware acceleration and dedicated compute architectures for real-time onboard batch or incremental optimization (Hao et al., 2022)
• Seamless fusion of “soft” (non-Gaussian, multimodal) likelihoods and sensor reporting in distributed and federated settings (Dagan et al., 2022, Gaglione et al., 2021)

Factor graph optimization thus provides a mathematically rigorous, highly modular, and extensible foundation that is rapidly superseding purely filter-based schemes for principled, robust, and real-time multi-sensor fusion in complex robotic and estimation tasks.