GNSS/INS组合导航左手对称性等变滤波模型及算法

doi:10.11947/j.AGCS.2025.20240473

测绘学报 ›› 2025, Vol. 54 ›› Issue (8): 1389-1403.doi: 10.11947/j.AGCS.2025.20240473

GNSS/INS组合导航左手对称性等变滤波模型及算法

罗亚荣¹(), 卢文韬², 郭迟¹^,³(), 刘经南¹

^1.武汉大学卫星导航定位技术研究中心，湖北　武汉　430079
^2.武汉大学电子信息学院，湖北　武汉　430079
^3.湖北珞珈实验室，湖北　武汉　430079

收稿日期:2024-11-21 修回日期:2025-07-07 出版日期:2025-09-16 发布日期:2025-09-16
通讯作者: 郭迟 E-mail:yarongluo@whu.edu.cn;guochi@whu.edu.cn
作者简介:罗亚荣（1993—），男，博士，助理研究员，研究方向为几何状态估计理论及应用。E-mail：yarongluo@whu.edu.cn
基金资助:
国家自然科学基金(42404025);中国博士后科学基金(2023TQ0248);湖北省重大科技专项(2022AAA009)

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

Yarong LUO¹(), Wentao LU², Chi GUO¹^,³(), Jingnan LIU¹

^1.GNSS Research Center, Wuhan University, Wuhan 430079, China
^2.School of Electronic Information, Wuhan University, Wuhan 430079, China
^3.Hubei Luojia Laboratory, Wuhan 430079, China

Received:2024-11-21 Revised:2025-07-07 Online:2025-09-16 Published:2025-09-16
Contact: Chi GUO E-mail:yarongluo@whu.edu.cn;guochi@whu.edu.cn
About author:LUO Yarong (1993—), male, PhD, assistant researcher, majors in geometric state estimation theory and applications. E-mail: yarongluo@whu.edu.cn
Supported by:
The National Natural Science Foundation of China(42404025);China Postdoctoral Science Foundation(2023TQ0248);Major Science and Technology Project of Hubei Province(2022AAA009)

摘要/Abstract

摘要：

当前等变滤波理论主要面向机器人领域的局部观测而构建，无法自然地应用于具有全局观测的惯性基组合导航系统。针对上述问题，本文通过左手对称性构建一个适合全局观测的等变滤波框架，并将其应用于处理包含陀螺零偏和加速度计零偏状态的GNSS/INS组合导航系统。与矩阵李群上的左不变扩展卡尔曼滤波不同，首先，本文利用左手对称性构建等变滤波；其次，利用一个双坐标系群（two-frame group，TFG）推导了顾及零偏状态惯性导航系统上的等变误差动力学矩阵和噪声驱动矩阵；最后，利用一个Cartan-Schouten联络进行协方差平行传输从而完成流形上的曲率修正。试验结果表明：相对于左不变扩展卡尔曼滤波，本文算法在不同大失准角情况下具有较好的瞬态响应，且初始航向角误差为90°时，本文算法平均定位误差降低了36%；同时，组合导航系统在进行曲率修正后有效地提升了滤波稳健性，并且相较于不进行曲率修正的本文算法，其在进行曲率修正后，平均定位误差降低了14%。

关键词: 左手对称性, 群作用, 等变系统, 双坐标系群, GNSS/SINS组合导航, 曲率修正

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

中图分类号:

P228

罗亚荣, 卢文韬, 郭迟, 刘经南. GNSS/INS组合导航左手对称性等变滤波模型及算法[J]. 测绘学报, 2025, 54(8): 1389-1403.

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.

图/表 14

图1

图2

图3

图4

图5

图6

图7

图8

图9

图10

图11

图12

图13

表1

参考文献 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

GNSS/INS组合导航左手对称性等变滤波模型及算法

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

RichHTML

PDF

可视化

摘要/Abstract

引用本文

使用本文

图/表 14

参考文献 33

相关文章 1

编辑推荐

Metrics

本文评价