---
title: Factor Graph-Based Sensor Fusion
url: https://www.emergentmind.com/topics/factor-graph-based-sensor-fusion
type: topic
---

# Factor Graph-Based Sensor Fusion

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$ [2308.04847][2209.02207]. 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 [2409.12426][2308.04847].

## 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$             | [2209.02207]     |
| GNSS (pseudorange)        | Global position factor, clock-bias parameter             | [2511.23017]     |
| GNSS (carrier phase/DD)   | Double-difference phase factors, local increments        | [2209.14649]     |
| LiDAR                     | ICP registration factors (scan-to-scan, map-to-submap)   | [2209.02207]     |
| Camera (visual SLAM)      | Reprojection factors, image-based constraints            | [2106.06783]     |
| UWB/Ultrasonic            | TDOA/range factors, dynamic covariance for NLOS          | [2503.12726]     |
| Odometry/Encoders         | Kinematic preintegration factors                         | [2308.04847]     |
| Magnetometer              | Vector/scalar magnetic field factors, calibration states | [2312.14343]     |

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 [2309.11134][2511.23017][2504.06479].

## 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 [2209.02207]. 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 [2308.04847][2209.02207][2402.14308].

Marginalization via the Schur complement preserves information from pruned states as new ones arrive, critical for bounded-memory operation in real-time mobile systems [2511.23017][2503.12726][2310.14163]. High performance can be further achieved via hardware acceleration (e.g., FPGA factor-graph accelerators) [2209.02207].

## 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 [2511.23017][2503.12726].
- **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 [2503.12726][2409.12426].
- **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 [2510.00524].

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 [2309.11134].
- **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 [2308.04847][2409.12426][2504.06479].
- **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 [2504.06479].

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 [2308.04847][2511.23017][2503.12726][2203.01389]
- Robustness to outliers and sensor outages with minimal trajectory discontinuities [2209.14649][2402.14308][2310.14163]
- Computational feasibility for embedded/edge devices via sliding-window solvers and efficient backend implementations [2308.04847][2209.02207]
- Superior latency, consistency, and modularity compared to extended Kalman/filtering approaches [2308.04847][2203.01389][2409.12426]

Factor graphs have also been extended to multi-robot decentralized estimation, multitarget tracking with data association uncertainty, and leak localization in distributed infrastructures [2209.08401][2111.13589][2509.10982].

## 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 [2309.11134]
- Automated and adaptive noise model estimation, either via online EM or reinforcement learning agents for robust dynamic weighting [2409.12426][2106.06783]
- Hardware acceleration and dedicated compute architectures for real-time onboard batch or incremental optimization [2209.02207]
- Seamless fusion of “soft” (non-Gaussian, multimodal) likelihoods and sensor reporting in distributed and federated settings [2209.08401][2111.13589]

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.

Source: https://www.emergentmind.com/topics/factor-graph-based-sensor-fusion