Real-Time Loosely Coupled GNSS/IMU Factor Graph Optimization (RTFGO-LC)

Implementation and reproducible code for the paper:

Real-Time Loosely Coupled GNSS/IMU Integration via Factor Graph Optimization
submitted to European Control Conference (ECC) 2026.

If you use this work in your research, please cite:

@inproceedings{
    author={Cioaca, Radu-Andrei and Rusu, Cristian and Irofti, Paul and Caparra, Gianluca and Marinache, Andrei-Alexandru and Stoican, Florin}
    title={Real-Time Loosely Coupled GNSS/IMU Factor Graph Optimization}, 
    year={2026}
}

---

Overview

This repository contains the RTFGO-LC (Real-Time Factor Graph Optimization – Loosely Coupled) algorithm used in our GNSS/IMU fusion experiments.

The system:

• reads GNSS (PVT) + IMU (CSV) files
• merges them into a time-ordered measurement stream using k-way merge
• preprocesses GNSS & IMU
• performs factor graph optimization using GTSAM
• outputs navigation solution

---

Prerequisites

You need:

Python 3.9+

Python packages (install automatically):

pip install -r requirements.txt

Includes:

• numpy
• scipy
• pandas
• georinex
• xarray
• matplotlib
• folium

GTSAM (Required)

RTFGO-LC uses GTSAM for factor graphs.

Install GTSAM following the official instructions:

🔗 https://gtsam.org
🔗 https://github.com/borglab/gtsam

---

Usage

1. Configure dataset paths

Open __config.py and set dataset path:

Cfg.gnss_pvt_file       = "/path/to/pvt.pos"
Cfg.imu_file            = "/path/to/imu.csv"
Cfg.gt_file             = "/path/to/gt.txt"

Cfg.GNSS_mode = "loosely"
Cfg.sensor_fusion_mode = "RTFGO_LC"

Cfg.init_pos = [...]   # initial position
Cfg.init_vel = [...]   # initial velocity
Cfg.init_att = [...]   # initial quaternion

2. Run the GNSS/IMU Factor Graph Pipeline

From the repo root:

python run/main_ds.py

This will:

• read GNSS + IMU data
• preprocess IMU and PVT data
• execute the RTFGO-LC factor graph
• generate navigation results under ./out/
• produce Figure 3, subplots (b) and (c)

3. Inspect the results

Outputs appear in:

out/current_run

Common files:

• RTFGO_LC.csv — latitude, longitude, height, velocity, attitude
• state_trajectory.csv — estimated FGO key chains at different epochs

---

Fusion (RTFGO-LC)

Implemented in:

src/fusion/RTFGO_LC.py

This module:

• builds a factor graph with:
  • IMU preintegration factors
  • GNSS PVT factors
• runs optimization with GTSAM
• propagates IMU between key states
• updates both recent and past states (fixed lag)

Navigation State Machine

Located in:

src/app/nav_thread.py

Controls:

• waiting for first IMU + first GNSS
• initializing states
• creating factor graph keys
• deciding when to optimize vs. propagate
• producing outputs

---

License

BSD