A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting

MA Jie; WANG Wen; DENG Chenlong; TANG Weiming; QIU Shijin

doi:10.13203/j.whugis20240493

您当前的位置：

首页 >

文章列表页 >

A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting

Geodesy and Survey Engineering | 更新时间：2026-11-05

- A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting
- Geomatics and Information Science of Wuhan University Vol. 51, Issue 6, Pages: 1098-1109(2026)
- 作者机构：
  
  1.武汉理工大学航运学院，湖北武汉，430063
  2.水路交通控制全国重点实验室（武汉理工大学），湖北武汉，430063
  3.武汉大学卫星导航定位技术研究中心，湖北武汉，430079
- 作者简介：
- 基金信息：
- DOI：10.13203/j.whugis20240493
  CLC： P228
- Received：09 March 2025，
  
  Published：05 June 2026
- 稿件说明：
移动端阅览
马杰,王雯,邓辰龙，等.动态观测权调整的GNSS/INS抗差因子图优化算法[J].武汉大学学报（信息科学版）,2026,51(6):1098-1109.

MA Jie,WANG Wen,DENG Chenlong,et al.A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting[J].Geomatics and Information Science of Wuhan University,2026,51(6):1098-1109.
马杰,王雯,邓辰龙，等.动态观测权调整的GNSS/INS抗差因子图优化算法[J].武汉大学学报（信息科学版）,2026,51(6):1098-1109. DOI：,10.13203/j.whugis20240493

MA Jie,WANG Wen,DENG Chenlong,et al.A Robust Factor Graph Optimization Algorithm with Dynamic Observation Weight Adjusting[J].Geomatics and Information Science of Wuhan University,2026,51(6):1098-1109. DOI：,10.13203/j.whugis20240493

摘要

全球导航卫星系统（global navigation satellite system

GNSS）和惯性导航系统（inertial navigation system

INS）组合导航能够提供连续的定位服务，但在复杂的城市环境中，GNSS信号易受多路径效应和非视距信号干扰，异常观测值频发，显著影响组合导航的定位精度。针对传统因子图优化方法中固定权重难以适应GNSS量测噪声协方差动态变化，以及引入Huber核函数的抗差算法计算开销较大的问题，提出了一种基于动态观测权调整的抗差因子图优化算法。该算法构建了动态权函数，依据滑动窗口内的验后残差自适应调整GNSS观测在目标函数中的残差权重系数，不改变残差值本身，从而保持目标函数的二次形式。利用不同载体自采数据与不同环境下的开源数据进行实验验证，结果表明，相较于基于Huber核函数的抗差算法，所提算法的定位结果均方根误差和标准差分别降低了约12%和26%，计算时间缩短约18%，在提高组合导航定位精度的同时优化了计算效率。此外，基于开源数据分析了不同滑动窗口大小对算法抗差性能的影响，结果表明，当滑动窗口设置为30 s时，抗差效果与计算时间达到较好平衡，且所提算法不改变滑动窗口大小对定位精度与计算耗时的影响趋势。

Abstract

Objectives

The integration of global navigation satellite system (GNSS) and inertial navigation system (INS) provides continuous positioning services. However

in complex urban environments

GNSS signals are highly susceptible to multipath effects and non-line-of-sight interference

which can severely degrade the positioning accuracy of integrated navigation systems. Traditional factor graph optimization methods with fixed weights struggle to accommodate the dynamic variability of GNSS measurement noise covariance

while robust algorithms employing the Huber kernel function incur significant computational overhead.

Methods

A dynamic weight function-based robust factor graph optimization algorithm (FGO) is proposed

which dynamically adjusts the residual weight coefficients of GNSS observations in the objective function based on post-fit residuals within a sliding window. The algorithm preserves the quadratic form of the objective function by avoiding nonlinear transformations of residual values

ensuring computational efficiency and adaptability to varying observation quality. To process open-source data with the proposed robust algorithm

sliding windows of 10 s

30 s

and 50 s are utilized. The robust performance of the proposed algorithm under different sliding window sizes is analyzed.

Results

Compared to the inclusion of the Huber kernel function

the inclusion of the dynamic weight function adjustment mechanism improves the average positioning error standard deviation and positioning root mean square error by 26% and 12%

respectively. When the sliding window is set to 30 s

a satisfactory equilibrium between robustness and computation time can be attained.

Conclusions

Experimental results show that:(1) compared to FGO with the addition of Huber kernel function

the dynamic weight function adjustment can effectively reduce the influence of GNSS outliers

and has higher positioning accuracy and robustness. (2) The proposed robust algorithm does not change the trend of the sliding window size on localization and time consumption.

关键词

Keywords

references

张小红，张元泰，朱锋 . 城市复杂场景下GNSS定位的因子图优化方法及其抗差性能分析［J］. 武汉大学学报（信息科学版）， 2023 ， 48 （ 7 ）： 1050 - 1057 .

ZHANG Xiaohong ， ZHANG Yuantai ， ZHU Feng . Factor Graph Optimization for Urban Environment GNSS Positioning and Robust Performance Analysis ［J］. Geomatics and Information Science of Wuhan University ， 2023 ， 48 （ 7 ）： 1050 - 1057 .

XU B ， JIA Q Q ， HSU L T . Vector Tracking Loop-Based GNSS NLOS Detection and Correction： Algorithm Design and Performance Analysis ［J］. IEEE Transactions on Instrumentation and Measurement ， 2020 ， 69 （ 7 ）： 4604 - 4619 .

SUN Y ， FU L . Stacking Ensemble Learning for Non-Line-of-Sight Detection of Global Navigation Satellite System ［J］. IEEE Transactions on Instrumentation and Measurement ， 2022 ， 71 ： 3512510 .

GUO J W ， ZHOU Y L ， ZHAO S ， et al . A New GNSS Outlier Mitigation Method for GNSS/INS Integrated System ［J］. Measurement Science and Technology ， 2023 ， 34 （ 10 ）： 105118 .

柳景斌，赵智博，胡宁松，等 . 室内高精度定位技术总结与展望［J］. 武汉大学学报（信息科学版）， 2022 ， 47 （ 7 ）： 997 - 1008 .

LIU Jingbin ， ZHAO Zhibo ， HU Ningsong ， et al . Summary and Prospect of Indoor High-Precision Positioning Technology ［J］. Geomatics and Information Science of Wuhan University ， 2022 ， 47 （ 7 ）： 997 - 1008 .

YAN P H ， JIANG J G ， ZHANG F N ， et al . An Improved Adaptive Kalman Filter for a Single Frequency GNSS/MEMS-IMU/Odometer Integrated Navigation Module ［J］. Remote Sensing ， 2021 ， 13 （ 21 ）： 4317 .

SUN W ， LIU J Z . RCKF Cooperative Navigation Algorithm for Tightly Coupled Vehicle Ad Hoc Networks Based on Huber M Estimation ［J］. IEEE Access ， 2021 ， 9 ： 139888 - 139895 .

BARFOOT T D . State Estimation for Robotics ［M］. Cambridge ： Cambridge University Press ， 2024 .

WEN W S ， BAI X W ， KAN Y C ， et al . Tightly Coupled GNSS/INS Integration via Factor Graph and Aided by Fish-Eye Camera ［J］. IEEE Transactions on Vehicular Technology ， 2019 ， 68 （ 11 ）： 10651 - 10662 .

INDELMAN V ， WILLIAMS S ， KAESS M ， et al . Information Fusion in Navigation Systems via Factor Graph Based Incremental Smoothing ［J］. Robotics and Autonomous Systems ， 2013 ， 61 （ 8 ）： 721 - 738 .

CHEN D ， GAO G X . Probabilistic Graphical Fusion of LiDAR， GPS， and 3D Building Maps for Urban UAV Navigation ［J］. NAVIGATION ， 2019 ， 66 （ 1 ）： 151 - 168 .

WU X W ， XIAO B ， WU C H ， et al . Factor Graph Based Navigation and Positioning for Control System Design： A Review ［J］. Chinese Journal of Aeronautics ， 2022 ， 35 （ 5 ）： 25 - 39 .

崔晓珍，周琪，武东杰，等 . GNSS/IMU与里程计紧-松耦合的因子图融合定位方法［J］. 武汉大学学报（信息科学版）， 2024 ， 49 （ 10 ）： 1911 - 1921 .

CUI Xiaozhen ， ZHOU Qi ， WU Dongjie ， et al . Factor Graph Fusion Localization Method with Tight and Loose Coupling of GNSS/IMU and Odometry ［J］. Geomatics and Information Science of Wuhan University ， 2024 ， 49 （ 10 ）： 1911 - 1921 .

WEN W S ， PFEIFER T ， BAI X W ， et al . Factor Graph Optimization for GNSS/INS Integration： A Comparison with the Extended Kalman Filter ［J］. NAVIGATION ， 2021 ， 68 （ 2 ）： 315 - 331 .

黄紫如，柴洪洲，向民志，等 . 考虑信息滞后的AUV因子图多源信息融合定位算法［J］. 中国惯性技术学报， 2021 ， 29 （ 5 ）： 625 - 631 .

HUANG Ziru ， CHAI Hongzhou ， XIANG Minzhi ， et al . Multi-source Information Fusion Localization Algorithm Based on AUV Factor Graph Considering Information Delay ［J］. Journal of Chinese Inertial Technology ， 2021 ， 29 （ 5 ）： 625 - 631 .

唐卫明，戚克培，邓辰龙，等 . 因子图框架下里程计辅助GNSS/INS组合导航算法［J］. 测绘通报， 2024 （ 3 ）： 63 - 68 .

TANG Weiming ， QI Kepei ， DENG Chenlong ， et al . An Odometer-Aided GNSS/INS Integrated Navigation Algorithm Under the Framework of FGO ［J］. Bulletin of Surveying and Mapping ， 2024 （ 3 ）： 63 - 68 .

潘献飞，宁治文，王茂松，等 . 基于因子图的导航定位技术应用分析与思考［J］. 控制理论与应用， 2023 ， 40 （ 12 ）： 2130 - 2141 .

PAN Xianfei ， NING Zhiwen ， WANG Maosong ， et al . Analysis and Reflection on the Navigation and Positioning Application Based on Factor Graph ［J］. Control Theory & Applications ， 2023 ， 40 （ 12 ）： 2130 - 2141 .

XIONG J ， XIONG Z ， ZHUANG Y ， et al . Fault-Tolerant Cooperative Positioning Based on Hybrid Robust Gaussian Belief Propagation ［J］. IEEE Transactions on Intelligent Transportation Systems ， 2023 ， 24 （ 6 ）： 6425 - 6431 .

LI X X ， YU H ， WANG X B ， et al . FGO-GIL： Factor Graph Optimization-Based GNSS RTK/INS/LiDAR Tightly Coupled Integration for Precise and Continuous Navigation ［J］. IEEE Sensors Journal ， 2023 ， 23 （ 13 ）： 14534 - 14548 .

KANHERE A V ， MINA T ， SHETTY A ， et al . Factor Graph-Based Spoofing Mitigation Using the Chimera Signal Enhancement ［C］// The 35th International Technical Meeting of the Satellite Division of the Institute of Navigation （ION GNSS+ 2022）， Denver， USA， 2022 .

SÜNDERHAUF N ， PROTZEL P . Switchable Constraints for Robust Pose Graph SLAM ［C］// IEEE/RSJ International Conference on Intelligent Robots and Systems ， Vilamoura-Algarve， Portugal ， 2012 .

AGARWAL P ， TIPALDI G D ， SPINELLO L ， et al . Robust Map Optimization Using Dynamic Covariance Scaling ［C］// IEEE International Conference on Robotics and Automation ， Karlsruhe， Germany ， 2013 .

JIN Y L ， LIU S Y ， YU F ， et al . INS/GNSS/UWB/OD Robust Navigation Algorithm Based on Factor Graph ［C］// IEEE International Conference on Mechatronics and Automation （ICMA）， Harbin， China ， 2023 .

蒋郡祥 . 基于图优化的视觉/惯性/GNSS融合导航方法研究［D］. 武汉：武汉大学， 2021 .

JIANG Junxiang . Research on Graph Optimization-Based Visual/InertialGNSS Integrated Navigation Method ［D］. Wuhan ： Wuhan University ， 2021 .

陈映秋，旷俭，牛小骥，等 . 基于车轮安装惯性测量单元的车载组合导航［J］. 中国惯性技术学报， 2018 ， 26 （ 6 ）： 799 - 804 .

CHEN Yingqiu ， KUANG Jian ， NIU Xiaoji ， et al . Vehicle Integrated Navigation Based on Wheel Mounted MEMS-IMU ［J］. Journal of Chinese Inertial Technology ， 2018 ， 26 （ 6 ）： 799 - 804 .

马晓爽 . 基于因子图的AUV惯性基多源融合定位技术研究［D］. 南京：东南大学， 2022 .

MA Xiaoshuang . Research on Multi-source Fusion Positioning Technology with Inertial Base for AUV Based on Factor Graph ［D］. Nanjing ： Southeast University ， 2022 .

TANG H L ， ZHANG T S ， NIU X J ， et al . Impact of the Earth Rotation Compensation on MEMS-IMU Preintegration of Factor Graph Optimization ［J］. IEEE Sensors Journal ， 2022 ， 22 （ 17 ）： 17194 - 17204 .

WANG J W ， CHEN X Y ， SHI C F ， et al . Robust M -Estimation-Based ICKF for GNSS Outlier Mitigation in GNSS/SINS Navigation Applications ［J］. IEEE Transactions on Instrumentation and Measurement ， 2023 ， 72 ： 8505617 .

陈熙源，周云川，钟雨露，等 . 基于变分贝叶斯的鲁棒自适应因子图优化组合导航算法［J］. 仪器仪表学报， 2024 ， 45 （ 1 ）： 120 - 129 .

CHEN Xiyuan ， ZHOU Yunchuan ， ZHONG Yulu ， et al . Robust Adaptive Factor Graph Optimization Integrated Navigation Algorithm Based on Variational Bayesian ［J］. Chinese Journal of Scientific Instrument ， 2024 ， 45 （ 1 ）： 120 - 129 .

赵前程，田俊英，伍济钢 . 联合Huber核函数与可切换约束算法改进的视觉惯性SLAM方法［J］. 中国惯性技术学报， 2023 ， 31 （ 4 ）： 375 - 380 .

ZHAO Qiancheng ， TIAN Junying ， WU Jigang . An Improved Vision Inertial SLAM Method Combining Huber Kernel Function and Switchable Constraint Algorithm ［J］. Journal of Chinese Inertial Technology ， 2023 ， 31 （ 4 ）： 375 - 380 .

YANG Y . Robust Estimation of Geodetic Datum Transformation ［J］. Journal of Geodesy ， 1999 ， 73 （ 5 ）： 268 - 274 .

GREATGNSS Rsearch ， Application and Teaching Group from SGG of Wuhan University . GREAT Dataset： A Vehicle-Mounted Multi-sensor Raw Observation Dataset in Complex Urban Environment ［EB/OL］. ［ 2024-11-08 ］. https：//github.com/GREAT-WHU/GREAT-Dataset.git https://github.com/GREAT-WHU/GREAT-Dataset.git .

TAKASU T . RTKLIB： An Open Source Program Package for GNSS Positioning ［EB/OL］. （ 2019-03-01 ）［ 2020-12-29 ］. https：//www.rtklib.com https://www.rtklib.com .

Views

下载量

CSCD

Alert me when the article has been cited

提交

Tools

Publicity Resources

A Lightweight Collaborative Semantic Mapping Method Based on Factor Graph Optimization

Factor Graph Optimization for Urban Environment GNSS Positioning and Robust Performance Analysis

Spatiotemporal Evolution and Explainable Analysis of Grassland Vulnerability in the Hohhot-Baotou-Ordos-Yulin Urban Agglomeration

Intelligent Change Detection Method and Application Based on Wuhan-1 Satellite Imagery

Research Progress on Uncertainty in Radiometric Benchmark Transfer for Ocean Targets

Related Author

LI Xingxing

SUN Meng

ZHOU Yuxuan

ZHANG Xiaohong

ZHANG Yuantai

ZHU Feng

Related Institution

School of Geodesy and Geomatics, Wuhan University

School of Geodesy and Geomatics

Hubei Luojia Laboratory, Wuhan University

Chinese Antarctic Center of Surveying and Mapping, Wuhan University

AI问答

⁰