Geometric filters are known to be highly sensitive to model mismatch — a critical limitation in real-world applications. To address this issue, this paper introduces a novel filtering framework grounded in linear constraint enforcement. We begin by analyzing how model mismatch propagates through the filtering process in group-affine systems, revealing the fundamental mechanism behind error accumulation in conventional geometric filters such as the invariant extended Kalman filter. Building on this insight, we design a constrained filtering strategy that actively regulates the gain matrix along the direction of model deviation, thereby suppressing error growth while preserving the intrinsic geometric structure of the underlying Lie group manifold. Crucially, the proposed method achieves this enhanced robustness without sacrificing computational efficiency, making it suitable for real-time deployment. Finally, simulations and experiments on an integrated inertial navigation and global navigation satellite system (INS/GNSS) demonstrate that the proposed approach achieves substantial improvements in estimation accuracy compared to the standard InEKF, offering an effective solution for robust filtering in complex environments.
[1]BARRAU A, BONNABEL S.The geometry of navigation problems[J].IEEE Transactions on Automatic Control, 2022, 68(2):689-704
[2]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
[3]FU M, LU X, JIN Y, et al.Semantic map-based visual localization with consistency guarantee[J].IEEE Sensors Journal, 2023, 24(1):1065-1078
[4]LEE T.Bayesian attitude estimation with the matrix Fisher distribution on SO(3)[J].IEEE Transactions on Automatic Control, 2018, 63(10):3377-3392
[5]WANG W, LEE T.INS-GNSS Navigation for Large Attitude Uncertainties with the Matrix Fisher–Gaussian Distribution[C]//2023 American Control Conference (ACC). 2023: 4858-4863.
[6]BARRAU A, BONNABEL S.The invariant extended Kalman filter as a stable observer[J].IEEE Transactions on Automatic Control, 2016, 62(4):1797-1812
[7]BARRAU A, BONNABEL S.Invariant kalman filtering[J].Annual Review of Control, Robotics, and Autonomous Systems, 2018, 1(1):237-257
[8]VAN GOOR P, HAMEL T, MAHONY R.Equivariant filter (eqf)[J].IEEE Transactions on Automatic Control, 2022, 68(6):3501-3512
[9]JIN Y, ZHANG W A, TANG J, et al.A nonlinear filter for pose estimation based on fast unscented transform on lie groups[J].IEEE Robotics and Automation Letters, 2024, 9(11):10431-10438
[10]杨旭升, 夏晓翠, 金宇强, 等.基于广义最大相关熵准则的几何滤波方法[J].自动化学报, 2025, 51(07):1612-1625
[11]YANG X S, XIA X C, JIN Y Q, et al.Geometric Filtering Method Based on Generalized Maximum Correntropy Criterion[J].Acta Automatica Sinica, 2025, 51(7):1612-1625
[12]Brossard M, Barrau A, Bonnabel S.A code for unscented Kalman filtering on manifolds (UKF-M)[C]//2020 IEEE International Conference on Robotics and Automation (ICRA). 2020: 5701-5708.
[13]JIN Y, ZHANG W A, LU X, et al.Computationally efficient Lie algebraic particle filters for state estimation[J].IEEE Transactions on Automatic Control, 2024, 70(5):3370-3377
[14]崔加瑞, 张礼廉, 王茂松, 等.基于改进李群卡尔曼滤波的惯性里程计组合导航方法[J].控制理论与应用, 2023, 40(12):2179-2186
[15]CUI J R, ZHANG L L, WANG M S, et al.An improved LG-EKF for SINSODO integrated navigation[J].Control Theory & Applications, 2023, 40(12):2179-2186
[16]何友, 刘瑜, 李耀文, 等.多源信息融合发展及展望[J].航空学报, 2025, 46(6):531672-
[17]HE Y, LIU Y, LI Y W, et al.Development and prospects of multisource information fusion[J].Acta Aeronautica et Astronautica Sinica, 2025, 46(6):531672-
[18]DUNíK J, STRAKA O, KOST O, et al.Noise covariance matrices in state - space models: A survey and comparison of estimation methods—Part I[J].International Journal of Adaptive Control and Signal Processing, 2017, 31(11):1505-1543
[19]南子寒, 刘大禹, 董明, 梁文宁, 赵雪薇, 马伊琳, 关瑶.拒止下多源自主导航鲁棒滤波方法[J].航空学报, 2024, 45(S1):730782-
[20]NAN Z H, LIU D Y, DONG M, et al.Robust filtering method for GNSS denied multi-source autonomous navigation[J].Acta Aeronautica et Astronautica Sinica, 2024, 45(S1):730782-
[21]CHEN B, LIU X, ZHAO H, et al.Maximum correntropy Kalman filter[J].Automatica, 2017, 76:70-77
[22]CHANG L, HU B, CHANG G, et al.Huber-based novel robust unscented Kalman filter[J].IET Science, Measurement & Technology, 2012, 6(6):502-509
[23]ZHANG J, YANG X, ZHANG W A.A progressive Bayesian filtering framework for nonlinear systems with heavy-tailed noises[J].IEEE Transactions on Automatic Control, 2022, 68(3):1918-1925
[24]胡玉梅, 潘泉, 邓豹.基于信息的传感器航迹自适应滤波算法[J].航空学报, 2024, 45(20):629825-
[25]HU Y M, PAN Q, DENG B.A Fisher information based adaptive filtering algorithm for sensor trajectory planning[J].Acta Aeronautica et Astronautica Sinica, 2024, 45(20):629825-
[26]TEIXEIRA B O S, CHANDRASEKAR J, PALANTHANDALAM-MADAPUSI H J, et al.Gain-constrained Kalman filtering for linear and nonlinear systems[J].IEEE Transactions on Signal Processing, 2008, 56(9):4113-4123
[27]HRUSTIC E, BEN ABDALLAH R, VILà‐VALLS J, et al.Robust linearly constrained extended Kalman filter for mismatched nonlinear systems[J].International Journal of Robust and Nonlinear Control, 2021, 31(3):787-805
[28]BOURMAUD G, MéGRET R, ARNAUDON M, et al.Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions[J].Journal of Mathematical Imaging and Vision, 2015, 51:209-228
CJA