Multi-source Integrated Navigation Algorithm for Iterated Maximum Posteriori Estimation Based on Sliding-window Factor Graph

doi:10.3969/j.issn.1000-1093.2019.04.016

Abstract

Abstract: In the process of data fusion in multi-source integrated navigation using factor graph, the time-varying characteristics of the subsystem's observed noise have a great influence on the estimation accuracy of navigation state. In order to solve the problem, a Gaussian model-based method to estimate the mean vector and covariance matrix of sub-system observation is proposed. In the proposed method, the observed-measurement residuals for each iterative cycle in the process of factor graph optimization are utilized to update the maximum posteriori estimated values of mean vectors and covariance matrices. A more accurate estimated value of navigation state can be obtained by estimating the sub-system noise state. The influence of the new algorithm on the convergence of optimization process was also deduced. Both the simulated and experimental results show that, compared with the existing algorithms as factor graph, maximum likelihood estimation based factor graph and maximum posteriori based factor graph, the proposed factor graph method based on iterative maximum posteriori estimation can effectively improve the accuracy of navigation estimation when the subsystem observing state varies. Key

Key words: multi-sourceintegratednavigation, datafusion, factorgraph, inertialnavigationsystem, globalnavigationsatellitesystem, ultra-widebandnavigationsystem

CLC Number:

V249.32⁺8

XU Haowei, LIAN Baowang, LIU Shangbo. Multi-source Integrated Navigation Algorithm for Iterated Maximum Posteriori Estimation Based on Sliding-window Factor Graph[J]. Acta Armamentarii, 2019, 40(4): 807-819.

References

［1］ LI Z, WANG R, GAO J. An approach to improve the positioning performance of GPS/INS/UWB integrated system with two-step filter[J]. Remote Sensing, 2017, 10(1): 19-34.
[2] WANG M, LI Y, FENG G, et al. Key technologies of GNSS/INS/VO deep integration for UGV navigation in urban canyon[C]∥Proceedings of the 11th Asian Control Conference. Gold Coast, Australia: IEEE, 2017: 2546-2551.
[3] XU H W, LIAN B W, TOTH C K, et al. An airborne LiDAR/INS integrated navigation algorithm based on fuzzy controlled SIFT[C]∥Proceedings of the 2017 International Technical Meeting of the Institute of Navigation. Monterey. CA, US: the Institute of Navigation, 2017: 313-326.
[4] YUAN G M, YUAN K F, ZHANG H W. A variable proportion adaptive federal Kalman filter for INS/ESGM/GPS/DVL integrated navigation system[C]∥Proceedings of the 4th International Joint Conference on Computational Sciences and Optimization. Kunming/Lijiang, Yunnan, China: IEEE, 2011: 978-981.
[5] 钱进.改进的联合Kalman滤波及其在组合导航中的应用[J].火力与指挥控制,2009,34(7):142-144.
QIAN J. A modified federal Kalman filter algorithm and it's application at integrated navigation system[J]. Fire Control and Command Control, 2009, 34(7):142-144. (in Chinese)
[6] XU H W, LIAN B W. Fault detection for multi-source integrated navigation system using fully convolutional neural network[J]. IET Radar, Sonar & Navigation, 2018, 12(7):774-782.
[7] GAO W, ZHANG Y, WANG J. A strapdown inertial navigation system/Beidou/Doppler velocity log integrated navigation algorithm based on a cubature Kalman filter[J]. Sensors, 2014, 14(1): 1511-1527.
[8] 何伟, 廉保旺, 冯晓明. 基于联邦卡尔曼滤波的组合导航定位算法[J]. 火力与指挥控制, 2012, 37(8):147-150.
HE W, LIAN B W, FENG X M. Integrated navigation and location algorithm based on Federated Kalman filter[J]. Fire Control and Command Control, 2012, 37(8):147-150. (in Chinese)
[9] 高军强, 汤霞清, 张环,等. 基于因子图的车载INS/GNSS/OD组合导航算法[J]. 系统工程与电子技术, 2018, 40(11): 2547-2554.
GAO J Q, TANG X Q, ZHANG H, et al. Vehicle INS/GNSS/OD integrated navigation algorithm based on factor graph[J]. Systems Engineering and Electronics, 2018, 40(11): 2547-2554.(in Chinese)

[10] 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.
[11] ZENG Q, CHEN W, LIU J, et al. An improved multi-sensor fusion navigation algorithm based on the factor graph[J]. Sensors, 2017, 17(3): 641-656.
[12] 王慧哲, 曾庆化, 刘建业, 等. 基于因子图的无人机全源导航关键技术研究[J]. 导航与控制, 2017, 16(2):1-5.
WANG H Z, ZENG Q H, LIU J Y, et al. Research on the key technology of UAV of all source position navigation based on factor graph[J]. Navigation and Control, 2017, 16(2):1-5. (in Chinese)
[13] 吕瑞, 陈龙, 翁雪, 等. 利用先验点图模型的SLAM后端优化算法[J]. 武汉大学学报(信息科学版), 2014, 39(6):745-749.
L R, CHEN L, WENG X. A back-end optimization algorithm of SLAM based on graph model with prior points[J]. Geomatics and Information Science of Wuhan University, 2014, 39(6):745-749. (in Chinese)
[14] CHIU H, WILLIAMS S, DELLAERT F, et al. Robust vision-aided navigation using sliding-window factor graphs[C]∥Proceedings of 2013 IEEE International Conference on Robotics and Automation. Karlsruhe, Germany: IEEE, 2013: 46-53.
[15] CROUCH M S, MCGREGOR A, STUBBS D. Dynamic graphs in the sliding-window model[M]. London, UK: Springer, 2013.
[16] 秦永元, 张洪钺，汪叔华. 卡尔曼滤波与组合导航原理[M]. 西安：西北工业大学出版社, 1998:277-281.
QIN Y Y, ZHANG H Y, WANG S H. Theory of Kalman filter and integrated navigation[M].Xi'an: Northwestern Polytechnic University Press, 1998:277-281.(in Chinese)
[17] INDELMAN V, WILLIAMS S, KAESS M. Factor graph based incremental smoothing in inertial navigation systems[C]∥Proceedings of the 15th International Conference on Information Fusion. Singapore: IEEE, 2012: 2154-2161.
[18] PAN J X, FANG K T. Growth curve models and statistical diagnostics[M]. New York, NY, US: Springer, 2002.
[19] ELISHA Y B, INDELMAN V. Active online visual-inertial navigation and sensor calibration via belief space planning and factor graph based incremental smoothing[C]∥Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver, Canada: IEEE, 2017: 2616-2622.
[20] LIU H Y, ZHANG Z Y, GRIMM K J. Comparison of inverse Wishart and separation-strategy priors for Bayesian estimation of covariance parameter matrix in growth curve analysis[J]. Structural Equation Modeling: A Multidisciplinary Journal, 2016, 23(3): 354-367.
[21] HARTLEY H O. The modified Gauss-Newton method for the fitting of non-linear regression functions by least squares[J]. Technometrics, 1961, 3(2): 269-280.
[22] ECKENHOFF K, GENEVA P, HUANG G. Direct visual-inertial navigation with analytical preintegration[C]∥Proceedings of IEEE International Conference on Robotics and Automation. Marina Bay Sands, Singapore: IEEE, 2017: 1429-1435.

第40卷
第4期2019 年4月兵工学报ACTA
ARMAMENTARIIVol.40No.4Apr.2019

[1]	ZHOU Kexin, DING Xuhui, L Dedong, BU Xiangyuan, An Jianping. Research on LDPC Decoding Algorithm with High Efficiency and Low Complexity for Space-based Information Transmission [J]. Acta Armamentarii, 2024, 45(4): 1176-1185.
[2]	ZHANG Yu， WANG Peng， LIU Jiyuan， ZHONG Rongxing， WEI Linzhe， CHI Cheng. Motion Error Estimation Algorithm Based on DPC and INS for Synthetic Aperture Sonar [J]. Acta Armamentarii, 2021, 42(3): 588-597.
[3]	NING Xiaolei, WU Yingxia， ZHAO Xin. Small Sample Circular Error Probable Estimation Based on Fusing Simulation and Flight Test Data [J]. Acta Armamentarii, 2021, 42(2): 388-398.
[4]	DAI Tian， MIAO Lingjuan， SHAO Haijun. A Multi-beam Sonar-based Graph SLAM Method [J]. Acta Armamentarii, 2020, 41(7): 1384-1392.
[5]	ZHANG Zheng， LIU Chunguang， MA Xiaojun， ZHANG Yunyin. Method for Estimating Sideslip Angle of All-wheel Drive Vehicle Based on Data Fusion [J]. Acta Armamentarii, 2020, 41(5): 842-849.
[6]	SHEN Shibin, XIE Fei, ZHAO Jing, QIAN Weixing, KANG Guohua, LIU Xixiang. A Signal Demodulation Method for Ultra-tightly Coupled INS/GNSS Navigation System Based on Phase Control [J]. Acta Armamentarii, 2020, 41(3): 495-506.
[7]	HU Jie， YAN Yongjie， WANG Zihui. Vehicle Integrated Navigation Based on Velocity Constraint and Fuzzy Adaptive Filtering [J]. Acta Armamentarii, 2020, 41(2): 231-238.
[8]	CHANG Zhenjun, ZHANG Zhili, ZHOU Zhaofa, XU Zhihao, GUO Qi. Initial Alignment for Rotating SINS Based on On-line Compensation of Size Effect [J]. Acta Armamentarii, 2020, 41(10): 2016-2022.
[9]	WANG Qi, WANG Lixin, LIANG Shuhui, QIN Weiwei, SHEN Qiang. Selection of System Models for Continuous Self-calibration of Hybrid Inertial Navigation System Based on Observability Analysis [J]. Acta Armamentarii, 2020, 41(1): 56-67.
[10]	ZHU Bing, XU Jiangning, WU Miao, LI Jingshu, HE Hongyang, LI Feng. An Improved Optimal Alignment Method of Strapdown Inertial Navigation System Based on Reverse Navigation Algorithm [J]. Acta Armamentarii, 2019, 40(9): 1860-1870.
[11]	DONG Chunmei， REN Shunqing， CHEN Xijun， LI Wei. A System-level Calibration Method for Laser Gyro SINS [J]. Acta Armamentarii, 2019, 40(8): 1618-1626.
[12]	NI Longqiang, ZHANG Lihua, YAO Xintao, HU Gaoge, LIU Penghui. A Deep Fusion Method Based on Rough Sets and Evidence Theory for Local Conflict Evidence Synthesis [J]. Acta Armamentarii, 2019, 40(12): 2560-2569.
[13]	LI Qunsheng， ZHAO Yan， WANG Jinda. A Vision Aided MEMS-SINS/GPS Ultra-tight Coupled Navigation System Suitable for High Dynamic and Strong Interference Environment [J]. Acta Armamentarii, 2019, 40(11): 2241-2249.
[14]	LIU Sheng-pan， WANG Wen-ju， RAO Xing-qiao. Error Analysis and Rotation Scheme Research of Single-axis Rotary SINS [J]. Acta Armamentarii, 2018, 39(9): 1749-1755.
[15]	LI Wei， JIANG Jun-feng， REN Hai-bo， ZHANG Guang-hui. Modeling and Control of Pointing-stabilizing-tracking System for Marching Guns [J]. Acta Armamentarii, 2018, 39(8): 1479-1490.