GNSS/SINS integrated navigation method considering the geometric property of biases state

doi:10.11947/j.AGCS.2025.20240232

Abstract

Abstract:

In the current design of invariant extended Kalman filter (EKF), biases states are not included in the kinematic equations with geometric properties due to that the kinematic systems of strapdown inertial navigation system (SINS) with biases no longer have group affine properties. This article constructs a group and a group action to satisfy the equivariant property of the kinematic equation containing biases state, which can naturally handle the problem of inertial based integrated navigation systems containing gyro bias and acceleration bias, and the linearization error of navigation state error dynamics can be reduced theoretically. Although the integrated navigation based on invariant EKF has received widespread attention, there has been few research on GNSS/SINS tightly coupled integrated navigation based on equivariant error in the world frame. Therefore, this article proposes a GNSS/SINS tightly coupled integrated navigation system based on equivariant errors in the world frame. Unlike the currently popular invariant EKF, the equivariant error constructed in this paper is based on a symmetry that appropriately includes all states in the group structure. The experimental results show that the filtering algorithm proposed in this paper has better transient response under different large misalignment angles compared to the right invariant EKF. At the same time, the robust model in the tightly coupled integrated navigation in the world frame effectively improves filtering robustness.

Key words: homogeneous manifold, group action, equivariant system, equivariant error, world frame, GNSS/SINS integrated navigation, robust model

CLC Number:

P228

Yarong LUO, Chi GUO, Wei OUYANG, Jingnan LIU. GNSS/SINS integrated navigation method considering the geometric property of biases state[J]. Acta Geodaetica et Cartographica Sinica, 2025, 54(1): 26-39.

Figures/Tables 8

Fig. 1

Fig. 2

Tab. 1

Fig. 3

Fig. 4

Fig. 5

Tab. 2

Tab. 3

References 34

[1]	刘经南, 罗亚荣, 郭迟, 等. PNT智能与智能PNT[J]. 测绘学报, 2022, 51(6): 811-828. DOI:. doi: 10.11947/j.AGCS.2022.20220152
	LIU Jingnan, LUO Yarong, GUO Chi, et al. PNT intelligence and intelligent PNT[J]. Acta Geodaetica et Cartographica Sinica, 2022, 51(6): 811-828. DOI:. doi: 10.11947/j.AGCS.2022.20220152
[2]	BARRAU A, BONNABEL S. The invariant extended Kalman filter as a stable observer[J]. IEEE Transactions on Automatic Control, 2017, 62(4): 1797-1812.
[3]	CHANG Lubin, LUO Yarong. Log-linear error state model derivation without approximation for INS[J]. IEEE Transactions on Aerospace and Electronic Systems, 2023, 59(2): 2029-2035.
[4]	HARTLEY R, GHAFFARI M, EUSTICE R M, et al. Contact-aided invariant extended Kalman filtering for robot state estimation[J]. The International Journal of Robotics Research, 2020, 39(4): 402-430.
[5]	XU Bo, GUO Yu. A novel DVL calibration method based on robust invariant extended Kalman filter[J]. IEEE Transactions on Vehicular Technology, 2022, 71(9): 9422-9434.
[6]	CUI Jiarui, WANG Maosong, WU Wenqi, et al. Lie group based nonlinear state errors for MEMS-IMU/GNSS/magnetometer integrated navigation[J]. Journal of Navigation, 2021, 74(4): 887-900.
[7]	CHANG Lubin, DI Jingbo, QIN Fangjun. Inertial-based integration with transformed INS mechanization in Earth frame[J]. ASME Transactions on Mechatronics, 2022, 27(3): 1738-1749.
[8]	LUO Yarong, LU Fei, GUO Chi, et al. Matrix Lie group-based extended Kalman filtering for inertial-integrated navigation in the navigation frame[J]. IEEE Transactions on Instrumentation and Measurement, 2024, 73: 1-16.
[9]	罗亚荣, 郭文飞, 齐书峰, 等. 不变RTS平滑算法及其在5G/SINS组合导航中的应用研究[J]. 武汉大学学报(信息科学版), 2024, 49(5): 747-755.
	LUO Yarong, GUO Wenfei, QI Shufeng, et al. Invariant RTS smoother and its application in 5G/SINS integrated navigation system[J]. Geomatics and Information Science of Wuhan University, 2024, 49(5): 747-755.
[10]	LUO Yarong, GUO Chi, CHEN Yichao. Filter and piecewise smoother on the matrix Lie group[J]. GPS Solutions, 2023, 27(4): 163.
[11]	李昕, 孟硕林, 黄观文, 等. GNSS/SINS定位稳健SE₂(3)-EKF方法[J]. 测绘学报, 2023, 52(10): 1640-1649. DOI:. doi: 10.11947/j.AGCS.2023.20220526
	LI Xin, MENG Shuolin, HUANG Guanwen, et al. Robust GNSS/SlNS positioning based on the SE₂(3)-EKF framework[J]. Acta Geodaetica et Cartographica Sinica, 2023, 52(10): 1640-1649. DOI:. doi: 10.11947/j.AGCS.2023.20220526
[12]	崔加瑞, 张礼廉, 王茂松, 等. 基于改进李群卡尔曼滤波的惯性/里程计组合导航方法[J]. 控制理论与应用, 2023, 40(12): 2179-2186.
	CUI Jiarui, ZHANG Lilian, WANG Maosong, et al. An improved LG-EKF for SINS/ODO integrated navigation[J]. Control Theory & Applications, 2023, 40(12): 2179-2186.
[13]	郭迟, 陈毅超, 罗亚荣. 一种世界坐标系下的GNSS/SINS松组合导航矩阵李群滤波算法[J]. 中国惯性技术学报, 2024, 32(3): 242-249.
	GUO Chi, CHEN Yichao, LUO Yarong. Matrix Lie group filtering algorithm for GNSS/SINS loosely integrated navigation in the world frame[J]. Journal of Chinese Inertial Technology, 2024, 32(3): 242-249.
[14]	LEE W, GENEVA P, YANG Yulin, et al. Tightly-coupled GNSS-aided visual-inertial localization[C]//Proceedings of 2022 International Conference on Robotics and Automation. Philadelphia: IEEE, 2022: 9484-9491.
[15]	LIU Changwu, JIANG Chen, WANG Haowen. InGVIO: a consistent invariant filter for fast and high-accuracy GNSS-visual-inertial odometry[J]. IEEE Robotics and Automation Letters, 2023, 8(3): 1850-1857.
[16]	BARRAU A, BONNABEL S. Intrinsic filtering on Lie groups with applications to attitude estimation[J]. IEEE Transactions on Automatic Control, 2015, 60(2): 436-449.
[17]	VAN GOOR P, HAMEL T, MAHONY R. Equivariant filter (EqF): a general filter design for systems on homogeneous spaces[C]//Proceedings of 2020 IEEE Conference on Decision and Control. Jeju: IEEE, 2020: 5401-5408.
[18]	VAN GOOR P, HAMEL T, MAHNOY R. Equivariant filter (EqF)[J]. IEEE Transactions on Automatic Control, 2023, 68(6): 3501-3512.
[19]	MAHONY R, VAN GOOR P, HAMEL T. Observer design for nonlinear systems with equivariance[J]. Annual Review of Control, Robotics, and Autonomous Systems, 2022, 5: 221-252.
[20]	VAN GOOR P, MAHONY R. An equivariant filter for visual inertial odometry[C]//Proceedings of 2021 IEEE International Conference on Robotics and Automation. Xi'an: IEEE, 2021: 14432-14438.
[21]	VAN GOOR P, MAHONY R, HAMEL T, et al. Constructive observer design for visual simultaneous localisation and mapping[J]. Automatica, 2021, 132: 109803.
[22]	MAHONY R, TRUMPF J. Equivariant filter design for kinematic systems on Lie groups[J]. IFAC-PapersOnLine, 2021, 54(9): 253-260.
[23]	LUO Yarong, GUO Chi, LIU Jingnan. Equivariant filtering framework for inertial-integrated navigation[J]. Satellite Navigation, 2021, 2(1): 30.
[24]	NG Y, VAN GOOR P, HAMEL T, et al. Equivariant observers for second-order systems on matrix Lie groups[J]. IEEE Transactions on Automatic Control, 2023, 68(4): 2468-2474.
[25]	BARRAU A, BONNABEL S. The geometry of navigation problems[J]. IEEE Transactions on Automatic Control, 2023, 68(2): 689-704.
[26]	FORNASIER A, NG Y, MAHONY R, et al. Equivariant filter design for inertial navigation systems with input measurement biases[C]//Proceeding of 2022 International Conference on Robotics and Automation. Philadelphia: IEEE, 2022: 4333-4339.
[27]	FORNASIER A, NG Y, BROMMER C, et al. Overcoming bias: equivariant filter design for biased attitude estimation with online calibration[J]. IEEE Robotics and Automation Letters, 2022, 7(4): 12118-12125.
[28]	杨元喜. 抗差估计理论及其应用[M]. 北京: 八一出版社, 1993: 107-110.
	YANG Yuanxi. Robust estimation theory and application[M]. Beijing: Bayi Press, 1993: 107-110.
[29]	AGAMENNONI G, NIETO J I, NEBOT E M. Approximate inference in state-space models with heavy-tailed noise[J]. IEEE Transactions on Signal Processing, 2012, 60(10): 5024-5037.
[30]	CHANG Guobin. Robust Kalman filtering based on Mahalanobis distance as outlier judging criterion[J]. Journal of Geodesy, 2014, 88(4): 391-401.
[31]	BARRAU A, BONNABEL S. Linear observed systems on groups[J]. Systems & Control Letters, 2019, 129: 36-42.
[32]	REN Xia, YANG Yuanxi. Development of comprehensive PNT and resilient PNT[J]. Journal of Geodesy and Geoinformation Science, 2023, 6(3): 1-8.
[33]	TANG Hailiang, ZHANG Tisheng, NIU Xiaoji, 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.
[34]	HSU L T, KUBO N, WEN Weisong, et al. UrbanNav: an open-sourced multisensory dataset for benchmarking positioning algorithms designed for urban areas[C]//Proceedings of the 34th International Technical Meeting of the Satellite Division of The Institute of Navigation. St. Louis: Institute of Navigation, 2021: 226-256.

方法	横滚角	俯仰角	航向角
传统EKF	0.640 2	0.751 5	18.960 4
右不变EKF	0.336 1	0.356 2	10.969 3
本文算法	0.344 7	0.324 2	10.252 2

方法	北向	东向	垂向	3D
传统EKF	4.485 4	1.747 3	9.773 5	10.895
右不变EKF	4.481 2	1.748 1	9.772 9	10.893
本文算法	4.484 0	1.734 1	9.773 0	10.891

方法	北向	东向	垂向	3D
传统EKF	0.561 5	1.382 8	3.462 3	3.770
右不变EKF	0.566 8	1.389 3	3.453 6	3.766
本文算法	0.615 7	1.384 4	3.435 0	3.754