Degeneration of Sliding-Window Factor Graph Optimization into Iterated Extended Kalman Filtering

AI summary

Overview

Research area: State estimation for robotics and wireless navigation, specifically the theoretical relationship between sliding-window factor graph optimization (SW-FGO) and iterated extended Kalman filtering (IEKF), validated on time-of-arrival (TOA) simulations and urban GNSS/INS tightly coupled fusion.

Technical level: Advanced. The paper is written in the language of maximum a posteriori (MAP) estimation, Schur complement marginalization, QR decomposition, and information-form covariance updates. It is not a tutorial.

One-sentence scope: The paper defines a conceptual framework called Re-FGO and shows that, under four explicit structural conditions, an undirected sliding-window factor graph degenerates exactly into the IEKF recursion.

What This Paper Is About

The navigation community has debated for years whether factor graph optimization is fundamentally better than Kalman filtering, or whether the two are the same estimator under different bookkeeping. The paper's goal is to remove that ambiguity by giving the precise conditions under which a sliding-window factor graph becomes numerically identical to an iterated extended Kalman filter. Rather than proposing a new estimator, the authors build a conceptual bridge, Re-FGO, and then verify the equivalence in both simulation and real-world GNSS/INS data.

Key Contributions

1. A topological mapping between directed probabilistic graphical models and undirected factor graphs. The authors recast the conventionally one-directional Kalman filtering pipeline inside a graph optimization structure, so that the directed information flow of filtering can be expressed with factor graph language.

2. A two-stage marginalization pipeline. Stage 1 is sequential state elimination by Schur complement, which reproduces the prediction step and keeps the window at exactly one state; Stage 2 is factor-only anchoring by QR decomposition, which locks the linearization point of the measurement model at the converged posterior mode instead of removing the state node.

3. The Re-FGO conceptual perspective and a degeneration proposition. The paper states four conditions (Markov chain topology, single-state window w = 1, iterative Gauss-Newton equivalence, and a fixed linearization anchor) under which undirected factor graph optimization exactly replicates the state and covariance updates of the closed-form IEKF, and it maps the correspondence step by step in a five-step table.

4. Simulation and real-world validation with sensitivity tests. The authors confirm the equivalence on four synthetic TOA datasets and on Hong Kong urban GNSS/INS data, and they analyze how omitting the second marginalization affects performance in nonlinear and non-Gaussian cases.

Main Findings

• Exact numerical equivalence, not approximation. Across the four simulation schemes (L+G, NL+G, L+NG, NL+NG), the mean trajectory difference between IEKF and Re-FGO ranged from 8.88e-15 m to 1.19e-13 m, while EKF and the single-linearization variant Re-FGO₁ differed by 1.36e-12 m to 2.24e-11 m. The authors describe these discrepancies as machine-precision level, on the order of 10^-15 to 10^-11 m.

• The equivalence also holds in real urban GNSS/INS data. In the Open, Medium, Deep, and Harsh urban scenarios, the mean 3D trajectory difference norm between IEKF and Re-FGO was 3.59e-9 m, 3.86e-9 m, 4.57e-9 m, and 4.77e-9 m respectively. For EKF versus Re-FGO₁ the values were 2.87e-9 m, 2.35e-9 m, 3.12e-9 m, and 3.13e-9 m.

• A common optimization landscape. The residual norm descent paths at the first epoch of the NL+NG test show identical residual decay patterns and Jacobian directions for IEKF and Re-FGO, indicating they traverse the same optimization landscape rather than merely landing at similar answers.

• The second marginalization matters most under heavy nonlinearity and non-Gaussian noise. Comparing Re-FGO against a variant without second marginalization, the CP95 positioning error absolute difference percentage was 0.47% (L+G), 0.59% (NL+G), 0.97% (L+NG), and 1.64% (NL+NG). Concrete CP95 values were 0.1717, 0.1689, 9.8168 and 9.0481 m for Re-FGO, versus 0.1725, 0.1679, 9.9116 and 8.9000 m for the variant without the second marginalization.

• Iteration alone is not enough for recursive stability. The paper argues that iterative refinement improves estimation within a single epoch but that long-term stability depends on anchoring the Jacobian, because a "floating" linearization point that can be changed by iteration decouples the information gain from the nonlinear manifold and causes cumulative geometric distortion in the Markov chain.

• A clear distinction from MSCKF. The Multi-State Constraint Kalman Filter also mixes filtering and optimization, but because it delays marginalization across a buffer of past poses it does not enforce the strict single-state Markov property within each epoch, so it is not a direct bridge between standard SW-FGO and the EKF.

• Simple window-size-one FGO is non-trivial. The paper explains two structural barriers: the initialization of SW-FGO differs from the prediction-supplied initialization of EKF, and the standard Schur complement marginalization requires at least two variables in the graph, so a naive single marginalization per epoch leaves a structural gap.

Methodology in Plain English

The authors begin from the shared origin of both estimators, maximum a posteriori estimation, which minimizes the sum of squared Mahalanobis distances of motion and measurement residuals. From there, instead of trying to force a sliding-window graph to behave like a filter by simply setting the window size to one, they split each epoch into two graph operations.

In the first operation, they bring in the new state and the motion factor, then immediately eliminate the older state using the Schur complement. This collapses the historical information into a dense predictive prior on the surviving state, which mirrors the prediction step and keeps the active window at exactly one state. In the second operation, once Gauss-Newton iterations have converged to the posterior mode, they apply a thin QR factorization that does not remove the state node but instead locks the measurement Jacobian at that converged mode. This mirrors the update step and prevents the measurement linearization from drifting in later epochs.

They then state the four conditions under which this Re-FGO pipeline is the IEKF, and verify the claim empirically. The simulation uses a receiver in uniform circular motion with TOA measurements, under four schemes that cross low and high nonlinearity with Gaussian and non-Gaussian noise, using a Gaussian mixture model of 0.8·N(0, 0.1) m plus 0.2·N(0, 10) m, with emitter anchor radii of 1000 m or 105 m, over 100 epochs. The real-world test uses UrbanNav data from Hong Kong urban environments, with a u-blox F9P receiver logging GPS L1 raw measurements at 1 Hz and an Xsens MTi-10 IMU providing data at 400 Hz, synchronized through ROS, and benchmarked against a NovAtel SPAN-CPT ground truth from Inertial Explorer 8.9 forward-backward smoothing. All experiments ran in MATLAB 2024b on an Intel NUC (i7-1260P, 16 GB RAM).

Why This Matters

The paper gives the community a precise answer to a question that has mostly been handled informally: it clarifies that apparent performance gaps between graph optimization and filtering cannot be attributed to the paradigms themselves unless parameters such as window size and iteration count are explicitly separated from the optimization strategy.

Real-world applications:

• Smartphone and vehicular urban positioning, where GNSS outliers from multipath and non-line-of-sight reception dominate, and where the Google Smartphone Decimeter Challenge has seen FGO-based entries win the 2021 and 2022 editions while a refined EKF smoothing framework outperformed complex models in the 2023 edition.
• GNSS/INS tightly coupled navigation in dense cities, the setting used here, where signal blockages and severe multipath make the linearization point of the measurement model unreliable.
• Indoor and wireless localization built on Wi-Fi, Bluetooth, ultra-wideband, and 5G measurements, which are frequently corrupted by non-line-of-sight and multipath effects and often violate the unbiased Gaussian assumption.
• Location-based services such as precision agriculture, intelligent traffic, and safety and rescue, all cited in the paper as downstream users of these estimation techniques.

Industry relevance: practitioners who build graph-based navigation stacks can use these conditions to reason about when a graph formulation offers nothing beyond a well-tuned iterative filter, and when the extra machinery of a sliding window genuinely adds value. The result also suggests that the two-stage marginalization protocol, rather than the graph representation alone, is what preserves structural consistency.

Future Directions

• Extending the degeneracy analysis beyond the single-state window. The paper restricts Re-FGO to w = 1; how the equivalence degrades for larger windows and how the second marginalization interacts with delayed marginalization schemes such as MSCKF is left open.
• Quantifying computational cost. The experimental setup is reported (MATLAB 2024b, Intel NUC i7-1260P, 16 GB RAM), but the truncated content does not report runtime or complexity comparisons between Re-FGO, IEKF, and SW-FGO.
• Robust and non-Gaussian measurement handling. The L+NG and NL+NG results show the largest sensitivity to the missing second marginalization, raising the question of how robust kernels should be combined with fixed linearization anchors.
• Wider validation across sensors and platforms. The current validation covers TOA simulation and urban GNSS/INS; extending it to other wireless modalities, and to applications where the Markov assumption is only approximately satisfied, is a natural next step.

Target Audience

Researchers and graduate students in robotics, estimation theory, and wireless navigation who already understand Kalman filtering and factor graphs and want a rigorous statement of how the two relate. It is also relevant to engineers maintaining graph-based GNSS/INS or SLAM pipelines who need to justify design choices such as window size, iteration count, and marginalization strategy, and to reviewers and competition organizers trying to interpret performance differences between filtering and optimization entrants.

Authors’ abstract

Sliding window factor graph optimization (SW-FGO) is widely recognized for its robustness, yet its theoretical relationship with the extended Kalman filter (EKF) remains a subject of debate. This paper establishes the sufficient conditions to bridge SW-FGO with the iterated extended Kalman filter (IEKF). We introduce recursive FGO (Re-FGO), a conceptual perspective that employs a two-stage marginalization pipeline to mathematically degenerate the factor graph optimization to the IEKF recursive update. By enforcing the Markov assumption and a single-state window, we prove the theoretical equivalence between the IEKF and Re-FGO. This degeneration is validated through simulations and real-world urban GNSS and INS tightly coupled fusion experiments. The results confirm that Re-FGO exactly reproduces IEKF estimation behavior, demonstrating that the two-stage marginalization pipeline is foundational to enforce structural consistency, thereby successfully uniting graph-based smoothing and filtering paradigms under unified optimization principles.