因子图优化在GNSS/INS高精度定位中的应用与实践

1. GREAT-FGO项目概述

GREAT-FGO是武汉大学GREAT团队开源的一款基于因子图优化框架的高精度定位导航软件。作为一名长期从事GNSS/INS组合导航研究的工程师，我深知传统扩展卡尔曼滤波（EKF）在复杂环境下的局限性。GREAT-FGO的创新之处在于将机器人SLAM领域成熟的因子图优化技术引入卫星导航定位，通过保持历史观测约束实现了厘米级定位精度。

这个开源项目特别适合三类读者：GNSS算法研究人员希望了解前沿的优化方法，自动驾驶工程师需要稳定的定位解决方案，以及测绘领域从业者追求更高精度的定位结果。我在实际道路测试中发现，相比传统RTK算法，GREAT-FGO在城市峡谷环境中的定位可用性提升了约40%。

2. 因子图优化理论基础

2.1 传统滤波方法的局限性

在工程实践中，EKF存在两个致命缺陷：首先是线性化误差累积问题。当车辆急转弯时（角速度超过50°/s），EKF的一阶泰勒展开近似会导致姿态误差快速累积。我曾测试过一个案例：在90度弯道处，EKF的位置误差达到了1.2米，而FGO仅0.3米。

其次是马尔可夫假设的限制。传统滤波只保留当前状态，无法回溯修正历史数据。当GNSS信号被临时遮挡时，EKF只能依赖IMU推算，而FGO可以利用前后时刻的观测进行联合优化。这个特性使得FGO在隧道等场景中的表现明显优于EKF。

2.2 因子图的核心优势

因子图将定位问题建模为概率图模型，包含两类节点：

• 状态节点（圆形）：位置、速度、姿态等
• 因子节点（方形）：GNSS观测、IMU预积分等约束

这种表示方法的关键优势在于：

1. 滑动窗口机制保留多个历史状态
2. 所有观测共同参与优化
3. 异常数据可被动态检测和剔除

在开源代码中，滑动窗口大小通过GWINDOW_SIZE参数控制（默认10个历元）。实际测试表明，窗口大小与定位精度的关系如下表所示：