---
title: Sliding-Window Factor Graph Optimization
url: https://www.emergentmind.com/topics/sliding-window-factor-graph-optimization
type: topic
---

# Sliding-Window Factor Graph Optimization

Sliding-window factor graph optimization (SW-FGO) is a state estimation and data association paradigm that formulates recent system states and measurements within a “window” as a factor graph, incrementally updates the graph as new data arrive, and marginalizes out old states using Schur complement to preserve sparsity and computational tractability. This technique provides bounded-memory nonlinear optimization, supporting real-time application in localization, tracking, and sensor fusion under challenging conditions where batch optimization is impractical and recursive filters are suboptimal.

## 1. Architecture and Problem Formulation

SW-FGO represents the unknown states—such as system poses, velocities, biases, or decision variables—over a fixed recent temporal horizon. At each timestep, states are added at the leading edge of the window, connected by sensor-derived constraint factors; on exceeding the maximum window length, the oldest states are marginalized, with their statistical influence retained in a condensed prior.

For example, in high-precision indoor positioning, the system state at each discrete instant $i$ comprises pose $T_i \in SE(3)$, velocity $v_i$, and IMU biases $b^a_i, b^g_i$, assembled over a window of length $W$ as
$$
X = \{T_{k-W+1}, v_{k-W+1}, b^a_{k-W+1}, b^g_{k-W+1}, \ldots, T_k, v_k, b^a_k, b^g_k\}
$$
with measurements encoded as factors imposing residual errors between predicted and observed sensor outputs [2503.12726].

A parallel structure arises in GNSS-inertial navigation, with receiver ECEF position $p_k$, velocity $v_k$, and clock bias $dt_k$ at epoch $k$:
$$
x_k = [p_k^T, v_k^T, c\,dt_k]^T
$$
and the active state set $X = \{x_{K-N+1}, \ldots, x_K\}$ over the window of $N$ epochs [2109.00683].

## 2. Factor Types and Sensor Fusion

SW-FGO is distinguished by the diversity and structure of factor types, each encoding a physical or logical observation model with compatible residual and covariance:

- **IMU pre-integration factors:** Capture continuous-time inertial motion constraints between successive states using pre-integrated IMU measurements corrected for bias and covariance, following the Forster et al. formulation. The residual combines rotational, velocity, and position terms, enabling trajectory smoothness and scale estimation [2503.12726, 2402.05540].
- **Range and ToA/TDoA factors:** Model geometric distance constraints from time-of-arrival sensors, e.g., UWB anchors or GNSS pseudoranges, typically as nonlinear scalar residuals with robust kernels (Huber/Cauchy) for outlier mitigation. In some frameworks, TDoA factors utilize anchor-pair range differences [2503.12726, 2109.00683].
- **Ultrasonic and elevation factors:** For constrained environments, scalar or vector factors encode physical ranges and vertical priors, often stacking elevation and planar range terms to exploit ultrasonic or altimetric readings [2503.12726].
- **Scan-to-scan and scan-to-map registration:** For range-inertial SLAM, GICP-type cost functions register rolling point cloud scans against prior maps or each other via distribution-to-distribution error, incorporating robust covariance handling and analytic Jacobians [2402.05540].
- **Windowed carrier-phase (WCP) and Doppler constraints:** In positioning, time-correlated carrier-phase factors connect multiple GNSS states to exploit inter-epoch consistency and enhance robustness in ambiguous or multipath-rich regimes. Null-space projection removes phase ambiguities [2109.00683].
- **Track association variables and exclusion constraints:** In tracking (e.g., SWTrack), hypothesis variables $z_h \in \{0,1\}$ represent association decisions for detected object paths through the frame window, with hard exclusion factors ensuring unique assignment per detection and lifted skip-edges addressing missed observations [2402.17892].

## 3. Sliding Window Operation and Marginalization

The sliding mechanism enables online operation with bounded computational resources. The process comprises:

- **Initialization:** Window is filled with initial state estimates using physical priors or coarse solutions (e.g., ultrasonic-enhanced localization, gravity-aligned poses in 3D maps) [2503.12726, 2402.05540].
- **State addition and factor insertion:** At each new timestep, a fresh state and all associated sensor factors are appended to the window’s trailing edge.
- **Marginalization:** When the window exceeds maximum length, the oldest node is removed. The statistical influence is encoded as a prior factor derived via the Schur complement on the linearized Hessian and gradient:
  $$
  r_{\text{prior}}(\Delta X) = H_m \Delta X - g_m
  $$
  where $H_m$ and $g_m$ are the marginalized Hessian and gradient [2503.12726, 2402.05540].
- **Factor pruning and densification:** Some frameworks perform hypothesis culling (top $M$-best), class/distance pruning, or robustification prior to marginalization for computational efficiency and stability [2402.17892].

This procedure ensures a constant-size optimization problem with information from marginalized states folded into a dense prior, preserving estimator consistency and reducing drift.

## 4. Optimization Algorithms and Computational Aspects

The SW-FGO backend solves the nonlinear least-squares objective:
$$
J(X) = \sum_{\text{IMU}} \|r_{IMU}\|_{\Sigma_I}^2 + \sum_{\text{range}} \rho(\|r_{range}\|_{\Sigma_{T}}^2) + \ldots + \|r_{\text{prior}}\|^2
$$
using manifold-aware Levenberg–Marquardt algorithms. The core steps are:

- **Residual linearization:** For each factor, compute the Jacobians with respect to connected states, using analytic expressions for high efficiency [2109.00683, 2402.05540].
- **Normal equation assembly:** Stack all information into sparse block-structured linear systems $(J^T W J + \lambda I)\Delta X = -J^T W r$, exploiting the limited window size and factor arity.
- **Update and retraction:** States are updated via manifold retractions (e.g., pose update via exponential map in $SE(3)$) and iterated until convergence per window step.
- **Marginalization as dense prior:** After convergence, the marginalized “prior” is stored and applied to the remaining states going forward.

Window sizes are chosen to balance information retention and computational load (e.g., $W=20$ poses for IMU at 200 Hz, or 5-second duration for lidar-inertial localization). Per-iteration cost is typically $O(mn)$; real-time operation is reported on embedded (Jetson Nano: 21 ms/LM iteration for indoor fusion) and desktop platforms [2503.12726, 2402.05540].

## 5. Adaptive Covariance and Robustness Strategies

SW-FGO excels at integrating time-varying sensor reliability:

- **Dynamic covariance estimation:** Sensor channel metrics (e.g., UWB CIR amplitude, rms-delay-spread) produce adaptive scaling of factor covariances, such as
  $$
  \Sigma_T^{(i,j)} = \sigma_0^2\exp(-\beta Q_{ij}) \text{ or } \sigma_{\min}^2 + (\sigma_{\max}^2-\sigma_{\min}^2)(1-\tanh{\gamma Q_{ij}})
  $$
  downgrading uncertain measurements in the optimizer [2503.12726].
- **Non-line-of-sight (NLOS) mitigation:** NLOS-detected UWB links are either given inflated covariance ($\Sigma_T \leftarrow \alpha \Sigma_T$ with $\alpha \gg 1$) or replaced by soft-penalty factors with low weight $w_{NLOS}$, suppressing their influence without discarding information [2503.12726].
- **Robust kernels:** All cost terms may be protected by robust loss functions (e.g., Huber, Cauchy) to resist outliers from multipath, degeneracy, or spurious correspondences [2503.12726, 2402.05540].
- **Hypothesis pruning and lifted edges:** In multi-object tracking, aggressive pruning and skip-edge insertion handle occlusion, misdetection, and reduce the data association search space [2402.17892].

These strategies maintain estimator stability under signal degradation, multi-path interference, and partial sensor outages, enabling sub-decimeter accuracy even at 40% packet reception rates in real deployments [2503.12726].

## 6. Performance Characteristics and Applications

SW-FGO has demonstrated substantial empirical improvements across challenging applications:

- **Indoor fusion positioning (IMU-UWB-ultrasonic):** Achieves 38% lower RMSE (12.3 cm) compared to EKF baselines, with robust performance in cluttered NLOS environments, and vertical drift suppression by a factor $\geq 6$ [2503.12726].
- **GNSS positioning in urban canyons:** Lane-level accuracy ($\sim$1.8–3.0 m) is obtained by leveraging windowed carrier-phase constraints, outperforming EKF-based TDCP fusion, and showing resilience on low-cost platforms [2109.00683].
- **3D multi-object tracking:** SWTrack delivers improved AMOTA and reduced FP/FN rates by jointly optimizing track assignment hypotheses in a window, outperforming frame-by-frame greedy trackers [2402.17892].
- **Range-inertial localization on 3D prior maps:** Tightly-coupled scan-to-scan and scan-to-map windowed optimization exhibits robust operation in severe point cloud degeneration and featureless regions, significantly exceeding filter-based approaches [2402.05540].

In all cases, the tightly-coupled, sliding-window design enables real-time estimation with high resilience to corruption, occlusion, or measurement outage.

## 7. Representative Frameworks and Implementation Notes

Several open or published frameworks implement SW-FGO in diverse modalities:

| System/Application      | State Vector          | Main Factor Types      | Marginalization Approach            |
|------------------------|-----------------------|-----------------------|-------------------------------------|
| IMU-UWB-Ultrasonic Fusion [2503.12726] | Pose, velocity, biases | IMU, UWB TDoA, ultrasonic, NLOS     | Dense prior (Hessian, gradient)     |
| GNSS Windowed Carrier-Phase [2109.00683] | ECEF pos., vel., clock | Pseudorange, Doppler, WCP           | Drop-oldest state, efficient update |
| 3D Multi-Object Tracking (SWTrack) [2402.17892] | Track hypotheses      | Cost unary, skip/lifted, exclusion  | Hypothesis culling, LP relaxation   |
| Range-Inertial on 3D Map [2402.05540] | Pose, velocity, biases | IMU, scan-to-scan, scan-to-map      | Bayes-tree (iSAM2)                  |

GTSAM-style nonlinear optimization on SE(3), robust prior marginalization, and GPU acceleration for point cloud registration are frequently used; window sizes are tailored by scenario and platform to guarantee both information retention and resource efficiency.

---

SW-FGO is now a mature methodology enabling real-time, robust state estimation in scenarios that defy classical filter-based and batch optimization approaches, with applications ranging from sensor fusion and positioning to multi-hypothesis tracking and SLAM, leveraging the structure and flexibility of factor graphs for bounded-memory, adaptively robust inference.

Source: https://www.emergentmind.com/topics/sliding-window-factor-graph-optimization