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

This paper presents a real-time, tightly coupled GNSS-IMU integration method based on factor graph optimization that utilizes incremental optimization with fixed-lag marginalization to achieve robust, high-accuracy positioning in dense urban environments, as validated by experiments on the UrbanNav dataset.

The Gist

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

The Big Picture: Navigating the "Concrete Canyon"

Imagine you are walking through a massive city with skyscrapers towering on both sides. You are trying to find your way using two tools:

1. A Satellite Map (GNSS): It's great in open fields, but in the city, tall buildings block the signal, bounce it off glass (multipath), or hide it completely. It's like trying to hear a whisper in a noisy stadium.
2. A Pedometer and Gyroscope (IMU): This is a sensor on your wrist that counts your steps and senses your turns. It works perfectly in the dark, but it has a flaw: it slowly gets "drunk" on its own errors. If you walk for 10 minutes, it might think you are in a different city than you actually are.

The Problem: Most navigation systems today try to combine these two, but they do it in a way that is either too simple (ignoring the satellite's raw data) or too slow (waiting until the end of the trip to fix the errors).

The Solution: This paper introduces a new "smart navigator" called RTFGO-TC. It combines the two sensors in a way that is real-time (works instantly) and tightly coupled (they talk to each other constantly, not just occasionally).

---

The Core Concept: The "Group Chat" vs. The "Diary"

To understand the innovation, let's look at how computers usually solve navigation problems.

1. The Old Way: The "Diary" (Kalman Filter)

Imagine a detective who writes a diary. Every time a new clue comes in, they write it down and make a guess about where the suspect is. Once they write it, they never look back. If they made a mistake in the morning, they carry that mistake forward all day.

• Pros: Fast.
• Cons: If the morning guess was wrong, the whole day is wrong. It can't "rethink" past decisions.

2. The "Offline" Smart Way: The "Group Chat" (Standard Factor Graph Optimization)

Imagine a team of detectives who keep a massive group chat. When a new clue arrives, they don't just update the current guess; they scroll back through the entire chat history, re-evaluating every single clue together to find the perfect story.

• Pros: Extremely accurate. They catch mistakes from the past.
• Cons: It takes forever. You can't use this for a live navigation app because you'd have to wait until the end of the trip to get the answer.

3. The New Way: The "Sliding Window Group Chat" (This Paper's Method)

The authors created a system that acts like a sliding window over that group chat.

• It keeps a "chat history" of the last few minutes (the "lag").
• As new clues come in, it instantly re-evaluates the recent history to fix small errors.
• Once a clue gets too old (outside the window), it "archives" the most important parts of that old clue and deletes the rest to save space.
• The Result: You get the accuracy of the group chat (fixing past mistakes) with the speed of the diary (instant answers).

---

How It Works: The "Tightly Coupled" Magic

The paper calls this "Tightly Coupled." Here is a metaphor for what that means:

• Loosely Coupled (The Lazy Team): The Satellite team calculates a position and says, "We are at 5th Avenue." The Inertial team says, "We think we are at 6th Avenue." They average the two numbers and move on. If the satellite is lying (due to a building reflection), the average is still wrong.
• Tightly Coupled (The Detective Squad): The system doesn't just look at the final position. It looks at the raw evidence.
  • It asks the satellite: "Which specific satellites did you see? How strong was the signal?"
  • It asks the IMU: "How fast were you turning right now?"
  • It combines these raw numbers directly. If the satellite says "I see a signal from a building behind you," the system knows that's a lie (multipath) and ignores it, trusting the IMU's turn instead.

The "No-Extra-Sensor" Trick:
Usually, to know which way you are facing (attitude) without a compass, you need a special sensor. This system is clever: it figures out your direction by watching how your speed and position change over time. It's like figuring out which way a car is facing by watching how it turns and accelerates, even if you can't see the car.

---

The Results: Why Does This Matter?

The authors tested this in Hong Kong, one of the most difficult places on Earth for GPS (tall buildings, narrow streets).

1. Availability (Staying Online):

   • Standard GPS: Dropped out frequently (like a phone losing signal in an elevator).
   • This System: Stayed online 80% of the time, even when the satellite signal was terrible. It kept working when others gave up.

2. Accuracy (Getting the Right Street):

   • Horizontal (Left/Right): It was much more accurate than standard methods. It kept the car on the correct street.
   • Vertical (Up/Down): It was slightly less accurate at guessing the exact floor of a building. Why? Because in a city, satellites are usually high up, making it hard to judge height. The system prioritized getting the street right over the floor right, which is usually what matters for driving or walking.

3. Speed (Real-Time):

   • It runs fast enough on a standard laptop (even a MacBook Air) to be used in a live app. It doesn't need a supercomputer.

The Takeaway

This paper presents a navigation system that is smarter, tougher, and faster than current methods.

Think of it as upgrading from a GPS that gets confused by skyscrapers to a smart navigator that uses the buildings to help it figure out where it is. It constantly re-checks its recent history to correct mistakes, ensuring that even in the densest, most confusing cities, you can still find your way.

Technical Summary

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

1. Problem Statement

Reliable positioning in dense urban environments is severely challenged by GNSS signal blockage, multipath interference, and Non-Line-of-Sight (NLOS) reception. While Inertial Measurement Units (IMUs) offer high-rate relative motion estimation, they suffer from bias accumulation (drift) over time.

• Current Limitations: Most existing GNSS-IMU fusion systems rely on Kalman Filters (KF), which are single-pass estimators that may struggle with non-linearities. Furthermore, Factor Graph Optimization (FGO) methods, which offer superior accuracy through multi-pass smoothing and temporal consistency, are predominantly offline (batch processing). They require non-causal (future) data to refine past states, making them unsuitable for real-time applications where low latency and causal estimation are mandatory.
• The Gap: There is a lack of robust, real-time, tightly coupled (TC) GNSS-IMU solutions based on FGO that can operate causally without relying on external attitude sensors.

2. Methodology

The authors propose RTFGO-TC, a real-time, tightly coupled GNSS-IMU integration framework based on Factor Graph Optimization.

A. Core Architecture

• Tight Coupling: Unlike loosely coupled (LC) systems that fuse pre-calculated GNSS positions, RTFGO-TC fuses raw GNSS measurements (pseudoranges and Doppler shifts) directly with IMU pre-integrated measurements within a single optimization problem.
• State Vector: The system estimates a joint state vector including:
  • Position ($p$) and Velocity ($v$) in the navigation frame.
  • Attitude (Rotation matrix $C^n_b$).
  • IMU biases (accelerometer $b_a$ and gyroscope $b_\omega$).
  • Receiver clock bias ($\delta t$) and drift ($\delta \dot{t}$) for each satellite constellation.
• Factor Graph Components:
  • Prior Factors: Anchor the initial state.
  • IMU Pre-integration Factors: Constrain relative motion between time steps, accounting for biases.
  • Random Walk Factors: Model the slow evolution of biases and clock errors.
  • GNSS Pseudorange Factors: Constrain position and clock bias using raw pseudoranges (corrected for satellite clocks, relativity, etc.).
  • GNSS Doppler Factors: Constrain velocity and clock drift using pseudorange-rate measurements.

B. Real-Time Implementation Strategy

To enable real-time operation while retaining FGO benefits, the authors introduce three key modifications:

1. Incremental Fixed-Lag Smoothing: Instead of full batch smoothing, the system uses the iSAM2 incremental solver. It maintains a sliding window of states (fixed-lag). States older than the lag are marginalized (removed from the graph), with their information preserved as a condensed prior. This ensures causality and bounds computational complexity.
2. Doppler Integration: Dedicated factors for Doppler measurements are added to improve velocity and clock drift estimation, which indirectly aids attitude and position accuracy.
3. Self-Observability: The system does not use external attitude sensors (e.g., magnetometers or wheel odometry). Instead, attitude and inertial biases become observable through the dynamic coupling between velocity, position, and IMU measurements. This requires trajectories with sufficient dynamics (turns/accelerations) for convergence but removes hardware dependencies.

3. Key Contributions

• Real-Time FGO: The first demonstration of a tightly coupled GNSS-IMU FGO system capable of real-time, causal state estimation using fixed-lag marginalization.
• Tight Coupling without External Attitude: A formulation that estimates attitude and biases purely through the dynamic coupling of raw GNSS and IMU data, avoiding the need for additional sensors.
• Comprehensive Evaluation: A rigorous comparison against GNSS-only, Loosely Coupled (LC), and Offline Batch FGO methods in a highly degraded urban environment.

4. Experimental Results

The method was evaluated using the UrbanNav-HK-MediumUrban-1 dataset (Hong Kong), characterized by severe multipath and low GNSS availability (~38-42%).

• Service Availability:

  • RTFGO-TC and Offline SFGO-TC achieved ~80% service availability (within a 10m error threshold).
  • Loosely Coupled (LC) and GNSS-only solutions achieved only ~40%.
  • Significance: Tight coupling allows the system to remain operational during GNSS outages where LC methods fail.

• Positioning Accuracy (2D RMSE):

  • RTFGO-TC significantly outperformed GNSS-only and RTFGO-LC.
  • Loop 1: RTFGO-TC (6.44m) vs. GNSS-only (8.87m) vs. RTFGO-LC (10.92m).
  • Loop 2: RTFGO-TC (7.16m) vs. GNSS-only (8.25m) vs. RTFGO-LC (8.24m).
  • Note: While Offline SFGO-TC was slightly more accurate (4.79m/4.72m), RTFGO-TC achieved comparable performance with real-time capability.

• 3D Accuracy & Vertical Drift:

  • 3D RMSE for RTFGO-TC was slightly higher than LC in Loop 2 (22.88m vs 10.75m).
  • Cause: In dense urban canyons, vertical observability is poor. Without explicit vertical constraints (like map matching), vertical errors propagate through inertial integration. However, this has minimal impact on the critical horizontal accuracy required for ground navigation.

• Computational Performance:

  • Latency: The system runs in real-time on an Apple M3 processor.
  • Trade-off: Increasing the marginalization lag (window size) improves accuracy slightly but increases computation time significantly.
    • At a 60-second lag, optimization time is ~59ms (vs. ~6ms for LC).
    • This confirms the trade-off between tight coupling accuracy and computational load.

5. Significance and Conclusion

The paper demonstrates that Factor Graph Optimization can be successfully adapted for real-time, tightly coupled GNSS-IMU navigation.

• Robustness: RTFGO-TC provides a robust solution for dense urban environments where GNSS signals are intermittent, offering double the service availability of standard GNSS-only or LC approaches.
• Feasibility: It proves that the high accuracy of FGO does not require offline batch processing; fixed-lag marginalization effectively balances estimation quality with real-time constraints.
• Future Work: The authors plan to extend the framework to support carrier-phase measurements (PPP/RTK) and incorporate specific motion constraints (e.g., zero-velocity updates for pedestrians) to further enhance vertical accuracy and robustness.

In summary, RTFGO-TC represents a significant step forward in autonomous navigation, enabling high-precision, real-time positioning in challenging urban scenarios without relying on expensive external sensors or offline post-processing.