Continuous-time state estimation methods for multi-sensor fusion navigation

doi:10.11947/j.AGCS.2026.20250376

Abstract

Abstract:

Multi-sensor fusion effectively mitigates the limitations of individual navigation techniques, significantly enhancing the availability and reliability of navigation systems in complex environments. Conventional approaches for navigation parameter estimation, such as Kalman filtering and least-squares optimization, are predominantly based on discrete-time state models. These methods exhibit inherent limitations when dealing with asynchronous, multi-rate, and high-frequency sensor data. This paper proposes a continuous-time state estimation framework that employs uniform B-splines to parameterize the motion trajectory of the vehicle. By doing so, the original discrete pose estimation problem is transformed into the estimation of B-spline control points. The continuous-time analytical expression of the trajectory enables direct derivation to establish observation models that relate inertial measurement unit (IMU) outputs (i.e., acceleration and angular velocity) to the spline control points, thereby avoiding the precision degradation and noise property alterations associated with traditional inertial navigation system (INS) integration. Similarly, measurements from other sensors, such as position and velocity from GNSS, are also formulated as observation equations with respect to the spline control points, leading to a unified optimization problem. Simulation experiments demonstrate that the proposed method yields pose estimates with reduced high-frequency noise influence and improved smoothness. Specifically, it achieves a 41.4% improvement in positioning accuracy and a 35.0% improvement in attitude determination accuracy. During GNSS signal outages, the new approach shows slower error growth, reducing the maximum horizontal positioning error from 17.4 cm to 7.29 cm. In segments with missing IMU data, the method eliminates the need for data interpolation, enhancing positional accuracy by nearly twice and attitude accuracy by nearly fourfold. Experiments using real-world datasets further validate the effectiveness of the proposed method, showing that it maintains high performance even in complex environments, with positioning accuracy improved by 30.0% and attitude accuracy improved by 69.6%.

Key words: multi-sensor fusion, continuous-time state estimation, uniform B-splines, motion trajectory parameterization, pose estimation

CLC Number:

P228

Feng ZHU, Yuanmu LIAO, Rui ZHOU, Xiaohong ZHANG. Continuous-time state estimation methods for multi-sensor fusion navigation[J]. Acta Geodaetica et Cartographica Sinica, 2026, 55(5): 798-808.

Add to citation manager EndNote|Reference Manager|ProCite|BibTeX|RefWorks

URL: http://xb.chinasmp.com/EN/10.11947/j.AGCS.2026.20250376

http://xb.chinasmp.com/EN/Y2026/V55/I5/798

Figures/Tables 11

Fig. 1

Fig. 2

Fig. 3

Fig. 4

Tab. 1

Fig. 5

Tab. 2

Fig. 6

Fig. 7

Tab. 3

Fig. 8

References 25

[1]	李德仁, 洪勇, 王密, 等. 测绘遥感能为智能驾驶做什么？[J]. 测绘学报, 2021, 50(11): 1421-1431. DOI: . doi: 10.11947/j.AGCS.2021.20210280
	LI Deren, HONG Yong, WANG Mi, et al. What can photogrammetry and remote sensing do for intelligent driving？[J]. Acta Geodaetica et Cartographica Sinica, 2021, 50(11): 1421-1431. DOI: . doi: 10.11947/j.AGCS.2021.20210280
[2]	杨元喜. 综合PNT体系及其关键技术[J]. 测绘学报, 2016, 45(5): 505-510. DOI: . doi: 10.11947/j.AGCS.2016.20160127
	YANG Yuanxi. Integrated PNT system and its key technologies[J]. Acta Geodaetica et Cartographica Sinica, 2016, 45(5): 505-510. DOI: . doi: 10.11947/j.AGCS.2016.20160127
[3]	赵万龙, 孟维晓, 韩帅. 多源融合导航技术综述[J]. 遥测遥控, 2016, 37(6): 54-60.
	ZHAO Wanlong, MENG Weixiao, HAN Shuai. A survey of multi-source fusion navigation technology[J]. Journal of Telemetry, Tracking and Command, 2016, 37(6): 54-60.
[4]	BARFOOT T D. State estimation for robotics[M]. Cambridge: Cambridge University Press, 2017: 55-86.
[5]	MOURIKIS A I, ROUMELIOTIS S I. A multi-state constraint Kalman filter for vision-aided inertial navigation[C]//Proceedings of 2007 IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 2007: 3565-3572.
[6]	徐昊玮, 廉保旺, 刘尚波. 基于滑动窗迭代最大后验估计的多源组合导航因子图融合算法[J]. 兵工学报, 2019, 40(4): 807-819.
	XU Haowei, LIAN Baowang, LIU Shangbo. A factor graph fusion algorithm for multi-source integrated navigation based on sliding window iterative maximum a posteriori estimation[J]. Acta Armamentarii, 2019, 40(4): 807-819.
[7]	DELLAERT F, KAESS M. Factor graphs for robot perception[M]. Boston-Delft: Now Publishers, 2017: 19-24.
[8]	QIN Tong, LI Peiliang, SHEN Shaojie. VINS-mono: a robust and versatile monocular visual-inertial state estimator[J]. IEEE Transactions on Robotics, 2018, 34(4): 1004-1020.
[9]	SHAN Tixiao, ENGLOT B, MEYERS D, et al. LIO-SAM: tightly-coupled lidar inertial odometry via smoothing and mapping[C]//Proceedings of 2020 IEEE/RSJ International Conference on Intelligent Robots and Systems. Las Vegas: IEEE, 2020: 5135-5142.
[10]	LYNEN S, ACHTELIK M W, WEISS S, et al. A robust and modular multi-sensor fusion approach applied to MAV navigation[C]//Proceedings of 2013 IEEE International Conference on Robotics and Automation. Tokyo: IEEE, 2013: 3924-3931.
[11]	LÜ Jiajun, LANG Xiaobei, XU Jinhong, et al. Continuous-time fixed-lag smoothing for LiDAR-inertial-camera SLAM[J]. IEEE Transactions on Robotics, 2023, 39(4): 3017-3032.
[12]	FURGALE P, BARFOOT T D, SIBLEY G. Continuous-time batch estimation using temporal basis functions[C]//Proceedings of 2012 IEEE International Conference on Robotics and Automation. Piscataway: IEEE, 2012: 2088-2095.
[13]	FURGALE P, REHDER J, SIEGWART R. Unified temporal and spatial calibration for multi-sensor systems[C]//Proceedings of 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo: IEEE, 2013: 1280-1286.
[14]	FURGALE P, TONG C H, BARFOOT T D, et al. Continuous-time batch trajectory estimation using temporal basis functions[J]. International Journal of Robotics Research, 2015, 34(14): 1688-1710.
[15]	吕佳俊, 郎晓磊, 李宝润, 等. 基于B样条的连续时间轨迹状态估计研究综述[J]. 机器人, 2024, 46(6): 743-752.
	LÜ Jiajun, LANG Xiaolei, LI Baorun, et al. Review of continuous-time trajectory state estimation based on B-splines[J]. Robot, 2024, 46(6): 743-752.
[16]	QIAN Kun, WANG Jianguo, HU Baoxin. Novel integration strategy for GNSS-aided inertial integrated navigation[J]. Geomatica, 2015, 69(2): 217-230.
[17]	WU Yuanxin. iNavFIter: next-generation inertial navigation computation based on functional iteration[J]. IEEE Transactions on Aerospace and Electronic Systems, 2022, 58(5): 4220-4233.
[18]	ZHANG Hongyu, ZHU Maoran, CAI Qi, et al. Visual-inertial state estimation based on Chebyshev polynomial optimization[J]. IEEE Sensors Journal, 2025, 25(15): 29618-29629.
[19]	LANG Xiaolei, CHEN Chao, TANG Kai, et al. Coco-LIC: continuous-time tightly-coupled LiDAR-inertial-camera odometry using non-uniform B-spline[J]. IEEE Robotics and Automation Letters, 2023, 8(11): 7074-7081.
[20]	高翔, 张涛, 刘毅, 等. 视觉SLAM十四讲：从理论到实践[M]. 2版. 北京: 电子工业出版社, 2019: 70-75.
	GAO Xiang, ZHANG Tao, LIU Yi, et al. Fourteen lectures on visual SLAM: from theory to practice[M]. 2nd ed. Beijing: Publishing House of Electronics Industry, 2019: 70-75.
[21]	QIN Kaihuai. General matrix representations for B-splines[C]//Proceedings of 1998 IEEE International Conference on Systems, Man, and Cybernetics. Singapore: IEEE, 1998: 3903-3908.
[22]	OVRÉN H, FORSSÉN P E. Trajectory representation and landmark projection for continuous-time structure from motion[J]. The International Journal of Robotics Research, 2019, 38(6): 686-701.
[23]	SOLÀ J, DERAY J, ATCHUTHAN D. A micro Lie theory for state estimation in robotics[J]. arXiv Preprint, 2018(11): 1-17.
[24]	SOMMER C, USENKO V, SCHUBERT D, et al. Efficient derivative computation for cumulative B-splines on lie groups[C]//Proceedings of 2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition. Seattle: IEEE, 2020: 11145-11153.
[25]	张小红, 胡家欢, 任晓东. PPP/PPP-RTK新进展与北斗/GNSS PPP定位性能比较[J]. 测绘学报, 2020, 49(9): 1084-1100. DOI: . doi: 10.11947/j.AGCS.2020.20200328
	ZHANG Xiaohong, HU Jiahuan, REN Xiaodong. New progress of PPP/PPP-RTK and positioning performance comparison of BDS/GNSS PPP[J]. Acta Geodaetica et Cartographica Sinica, 2020, 49(9): 1084-1100. DOI: . doi: 10.11947/j.AGCS.2020.20200328

误差类型	分量	RMS		精度提升/（%）
误差类型	分量	DSE	CSE	精度提升/（%）
位置误差/cm	东向	1.14	0.60	47.4
	北向	0.96	0.64	33.3
	垂向	0.92	0.52	43.5
姿态误差/arcmin	航向角	0.62	0.31	50.0
	俯仰角	0.08	0.06	25.0
	横滚角	0.10	0.07	30.0

误差类型	分量	最大误差		精度提升/（%）
误差类型	分量	DSE	CSE	精度提升/（%）
位置误差/cm	东向	10.08	2.80	72.2
	北向	15.94	7.02	55.9
	垂向	5.90	1.35	77.1
姿态误差/arcmin	航向角	1.10	0.54	50.9
	俯仰角	0.25	0.24	4.0
	横滚角	0.38	0.15	60.5

误差类型	分量	RMS		精度提升/（%）
误差类型	分量	DSE	CSE	精度提升/（%）
位置误差/cm	东向	1.44	0.90	37.5
	北向	1.91	0.82	57.1
	垂向	1.76	0.88	50.0
姿态误差/arcmin	航向角	1.64	0.39	76.2
	俯仰角	0.50	0.11	78.0
	横滚角	0.50	0.10	80.0