Real-time loosely coupled GNSS and IMU integration via Factor Graph Optimization

This paper proposes a real-time, loosely coupled GNSS and IMU integration framework using Factor Graph Optimization that enhances service availability in challenging urban environments by trading off some positioning accuracy for improved computational efficiency compared to traditional batch methods.

The Gist

Here is an explanation of the paper using simple language and everyday analogies.

The Big Picture: Navigating a Foggy City

Imagine you are trying to walk through a massive, confusing city with tall skyscrapers (an "urban canyon"). You have two tools to help you find your way:

1. The Satellite Map (GNSS): This is like a GPS app on your phone. It's great when you are in an open park, but in a city, the tall buildings block the signal, bounce it off walls (creating "ghost" signals), or hide it completely. Sometimes, the map just says, "I don't know where you are."
2. The Inner Ear (IMU): This is like your sense of balance and motion. Even if you can't see the sun or stars, you know if you are walking forward, turning left, or speeding up. However, this sense isn't perfect. If you close your eyes and walk for 10 minutes, you will eventually drift off course because your brain makes tiny mistakes that add up over time.

The Problem:

• If you rely only on the Satellite Map, you get lost in the city.
• If you rely only on your Inner Ear, you eventually walk into a wall because of the drift.
• The Goal: Combine them so you get the best of both worlds.

The Old Way vs. The New Way

The Old Way (Post-Processing):
Imagine you take a video of your walk through the city. After you finish, you sit down with a computer, watch the whole video, and use a super-smart algorithm to figure out exactly where you were at every second. You can look at the future to correct the past.

• Pros: Very accurate.
• Cons: You have to wait until the end to know where you are. It's useless for a self-driving car that needs to know right now if it's about to hit a pedestrian.

The New Way (Real-Time FGO):
The authors of this paper created a system called RTFGO. Think of this as a smart navigator that updates your location while you are walking.

They use a method called Factor Graph Optimization. Imagine a giant, flexible spiderweb:

• The Knots: These are your location at different moments in time.
• The Strings: These connect the knots based on how you moved (IMU) or where the satellites said you were (GNSS).

In the past, this "spiderweb" kept getting bigger and bigger as you walked, eventually becoming too heavy for a computer to solve quickly. The authors figured out how to cut off the old, irrelevant parts of the web (a technique called Marginalization) so the computer stays fast, while still keeping the web strong enough to give a good answer.

The Three Big Trade-Offs (The "Juggling Act")

The paper explores three different ways to set up this smart navigator, showing that you can't have everything perfectly at once. You have to choose your priority:

1. The "Wait-and-See" Mode (High Accuracy, Low Availability)

• How it works: The system waits a few seconds to see if new satellite signals arrive before telling you where you are. It uses future information to fix past mistakes.
• Analogy: Like a teacher grading a test. They wait until they see the whole exam before giving you a final grade, allowing them to correct a mistake you made on question 1 based on your answer to question 10.
• Result: Very accurate, but if the satellite signal is lost for a long time, the system stops giving you answers.

2. The "Keep Moving" Mode (High Availability, Lower Accuracy)

• How it works: The system never waits. If the satellite signal disappears, it immediately switches to just using your "Inner Ear" (IMU) to guess where you are.
• Analogy: Like a runner who keeps running even if they lose their map. They might not be 100% sure of the exact path, but they never stop moving.
• Result: You always have a location (high availability), but because the "Inner Ear" drifts, your location might be a little off (lower accuracy).

3. The "Memory Limit" Mode (Balancing Speed and Smarts)

• How it works: The system decides how far back in time it wants to remember.
  • Remember everything: The computer gets slow and might crash (too heavy).
  • Forget everything old: The computer is super fast, but it loses the ability to correct old mistakes, making it more sensitive to noise.
• Analogy: Like a student taking a test. If they try to remember every single fact they've ever learned, they get overwhelmed. If they only remember what they learned 5 minutes ago, they are fast but might miss the big picture. The authors found a "sweet spot" (around 50 seconds of memory) that keeps the computer fast but still smart enough to be accurate.

The Conclusion

The paper proves that you can build a navigation system that works in real-time (instantly) even in the worst city environments.

• The Catch: To get instant results, you have to accept that sometimes you are slightly less accurate than if you waited to process the data later.
• The Win: For self-driving cars, drones, and robots, being "good enough" and "instant" is much better than being "perfect" but "late."

In short: The authors built a navigation system that acts like a smart, fast-thinking human who knows when to trust the map, when to trust their gut, and how to keep moving even when the map goes blank.

Technical Summary

Here is a detailed technical summary of the paper "Real-time loosely coupled GNSS and IMU integration via Factor Graph Optimization."

1. Problem Statement

Accurate Positioning, Navigation, and Timing (PNT) are critical for autonomous systems. While Global Navigation Satellite Systems (GNSS) are the primary source for outdoor positioning, they suffer significantly in urban canyon environments due to:

• Signal outages (loss of Line-of-Sight).
• Multipath effects and Non-Line-of-Sight (NLOS) reception.
• Interference and jamming.

To mitigate these issues, GNSS is often fused with Inertial Measurement Units (IMU). However, IMUs suffer from cumulative drift over time. While Factor Graph Optimization (FGO) has emerged as a superior framework for sensor fusion compared to traditional Kalman Filters (EKF) due to its ability to exploit temporal correlations and handle non-Gaussian noise, existing FGO implementations are primarily batch/post-processing methods. They lack real-time capability because:

1. They require all data to be collected before optimization.
2. They do not provide position estimates during GNSS outages (low service availability).
3. The computational cost grows unbounded as the graph expands, making real-time execution difficult.

2. Methodology

The authors propose RTFGO (Real-Time Factor Graph Optimization), a loosely coupled architecture designed to operate in real-time while maintaining the benefits of FGO.

A. Architecture Overview

• Loosely Coupled: The system fuses GNSS-derived states (position, velocity, time) with IMU data, rather than fusing raw pseudoranges (tightly coupled).
• Factor Graph Structure:
  • Nodes ($x_k$): Represent the state vector at time $k$, including pose ($R_k, p_k$), velocity ($v_k$), and IMU biases ($b_a, b_g$).
  • Factors:
    • Prior Factor: Anchors the initial state.
    • IMU Preintegration Factor: Constrains relative motion between consecutive states using preintegrated IMU measurements.
    • IMU Bias Random Walk: Models bias drift over time.
    • GNSS Factor: Constrains the estimated position against GNSS receiver outputs.
• Optimization: The system minimizes a cost function $J(X)$ (negative log-posterior) using iterative non-linear least squares (specifically the iSAM2 algorithm via the GTSAM library).

B. Key Modifications for Real-Time Operation

To transition from batch processing to real-time, the authors introduced three critical mechanisms:

1. IMU-Only Propagation:

   • During GNSS outages, the system continues to propagate the state using IMU data alone.
   • Trade-off: This increases service availability but introduces drift. The duration of propagation is limited by a time threshold based on IMU performance.

2. Smoothing Latency ($\tau$):

   • To leverage FGO's smoothing capability, the system can delay the output by a fixed time $\tau$.
   • This allows future GNSS measurements to refine past or current state estimates.
   • Trade-off: Higher latency improves accuracy but reduces real-time responsiveness. If $\tau$ equals the full trajectory length, it reverts to a batch method (SFGO).

3. Fixed-Lag Marginalization:

   • To prevent the factor graph from growing infinitely (which would crash real-time performance), states older than a specific "lag" are marginalized (removed) from the graph.
   • Trade-off: This bounds computational load and memory usage but discards historical information that could help correct biases, making the solution more sensitive to local noise.

3. Key Contributions

• Real-Time FGO Framework: The first demonstration of a loosely coupled GNSS/IMU FGO system capable of real-time operation in challenging urban environments.
• Trade-off Analysis: A comprehensive quantification of the trade-offs between positioning accuracy, service availability, and computational efficiency introduced by smoothing latency and marginalization lag.
• Service Availability Metric: Introduction of a metric ($A(\theta)$) that combines positioning accuracy and continuity, evaluating the fraction of time a valid solution exists within a specific error threshold, even during GNSS outages.
• Open Source: The code and experimental setup are made publicly available.

4. Experimental Results

The method was evaluated on the UrbanNav-HK-MediumUrban-1 dataset (dense urban canyon in Hong Kong) using a MacBook Air M3.

• Dataset Characteristics: Two loops with GNSS availability of only ~40% due to severe multipath and signal blockage.
• Accuracy vs. Availability:
  • SFGO (Batch/Post-processing): Achieved the highest accuracy (e.g., 9.33m 3D RMSE on Loop 2) but zero availability during GNSS outages.
  • RTFGO (Real-time, $\tau=0$): Substantially increased service availability by utilizing IMU propagation during outages. However, accuracy decreased (11.83m 3D RMSE) due to accumulated IMU drift and lack of future measurement refinement.
  • RTFGO (with Smoothing): Increasing smoothing latency improved accuracy (approaching SFGO levels) but reduced real-time responsiveness.
• Marginalization Impact:
  • Increasing the marginalization lag improved accuracy (reducing RMSE) but increased computational time.
  • A lag of 50 seconds achieved ~12.6m 3D RMSE with a computation time of ~5ms, demonstrating the feasibility of real-time operation.
• Cold Start: The system requires 3–5 consecutive GNSS fixes to establish a reliable initial heading (yaw), affecting availability at the very start of a trajectory.

5. Significance and Conclusion

This paper bridges the gap between the high accuracy of Factor Graph Optimization and the strict latency requirements of autonomous systems.

• Practical Impact: It proves that FGO can be deployed in real-time for autonomous vehicles in urban canyons, providing continuous positioning where GNSS-only solutions fail.
• Design Guidance: The study provides engineers with a clear framework to tune the system based on application needs:
  • High Safety/Accuracy: Use higher smoothing latency and longer marginalization lags (if compute allows).
  • High Availability/Responsiveness: Use lower latency and shorter lags, accepting slightly higher drift.
• Future Work: The authors plan to extend this to tightly coupled integration (fusing raw measurements) and incorporate environmental constraints (e.g., zero-velocity updates, map-based priors) to further enhance robustness during GNSS outages.