Left-handed symmetry equivariant filtering model and algorithm for GNSS/INS integrated navigation

doi:10.11947/j.AGCS.2025.20240473

Abstract

Abstract:

The current equivariant filtering theory is mainly built for local observation in the field of robotics, and cannot be naturally applied to inertial based integrated navigation systems with global observation. To solve the above problems, this paper constructs an equivariant filtering framework which is suitable for global observation through left-handed symmetry, and applies it to GNSS/SINS integrated navigation system which includes gyroscope bias and accelerometer bias states. Different from the left invariant extended Kalman filtering (EKF) on matrix Lie group, firstly, the left-handed symmetry is used to construct the equivariant filtering. Secondly, a two-frame group is employed to derive the linearized error dynamic matrix and the noise driven matrix of equivariant error for a biased inertial navigation system. Finally, a Cartan-Schouten connection is used for covariance parallel transport to complete the curvature correction on the manifold. The experimental results show that the proposed filtering algorithm has better transient response under different large misalignment angles compared with left invariant EKF. Additionally, when the initial yaw error is 90°, the average position error of the proposed filter reduces by 36% compared to left invariant EKF. At the same time, the integrated navigation system can effectively improve the robustness of the filter after curvature correction, and compared to the proposed filter without curvature correction, the average position error reduces by 14% after curvature correction.

Key words: left-handed symmetry, group action, equivariant system, two-frame group, GNSS/SINS integrated navigation, curvature correction

CLC Number:

P228

Yarong LUO, Wentao LU, Chi GUO, Jingnan LIU. Left-handed symmetry equivariant filtering model and algorithm for GNSS/INS integrated navigation[J]. Acta Geodaetica et Cartographica Sinica, 2025, 54(8): 1389-1403.

Figures/Tables 14

Fig. 1

Fig. 2

Fig. 3

Fig. 4

Fig. 5

Fig. 6

Fig. 7

Fig. 8

Fig. 9

Fig. 10

Fig. 11

Fig. 12

Fig. 13

Tab. 1

References 33

[1]	BARRAU A, BONNABEL S. The invariant extended Kalman filter as a stable observer[J]. IEEE Transactions on Automatic Control, 2017, 62(4): 1797-1812.
[2]	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.
[3]	VAN GOOR P, HAMEL T, MAHNOY R. Equivariant filter (EqF)[J]. IEEE Transactions on Automatic Control, 2023, 68(6): 3501-3512.
[4]	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.
[5]	BARRAU A, BONNABEL S. Linear observed systems on groups[J]. Systems & Control Letters, 2019, 129: 36-42.
[6]	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 Measurement, 2024, 73: 3329103.
[7]	李昕, 孟硕林, 黄观文, 等. 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
[8]	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.
[9]	MAHNOY 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.
[10]	MAHONY R, VAN GOOR P, HENEIN M, et al. Equivariant visual odometry in the wild[C]//Proceedings of the 59th IEEE Conference on Decision and Control. Jeju: IEEE, 2020: 1314-1319.
[11]	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.
[12]	CHEN Zhenhang, MIAO Zhiqiang, WANG Chuancheng, et al. Robust stereo VIO based on an equivariant filter for complex lighting environments[C]//Proceedings of 2022 IEEE International Conference on Robotics and Biomimetics. Jinghong: IEEE, 2022: 2128-2133.
[13]	胡建朗, 罗亚荣, 郭迟, 等. 一种结合等变滤波和视觉观测网络的混合视觉惯性里程计[J]. 导航定位与授时, 2024, 11(6): 28-46.
	HU Jianlang, LUO Yarong, GUO Chi, et al. A hybrid visual-inertial odometry combining equivariant filter and visual observation network[J]. Navigation Positioning and Timing, 2024, 11(6): 28-46.
[14]	TAO A, LUO Y, XIA C, et al. Equivariant filter for tightly coupled LiDAR-inertial odometry[C]//Proceedings of 2025 IEEE International Conference on Robotics and Automation. Atlanta: IEEE, 2025.
[15]	VAN GOOR P, MAHONY R, HAMEL T, et al. Constructive observer design for visual simultaneous localisation and mapping[J]. Automatica, 2021, 132: 109803.
[16]	FORNASIER A, VAN GOOR P, ALLAK E, et al. MSCEqF: a multi state constraint equivariant filter for vision-aided inertial navigation[J]. IEEE Robotics and Automation Letters, 2024, 9(1): 731-738.
[17]	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.
[18]	LUO Y, GUO C, LIU J. Equivariant filtering framework for inertial-integrated navigation[J]. Satellite Navigation, 2023, 2(1): 30.
[19]	FORNASIER A, NG Y, MAHONY R, et al. Equivariant filter design for inertial navigation systems with input measurement biases[C]//Proceedings of 2022 International Conference on Robotics and Automation. Philadelphia: IEEE, 2022: 4333-4339.
[20]	FORNASIER A, GE Yixiao, VAN GOOR P, et al. An equivariant approach to robust state estimation for the ArduPilot autopilot system[C]//Proceedings of 2024 IEEE International Conference on Robotics and Automation. Yokohama: IEEE, 2024: 11956-11962.
[21]	LUO Yarong, CHEN Yichao, TAO Anbo, et al. Left equivariant filter with autonomous error dynamics for inertial integrated navigation systems with biases[C]//Proceedings of the 36th Chinese Control and Decision Conference. Xi'an: IEEE, 2024: 341-348.
[22]	BARRAU A, BONNABEL S. The geometry of navigation problems[J]. IEEE Transactions on Automatic Control, 2023, 68(2): 689-704.
[23]	罗亚荣, 郭迟, 欧阳威, 等. 顾及零偏几何性质的GNSS/SINS组合导航方法[J]. 测绘学报, 2025, 54(1): 26-39. DOI: . doi: 10.11947/j.AGCS.2025.20240232
	LUO Yarong, GUO Chi, OUYANG Wei, et al. GNSS/SINS integrated navigation method considering the geometric property of biases state[J]. Acta Geodaetica et Cartographica Sinica, 2025, 54(1): 26-39. DOI: . doi: 10.11947/j.AGCS.2025.20240232
[24]	MUELLER M W, HEHN M, D'ANDREA R. Covariance correction step for Kalman filtering with an attitude[J]. Journal of Guidance, Control, and Dynamics, 2016, 40(9): 2301-2306.
[25]	GILL R, MUELLER M W, D'ANDREA R. Full-order solution to the attitude reset problem for Kalman filtering of attitudes[J]. Journal of Guidance, Control, and Dynamics, 2020, 43(7): 1232-1246.
[26]	MARKLEY F L, CHENG Yang, CRASSIDIS J L, et al. Error-covariance reset in the multiplicative extended Kalman filter for attitude estimation[J]. Journal of Guidance, Control, and Dynamics, 2023, 46(10): 1860-1873.
[27]	GE Yixiao, VAN GOOR P, MAHONY R. Equivariant filter design for discrete-time systems[C]//Proceedings of 2022 IEEE Conference on Decision and Control. Cancun: IEEE, 2022: 1243-1250.
[28]	GE Yixiao, VAN GOOR P, MAHONY R. A note on the extended Kalman filter on a manifold[C]//Proceedings of 2023 IEEE Conference on Decision and Control. Singapore: IEEE, 2023: 7687-7694.
[29]	GE Yixiao, VAN GOOR P, MAHONY R. A geometric perspective on fusing Gaussian distributions on Lie groups[J]. IEEE Control Systems Letters, 2024, 8: 844-849.
[30]	PENNEC X. Bi-invariant means on Lie groups with cartan-Schouten connections[M]//Geometric Science of Information. Berlin: Springer, 2013: 59-67.
[31]	COGLIATI A, MASTROLIA P. Cartan, Schouten and the search for connection[J]. Historia Mathematica, 2018, 45(1): 39-74.
[32]	LOIANNO G, WATTERSON M, KUMAR V. Visual inertial odometry for quadrotors on SE(3)[C]//Proceedings of 2016 IEEE International Conference on Robotics and Automation. Stockholm: IEEE, 2016: 1544-1551.
[33]	CALINON S. Gaussians on Riemannian manifolds: applications for robot learning and adaptive control[J]. IEEE Robotics & Automation Magazine, 2020, 27(2): 33-45.

方法	北向	东向	垂向	2D	3D
TFG	3.01	2.42	2.59	2.71	2.67
L	3.46	3.36	3.60	3.41	3.47
L-GM	3.45	2.30	2.94	2.88	2.90
Eq-L	2.98	2.32	2.48	2.65	2.59
Eq-L-GM	2.48	1.81	2.36	2.15	2.22