-
摘要: 本文对流形上的几何状态估计方法进行全面的梳理. 首先介绍流形上一般滤波器设计的思路, 然后从保持几何性质的观测器角度讨论流形空间中滤波器设计的思路以及带来的几何性质优势.流形上的几何状态估计器设计主要包括不变扩展卡尔曼滤波和等变滤波两个阶段.流形上的几何状态估计方法提供了一种通用的理论框架来进行滤波器设计, 通过灵活的群构造和群作用的选择, 可以针对不同观测类型设计保持系统几何结构的滤波器.在利用系统几何结构的基础上可以从根本上解决传统滤波器存在的不一致性问题, 并有望减少线性化误差, 从而提高了状态估计的精度和鲁棒性.最后本文对未来发展方向进行了展望.Abstract: This paper provides a comprehensive review of the methods for estimating geometric states on manifolds. Firstly, the design ideas of general filters on manifolds are introduced. Then, the design ideas of filters in manifold spaces and the resulting superior geometric properties are discussed from the perspective of observers that maintain geometric properties. The design of geometric state estimators on manifolds mainly includes two stages: Invariant extended Kalman filtering and equivariant filtering. The geometric state estimation method on manifolds provides a universal theoretical framework for filter design. Through flexible selection of group construction and group action, filters that maintain the system＇s geometric structure can be designed for different observation types. Based on the utilization of the system＇s geometric structure, the inconsistency problem existing in traditional filters can be fundamentally solved, and it is expected to reduce linearization errors, thereby improving the accuracy and robustness of state estimation. Finally, the future development directions are prospected.
-
图 1 流形上状态估计器设计方法研究脉络图
Fig. 1 Research context diagram of state estimator design method on manifolds
图 3 矩阵李群−李代数−欧氏空间映射关系示意图
Fig. 3 Schematic diagram of the mapping relationship among Lie groups, Lie algebras and Euclidean Spaces
图 4 李群、李代数与向量空间之间的指数/对数映射关系
Fig. 4 Exponential and logarithmic mapping relationship among Lie group, Lie algebra, and vector space
图 5 伴随作用在指数映射下的交换图
Fig. 5 Commutative diagram of adjoint actions under the exponential map
图 8 李群上的中心高斯分布示意图
Fig. 8 Schematic diagram of the central Gaussian distribution on the Lie groups
图 12 不变系统在左群作用和右群作用下的交换图
Fig. 12 Commutative diagram of invariant systems under left and right group actions
图 13 等变系统在左群作用和右群作用下的交换图
Fig. 13 Commutative diagram of equivariant systems under left and right group actions
图 14 左/右等变提升的伴随作用交换图
Fig. 14 Commutative diagram of left and right equivariant lifts under Adjoint action
图 16 系统输出在左/右群作用下的不变性交换图
Fig. 16 Commutative diagram of system output invariance under left and right group actions
图 17 系统输出在左/右群作用下的等变性交换图
Fig. 17 Commutative diagram of system output equivariance under left and right group actions
图 18 状态空间和输出空间上的局部坐标图和线性化
Fig. 18 The local coordinate chart and linearization on state space and output space
图 19 平行传输示意图. 其中$ {\mathit{\boldsymbol{ v }}}_{||} $表示$ \xi $点处切向量$ {\mathit{\boldsymbol{ v }}} $沿曲线平行移动到点$ \xi' $处的切向量, $ \Gamma $表示平行传输算符.
Fig. 19 Parallel transport schematic. Here, $ {\mathit{\boldsymbol{ v }}}_{||} $ represents the tangent vector at point $ \xi' $ obtained by parallel transporting the tangent vector $ {\mathit{\boldsymbol{ v }}} $ from point $ \xi $ along a curve, and $ \Gamma $ denotes the parallel transport operator
表 1 基于左不变误差和右不变误差的InEKF比较
Table 1 Comparisons of InEKF based on left invariant error and right invariant error
左不变误差 右不变误差 误差定义 $ \eta_t^l=X^{-1}\hat{X}=\exp_{ \mathbf{G}}({\zeta_t^l}^{\wedge}) $, $ \zeta_t^l\sim \mathcal{N}_{\mathbb{R}^n}({0\mathit{\boldsymbol{}}},\;\boldsymbol \Sigma^l) $ $ \eta_t^r=\hat{X}X^{-1} =\exp_{ \mathbf{G}}({\zeta_t^r}^{\wedge}) $, $ \zeta_t^r\sim \mathcal{N}_{\mathbb{R}^n}({0\mathit{\boldsymbol{}}},\;\boldsymbol \Sigma^r) $ 中心高斯分布 $ \hat{X}=X\exp_{ \mathbf{G}}({\zeta_t^l}^{\wedge})\sim \mathcal{N}_{ \mathbf{G}}^l(X,\;\boldsymbol\Sigma^l) $ $ \hat{X}=\exp_{ \mathbf{G}}({\zeta_t^r}^{\wedge})X\sim \mathcal{N}_{ \mathbf{G}}^r(X,\;\boldsymbol\Sigma^r) $ 误差状态转换 $ \zeta_t^l=\text{Ad}_{\hat{X}^{-1}}\zeta_t^r $ $ \zeta_t^r=\text{Ad}_{\hat{X}}\zeta_t^l $ 一步预测过程 $ \dfrac{\mathrm{d}}{\mathrm{d} t} \hat{X}={{\mathit{\boldsymbol{ f }}}}_{{\mathit{\boldsymbol{ v }}}_t}(\hat{X}) $, $ \dfrac{\mathrm{d}}{\mathrm{d} t} \hat{\boldsymbol\Sigma}^l={\mathit{\boldsymbol{ A }}}_t^l\hat{\boldsymbol\Sigma}^l+\hat{\boldsymbol\Sigma}^l{\mathit{\boldsymbol{ A }}}_t^l+{\mathit{\boldsymbol{ Q }}}_t $ $ \dfrac{\mathrm{d}}{\mathrm{d} t} \hat{X}={{\mathit{\boldsymbol{ f }}}}_{{\mathit{\boldsymbol{ v }}}_t}(\hat{X}) $, $ \dfrac{\mathrm{d}}{\mathrm{d} t} \hat{\boldsymbol\Sigma}^r={\mathit{\boldsymbol{ A }}}_t^r\hat{\boldsymbol\Sigma}^r+\hat{\boldsymbol\Sigma}^r{\mathit{\boldsymbol{ A }}}_t^r+\text{Ad}_{\hat{X}}{\mathit{\boldsymbol{ Q }}}_t\text{Ad}_{\hat{X}}^T $ 误差动力学矩阵的依赖性 $ {\mathit{\boldsymbol{ A }}}_t^l $通常依赖系统输入 $ {\mathit{\boldsymbol{ A }}}_t^r $通常依赖缓变量, 但$ \text{Ad}_{\hat{X}} $依赖系统轨迹 量测模型 $ {\bf{y}}^l=X{\bf{d}}+{\bf{n}} $ $ {\bf{y}}^r=X^{-1}{\bf{d}}+{\bf{n}} $ 不变新息 $ {\mathit{\boldsymbol{ V }}}^l=\hat{X}^{-1}({\bf{y}}^l-\hat{\bf{y}}^l) $ $ {\mathit{\boldsymbol{ V }}}^r=\hat{X}({\bf{y}}^r-\hat{\bf{y}}^r) $ 新息协方差 $ {\mathit{\boldsymbol{ S }}}^l={\mathit{\boldsymbol{ H }}}^l\boldsymbol\Sigma^l{{\mathit{\boldsymbol{ H }}}^l}^T+{\mathit{\boldsymbol{ D }}}^{-1}{\mathit{\boldsymbol{ R }}} {\mathit{\boldsymbol{ D }}}^{-T} $ $ {\mathit{\boldsymbol{ S }}}^r={\mathit{\boldsymbol{ H }}}^r\boldsymbol\Sigma^r{{\mathit{\boldsymbol{ H }}}^r}^T+{\mathit{\boldsymbol{ D }}}{\mathit{\boldsymbol{ R }}}{\mathit{\boldsymbol{ D }}}^T $ 卡尔曼滤波增益 $ {\mathit{\boldsymbol{ K }}}^l=\boldsymbol\Sigma^l {{\mathit{\boldsymbol{ H }}}^l}^T {{\mathit{\boldsymbol{ S }}}^l}^{-1} $ $ {\mathit{\boldsymbol{ K }}}^l=\boldsymbol\Sigma^r {{\mathit{\boldsymbol{ H }}}^r}^T {{\mathit{\boldsymbol{ S }}}^r}^{-1} $ 量测更新 $ \hat{X}^{+}=\hat{X}^{-}\exp_{ \mathbf{G}}(({\mathit{\boldsymbol{ K }}}^l {\mathit{\boldsymbol{ V }}}^l)^{\wedge}) $ $ \hat{X}^{+}=\exp_{ \mathbf{G}}(({\mathit{\boldsymbol{ K }}}^r {\mathit{\boldsymbol{ V }}}^r)^{\wedge})\hat{X}^{-} $ 后验协方差更新 $ \hat{\boldsymbol\Sigma}^l\leftarrow ({\mathit{\boldsymbol{ I }}}-{\mathit{\boldsymbol{ K }}}^l{\mathit{\boldsymbol{ H }}}^l)\hat{\boldsymbol\Sigma}^l $ $ \hat{\boldsymbol\Sigma}^r\leftarrow ({\mathit{\boldsymbol{ I }}}-{\mathit{\boldsymbol{ K }}}^r{\mathit{\boldsymbol{ H }}}^r)\hat{\boldsymbol\Sigma}^r $ 
下载: 导出CSV
表 2 基于作群作用和右群作用的等变滤波对比
Table 2 Comparisons of EqF based on left group action and right group action
左群作用 右群作用 群作用 $ \phi(X,\;\phi(Y,\;\xi))=\phi(XY,\;\xi) $ $ \phi(X,\;\phi(Y,\;\xi))=\phi(YX,\;\xi) $ 等变提升 $ \text{Ad}_{X}^{-1}\Lambda(\phi(X,\;\xi),\;\psi(X,\;{\mathit{\boldsymbol{ v }}}))=\Lambda(\xi,\;{\mathit{\boldsymbol{ v }}}) $ $ \text{Ad}_{X}\Lambda(\phi(X,\;\xi),\;\psi(X,\;{\mathit{\boldsymbol{ v }}}))=\Lambda(\xi,\;{\mathit{\boldsymbol{ v }}}) $ 等变提升的系统 $ \dot{X}=dR_{X}\Lambda(\phi(X,\;\xi^{\circ}),\;{\mathit{\boldsymbol{ v }}}) $ $ \dot{X}=dL_{X}\Lambda(\phi(X,\;\xi^{\circ}),\;{\mathit{\boldsymbol{ v }}}) $ 李群上的误差 $ E=\hat{X}^{-1}{X} $ $ E={X}\hat{X}^{-1} $ 误差动力学 $ \dot{E}={\bf{f}}_{{\mathit{\boldsymbol{ v }}}^{\circ}}(E)-dR_E {\bf{f}}_{{\mathit{\boldsymbol{ v }}}^{\circ}}(\mathrm{id}) $ $ \dot{E}={\bf{f}}_{{\mathit{\boldsymbol{ v }}}^{\circ}}(E)-dL_E {\bf{f}}_{{\mathit{\boldsymbol{ v }}}^{\circ}}(\mathrm{id}) $ 噪声驱动矩阵 $ {\mathit{\boldsymbol{ B }}}_t= \mathrm{D}_e|_{\xi^{\circ}}\vartheta(e) \mathrm{D}_{\eta_t}|_{ \mathrm{id}}\phi_{\xi^{\circ}}(\eta_t)\text{Ad}_{\hat{X}^{-1}}[\mathrm{D}_{{\mathit{\boldsymbol{ v }}}}|_{{\mathit{\boldsymbol{ v }}}_{m}}\Lambda(\hat{\xi},\;{\mathit{\boldsymbol{ v }}})] $ $ {\mathit{\boldsymbol{ B }}}_t= \mathrm{D}_e|_{\xi^{\circ}}\vartheta(e) \mathrm{D}_{\eta_t}|_{ \mathrm{id}}\phi_{\xi^{\circ}}(\eta_t)\text{Ad}_{\hat{X}}[\mathrm{D}_{{\mathit{\boldsymbol{ v }}}}|_{{\mathit{\boldsymbol{ v }}}_{m}}\Lambda(\hat{\xi},\;{\mathit{\boldsymbol{ v }}})] $ 等变输出 $ {\mathit{\boldsymbol{ C }}}_t^{\circ}= \mathrm{D}_{{\mathit{\boldsymbol{ y }}}}|_{{\mathit{\boldsymbol{ y }}}^{\circ}}\delta ({\mathit{\boldsymbol{ y }}})\cdot \mathrm{D}_e|_{\xi^{\circ}}h(e) \cdot \mathrm{D}_{\varepsilon}|_{{0\mathit{\boldsymbol{}}}}\vartheta^{-1}(\varepsilon) $ $ {\mathit{\boldsymbol{ C }}}_t^{\circ}= \mathrm{D}_{{\mathit{\boldsymbol{ y }}}}|_{{\mathit{\boldsymbol{ y }}}^{\circ}}\delta ({\mathit{\boldsymbol{ y }}})\cdot \mathrm{D}_e|_{\xi^{\circ}}h(e) \cdot \mathrm{D}_{\varepsilon}|_{{0\mathit{\boldsymbol{}}}}\vartheta^{-1}(\varepsilon) $ 通用输出 $ {\mathit{\boldsymbol{ C }}}_t= \mathrm{D}_{{\mathit{\boldsymbol{ y }}}}|_{\hat{{\mathit{\boldsymbol{ y }}}}}\delta({\mathit{\boldsymbol{ y }}}) \mathrm{D}_{\xi}|_{\hat{\xi}} h(\xi) \mathrm{D}_e|_{\xi^{\circ}}\phi_{\hat{X}}(e) \mathrm{D}_{\varepsilon}|_{{0\mathit{\boldsymbol{}}}} \vartheta^{-1}(\varepsilon) $ $ {\mathit{\boldsymbol{ C }}}_t= \mathrm{D}_{{\mathit{\boldsymbol{ y }}}}|_{\hat{{\mathit{\boldsymbol{ y }}}}}\delta({\mathit{\boldsymbol{ y }}}) \mathrm{D}_{\xi}|_{\hat{\xi}} h(\xi) \mathrm{D}_e|_{\xi^{\circ}}\phi_{\hat{X}}(e) \mathrm{D}_{\varepsilon}|_{{0\mathit{\boldsymbol{}}}} \vartheta^{-1}(\varepsilon) $ 量测更新 $ \hat{X}^+=\hat{X}^-\exp_{ \mathbf{G}}(\Delta) $ $ \hat{X}^+=\exp_{ \mathbf{G}}(\Delta)\hat{X}^- $
下载: 导出CSV
表 3 EKF、InEKF和EqF对比
Table 3 Comparisons of EKF, InEKF, and EqF
维度 扩展卡尔曼滤波 不变扩展卡尔曼滤波 等变滤波 核心思想 局部线性化. 在状态估计点附近对非线性函数进行一阶泰勒展开, 忽略高阶项 群仿射性诱导的线性化. 在李群单位元处对不变误差进行线性化, 通过对数线性性质获得无线性化误差并且自治的动力学方程 结构保持的滤波器设计. 将等变性作为滤波器设计的首要约束, 寻求在整个状态空间上保持系统几何结构的滤波方程 系统假设 状态空间位于欧式空间$ \mathbb{R}^n $, 系统可微, 对对称性无要求 状态空间位于李群, 系统动力学和观测模型在某个李群作用下是不变的 状态空间位于齐次流形, 包括李群. 需要考虑系统上的群的构造以及群作用的选择 适用场景 适用非线性系统, 但在强非线性场景下性能退化严重 惯性基组合导航系统、VIO、LIO等具有显式几何结构的状态估计问题 除了InEKF涵盖的场景外, 也适用于齐次流形上的状态估计, 适用范围更广. 可以自然地将零偏、外参、内参等待估状态纳入系统并探索几何性质 一致性与鲁棒性 一致性差: 线性化误差导致估计协方差常过于自信, 易发散. 对初始误差和线性化点敏感 一致性显著提升: 不变误差动力学的自治性使得协方差预测更贴近真实误差. 对大初始误差更鲁棒, 收敛域更大 一致性显著提升: 系统在状态对称变换和输入对称变换下等变, 这是收敛性的理论保证 计算复杂度 计算复杂度低. 为标准EKF计算, 涉及雅可比矩阵计算 与EKF相当. 主要区别在于雅可比矩阵不依赖于线性化点时可能减少计算量 依赖于具体实现. 其理论框架不指定具体算法, 实现时可能从简单到复杂 主要局限 线性化误差不可控, 在非向量空间上表述不自然等 要求系统具有严格的群仿射性, 对于不完美群仿射系统, 其性能可能下降 数学抽象程度高, 对于不具有等变性的系统, 需要根据具体应用场景分析
下载: 导出CSV
表 4 惯性基组合导航系统等变滤波对比
Table 4 Comparisons of EqF for inertial integrated navigation system
等变滤波 对称群 是否等变 误差类型 特征 应用场景 Fornasier等[160] 切群$ \mathbf{SE}_2(3)\ltimes \mathfrak{se}_2(3) $ 右等变 右等变误差 引入额外的虚拟速度零偏 VIO、LIO Fornasier等[132] 双坐标系群$ \mathbf{SO}(3)\ltimes(\mathbb{R}^6\oplus \mathbb{R}^6) $ 否 右等变误差 $ \mathbf{SO}(3) $分别作用于不同参考坐标系上的矢量 GNSS/INS Barrau等[47] 直积群$ \mathbf{SE}_2(3)\times \mathbb{R}^6 $ 右等变 右等变误差 姿态、速度和位置嵌入$ \mathbf{SE}_2(3) $再与零偏构成直积群 VIO、LIO Fornasier等[132] $ \mathbf{HG}(3)\ltimes \mathfrak{hg}(3)\times\mathbb{R}^3 $ 否 右等变误差 位置采用线性误差, 没有速度零偏输入 GNSS/INS Fornasier等[161] $ \mathbf{SE}_2(3)\ltimes \mathfrak{se}(3) $ 右等变 右等变误差 保持最小状态表示, 不需要引入额外的速度零偏 MSCKF 胡建朗等[165] $ \mathbf{SE}_2(3)\ltimes \mathbb{R}^6 $ 右等变 右等变误差 姿态和零偏嵌入$ \mathbf{SE}_2(3) $再与速度和位置构成直积群 VIO Luo等[133] 直积群$ \mathbf{SE}_2(3)\times \mathbb{R}^6 $ 左等变 左等变误差 姿态、速度和位置嵌入$ \mathbf{SE}_2(3) $再与零偏构成直积群 GNSS/INS 罗亚荣等[134] 双坐标系群 否 左等变误差 $ \mathbf{SO}(3) $分别作用于不同参考坐标系上的矢量 GNSS/INS
下载: 导出CSV
-
[1] 王巍, 陈巍, 孟凡琛. 面向多源自主导航的智能学习方法研究. 中国科学:信息科学, 2024, 54(12): 2778−2793 doi: 10.1360/SSI-2024-0192Wang Wei, Chen Wei, Meng Fanchen. Study on intelligent learning methods for multi-source autonomous navigation. Sci Sin Inform, 2024, 54(12): 2778−2793 doi: 10.1360/SSI-2024-0192 [2] 刘经南, 罗亚荣, 郭迟, 高柯夫. PNT智能与智能PNT. 测绘学报, 2022, 51(6): 811−828 doi: 10.11947/j.AGCS.2022.20220152Liu Jing-Nan, Luo Ya-Rong, Guo Chi, Gao Ke-Fu. PNT intelligence and intelligent PNT. Acta Geodaetica et Cartographica Sinica, 2022, 51(6): 811−828 doi: 10.11947/j.AGCS.2022.20220152 [3] Barrau A, Bonnabel S. Invariant Kalman filtering. Annual Review of Control, Robotics, and Autonomous Systems, 2018, 1: 237−257 doi: 10.1146/annurev-control-060117-105010 [4] Mahnoy R, van Goor P, Hamel T. Observer design for nonlinear systems with equivariance. Annual Review of Control, Robotics, and Autonomous Systems, 2022, 5: 221−252 doi: 10.1146/annurev-control-061520-010324 [5] Salcudean S. A globally convergent angular velocity observer for rigid body motion. IEEE Transactions on Automatic Control, 1991, 36(12): 1493−1497 doi: 10.1109/9.106169 [6] Thienel J, Sanner R M. A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise. IEEE Transactions on Automatic Control, 2003, 48(11): 2011−2015 doi: 10.1109/TAC.2003.819289 [7] Aghannan N, Rouchon P. An intrinsic observer for a class of Lagrangian systems. IEEE Transactions on Automatic Control, 2003, 48(6): 936−945 doi: 10.1109/TAC.2003.812778 [8] Bonnabel S, Martin P, Rouchon P. Symmetry-preserving observers. IEEE Transactions on Automatic Control, 2008, 53(11): 2514−2526 doi: 10.1109/TAC.2008.2006929 [9] Bonnabel S, Martin P, Rouchon P. Non-linear symmetry-preserving observers on Lie groups. IEEE Transactions on Automatic Control, 2009, 54(7): 1709−1713 doi: 10.1109/TAC.2009.2020646 [10] Bonnable S, Martin P, Salaün E. Invariant extended Kalman filter: Theory and application to a velocity-aided attitude estimation problem. In: Proceedings of the 48th IEEE Conference on Decision and Control (CDC) Held Jointly With 2009 28th Chinese Control Conference. Shanghai, China: IEEE, 2007. 1297-1304 [11] Barczyk M, Lynch A F. Invariant observer design for a helicopter UAV aided inertial navigation system. IEEE Transactions on Control System Technology, 2013, 21(3): 791−806 doi: 10.1109/TCST.2012.2195495 [12] Gui H, de Ruiter A H J. Quaternion invariant extended Kalman filtering for spacecraft attitude estimation. Journal of Guidance, Control, and Dynamics, 2018, 41(4): 863−878 doi: 10.2514/1.G003177 [13] Barczyk M. Invariant observer design of attitude and heading reference system. Control Theory Technology, 2019, 17(3): 228−240 doi: 10.1007/s11768-019-8028-6 [14] Phogat K S, Chang D E. Invariant extended Kalman filter on matrix Lie groups. automatica, 2020, 114: 108812 doi: 10.1016/j.automatica.2020.108812 [15] Ng Y, van Goor P, Hamel T, Mahony R. Equivariant systems theory and observer design for second-order systems on matrix Lie groups. In: Proceedings of the 59th IEEE Conference on Decision and Control (CDC). Jeju, Korea (South): IEEE, 2020. 4194-4199 [16] Ng Y, van Goor P, Hamel T, Mahony R. Equivariant observers for second-order systems on matrix Lie groups. IEEE Transactions on Automatic Control, 2023, 68(4): 2468−2474 doi: 10.1109/TAC.2022.3173926 [17] Barrau A, Bonnabel S. Intrinsic filtering on Lie groups with applications to attitude estimation. IEEE Transactions on Automatic Control, 2015, 60(2): 436−449 doi: 10.1109/TAC.2014.2342911 [18] Mahony R, Hamel T, Pflimlin J M. Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, 2008, 53(5): 1203−1218 doi: 10.1109/TAC.2008.923738 [19] Grip H F, Fossen T I, Johansen T A, Saberi. Attitude Estimation Using Biased Gyro and Vector Measurements With Time-Varying Reference Vectors. IEEE Transactions on Automatic Control, 2012, 57(5): 1332−1338 doi: 10.1109/TAC.2011.2173415 [20] Hua M D, Hamel T, Mahony R, Trumpf J. Gradient-like observer design on the Special Euclidean group SE(3) with system outputs on the real projective space. In: Proceedings of the 54th IEEE Conference and Decision and Control (CDC). Osaka, Japan: IEEE, 2015, 2139-2145 [21] Wang M M, Tayebi A. Hybrid pose and velocity-bias estimation on SE(3) using inertial and landmark measurements. IEEE Transactions on Automatic Control, 2018, 64(8): 3399−3406 doi: 10.1109/tac.2018.2879766 [22] Hua M D, Trumpf J, Hamel T, Mahony R, Morin P. Nonlinear observer design on SL(3) for homography estimation by exploiting point and line correspondences with application to image stabilization. automatica, 2020, 115: 108858 doi: 10.1016/j.automatica.2020.108858 [23] Bourmaud G, Mégret R, Giremus A, Berthoumieu Y. Discrete extended Kalman filter on Lie groups. In: Proceddings of the 21th European Signal Processing Conference (EUSIPCO 2013). IEEE, 2013. 1-5. [24] Bourmaud G, Mégret R, Arnaudon M, Giremus A. Continuous-discrete extended Kalman filter on matrix Lie groups using concentrated Gaussian distributions. Journal of Mathematical Imaging and Vision, 2015, 51: 209−228 doi: 10.1007/s10851-014-0517-0 [25] Bourmaud G, Mégret R, Giremus A, Berthoumieu Y. From intrinsic optimization to iterated extended Kalman filtering on Lie groups. Journal of Mathematical Imaging and Vision, 2016, 55(3): 284−303 doi: 10.1007/s10851-015-0622-8 [26] Sjøberg A M, Egeland O. An EKF for lie groups with application to crane load dynamics. Modeling, Identification and Control, 2019, 40(2): 109−124 doi: 10.4173/mic.2019.2.3 [27] Sy L W, Lovell N H, Redmond S J. Estimating lower body kinematics using a Lie group constrained extended Kalman filter and reduced IMU count. IEEE Sensors Journal, 2021, 21(18): 20969−20979 doi: 10.1109/JSEN.2021.3096078 [28] Joukov V, Ćesić J, Westermann K, Marković I, Petrović, Kulić D. Estimation and Observability Analysis of Human Motion on Lie Groups. IEEE Transactions on Cybernetics, 2020, 50(3): 1321−1332 doi: 10.1109/TCYB.2019.2933390 [29] He D, Xu W, Zhang F. Symbolic representation and toolkit development of iterated error-state extended Kalman filters on manifolds. IEEE Transactions on Industrial Electronics, 2023, 70(12): 12533−12544 doi: 10.1109/TIE.2023.3237872 [30] Guo H J, Liu H Y, Zhou Y, Hu X X. Robust state estimation via maximum correntropy EKF on matrix Lie groups with application to low-cost INS/GPS-integrated navigation ststems. IEEE Sensors Journal, 2023, 23(9): 9467−9479 doi: 10.1109/JSEN.2023.3251389 [31] Forbes J R, Zlotnik D E. Sigma point Kalman filtering on matrix Lie groups applied to the SLAM problem. In: Proceedings of the 3rd International Conference on Geometric Science of Information. Paris, France: Springer, 2017. 318-328 [32] Cantelobre T, Chahbazian C, Crous A, Bonnabel S. A real-times unscented Kalman filter on manifolds for challenging AUV anvigation. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Las Vegas, USA: IEEE, 2020. 2309-2316 [33] Sjøberg A M, Egeland O. Lie Algebraic Unscented Kalman Filter for Pose Estimation. IEEE Transactions on Automatic Control, 2022, 67(8): 4300−4307 doi: 10.1109/TAC.2021.3121247 [34] Hauberg S, Lauze F, Pedersen K S. Unscented Kalman filtering on Riemannian manifolds. Journal of mathematical imaging and vision, 2013, 46: 103−120 doi: 10.1007/s10851-012-0372-9 [35] Menegaz H M, Ishihara J Y, Kussaba H T. Unscented Kalman filters for Riemannian state-space systems. IEEE Transactions on Automatic Control, 2018, 64(4): 1487−1502 doi: 10.1109/tac.2018.2846684 [36] Hertzberg C, Wagner R, Frese U, Schröder. Integrating generic sensor fusion algorithms with sound state representations through encapsulation of manisolds. Information Fusion, 2013, 14(1): 57−77 doi: 10.1016/j.inffus.2011.08.003 [37] Brossard M, Condomines J P. Unscented Kalman filtering on Lie groups. In: Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver, BC, Canada: IEEE, 2017. 2485-2491 [38] Brossard M, Bonnabel S, Barrau A. Unscented Kalman filtering on Lie groups for visual inertial odometry. In: Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Madrid, Spain: IEEE, 2018. 649-655 [39] Brossard M, Barrau A, Bonnabel S. A code for unscented Kalman filtering on manifolds (UKF-M). In: Proceedings of the 2020 IEEE International Conference on Robotics and Automation (ICRA). Paris, France: IEEE, 2020. 5701-5708 [40] Du S, Huang Y L, Lin B, Qian J, Zhang Y G. A Lie Group Manifold-Based Nonlinear Estimation Algorithm and Its Application to Low-Accuracy SINS/GNSS Integrated Navigation. IEEE Transactions on Instrumentation and Measurement, 2022, 71: 1−27 doi: 10.1109/TIM.2022.3159950 [41] Li Tian-Zhi, Wang Jin-Zhi. Variational uncented Kalman filter on matrix Lie groups. automatica, 2025, 172: 111995−234 doi: 10.1016/j.automatica.2024.111995 [42] 杨旭升, 夏晓翠, 金宇强, 顾欣星, 张文安. 基于广义最大相关熵准则的几何滤波方法. 自动化学报, 2025, 51(7): 1612−1625 doi: 10.16383/j.aas.c240497Yang Xu-Sheng, Xia Xiao-Cui, Jin Yu-Qiang, Gu Xin-Xing, Zhang Wen-An. Geometric filtering method based on generalized maximum correntropy criterion. Acta Automatica Sinica, 2025, 51(7): 1612−1625 doi: 10.16383/j.aas.c240497 [43] Guo H J, Zhou Y, Liu H Y, Hu X X. Improved cubature Kalman filtering on matrix Lie groups based on intrinsic numerical integration error calibration with application to attitude estimation. Machines, 2022, 10(4): 265 doi: 10.3390/machines10040265 [44] Ćesić J, Marković I, Bukal M, Petrović I. Extended information filter on matrix Lie groups. automatica, 2017, 82: 226−234 doi: 10.1016/j.automatica.2017.04.056 [45] Rentmeesters Q, Absil P A, van Dooren P, Gallivan K, Srivastava A. An efficient particle filtering technique on the Grassmann manifold. In proceedings of the 2010 IEEE International Conference on Acoustics, Speech and Signal Processing. Dallas, TX, USA: IEEE, 2010, 3838-3841 [46] Jin Y Q, Zhang W A, Lu X Y, Chen B, Yu L. Computationally efficient Lie algebraic particle filters for state estimation. IEEE Transactions on Automatic Control, 2025, 70(5): 3370−3377 doi: 10.1109/TAC.2024.3508465 [47] Barrau A, Bonnabel S. The invariant extended Kalman filter as a stable observer. IEEE Transactions on Automatic Control, 2017, 62(4): 1797−1812 doi: 10.1109/TAC.2016.2594085 [48] Chang L, Luo Y R. Log-linear error state model derivation without approximation for INS. IEEE Transactions on Aerospace and Electronic Systems, 2023, 59(2): 2029−2035 doi: 10.1109/taes.2022.3197726 [49] Chauchat P, Barrau A, Bonnabel S. Kalman filtering with a class of geometric state equality constraints. In: Proceedings of the 2017 56th IEEE Conference on Decision and Control (CDC). Melbourne, Australia: IEEE, 2017. 2581-2586 [50] Barrau A, Bonnabel S. Extended Kalman filtering with nonlinear equality constraints: a geometric approach. IEEE Transactions on Automatic Control, 2020, 65(6): 2325−2338 doi: 10.1109/TAC.2019.2929112 [51] Barrau A, Bonnabel S. Linear observed systems on groups. Systems & control letters, 2019, 129: 36−42 doi: 10.1016/j.sysconle.2019.05.005 [52] Tejaswi K C, Maneesha W, Taeyoung L. Invariant Kalman Filter for Relative Dynamics. IFAC-PapersOnLine, 2025, 59(19): 382−387 doi: 10.1016/j.ifacol.2025.11.064 [53] Lavoie M A, Arsenault J, Forbes J R. An invariant extended H $\infty$ filter. In: Proceedings of the 2019 58th IEEE Conference on Decision and Control (CDC). Nice, France: IEEE, 2019. 7905-7910 [54] Chauchat P, Vila-Valls J, Chaumette E. Robust linearly constrained invariant filtering for a class of mismatched nonlinear systems. IEEE Control Systems Letters, 2022, 6: 223−228 doi: 10.1109/LCSYS.2021.3064931 [55] Jung J, Chan G. Gaussian mixture midway-merge for object slam with pose ambiguity. IEEE Robotics and Automation Letters, 2022, 8(1): 400−407 [56] Du S, Zhu F, Wang Z, Huang Y, Zhang Y. A novel lie group framework-based student’s t robust filter and its application to INS/DVL tightly integrated navigation. IEEE Transactions on Instrumentation and Measurement, 2024, 73: 1−21 [57] Hartley R, Ghaffari M, Eustice R M, et al. Contact-aided invariant extended Kalmna filtering for robot state estimation. The International Journal of Robotics Research, 2020, 39(4): 402−430 doi: 10.1177/0278364919894385 [58] Teng Sang-Li, Mueller M W, Sreenath K. Legged robot state estimation in slippery environments using invariant extended Kalman filter with velocity update. In: Proceedings of the 2021 IEEE International Conference on Robotics and Automation (ICRA). Xi'an, China: IEEE, 2021. 3104-3110 [59] Gao Y, Yuan C, Gu Y. Invariant filtering for legged humanoid locomation on a dynamic rigid surface. IEEE/ASME Transactions on Mechatronics, 2022, 27(4): 1900−1909 doi: 10.1109/TMECH.2022.3176015 [60] Nisticò Y, Kim H, Soares J C V, Fink G, Park H W, Semini C. Multi-sensor fusion for quadruped robot state estimation using invariant filtering and smoothing. IEEE Robotics and Automation Letters, 2025, 10(6): 6296−6303 doi: 10.1109/LRA.2025.3564711 [61] Youm D, Oh H, Choi S, Kim H, Jeon S, Hwangbo J. Legged robot state estimation with invariant extended Kalman filter using neural measurement network. In: Proceedings of the 2025 IEEE International Conference on Robotics and Automation (ICRA). Altanta, GA, USA: IEEE, 2025. 670-676 [62] Qian L, Lin X, Niu X, Huang Q, Li L, Guo G, Wang Z, Chen R. Avnet: learning attitude and velocity for vehicular dead reckoning using smartphone by adapting an invariant EKF. Satellite Navigation, 2025, 15(6). [63] He Z, Teng S L, Lin T Y, Ghaffari M, Gu Y. Invariant filtering for full-state estimation of ground robots in noninertial environments. IEEE/ASME Transactions on Mechatronics, 2025. doi: 10.1109/TMECH.2025.3575289 [64] Lee S J, Kim H B, Kim K S. Legged robot state estimation using invariant neural-augmented Kalman filter with a neural compensator. In: Proceedings of the 2025 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Hangzhou, China: IEEE, 2025. 15445-15452 [65] Liu Y H, Xu Q, Wang Y M, Leong H Y, Ou B K. Spiking neural-invariant Kalman fusion for accurate localization using low-cost IMUs. arXiv preprint arXiv: 2601.08248, 2026. [66] Potokar E R, Norman K, Mangelson J G. Invariant extended Kalman filtering for underwater navigation. IEEE Robotics and Automation Letters, 2021, 6(3): 5792−5799 doi: 10.1109/LRA.2021.3085167 [67] Xu Bo, Guo Yu. A novel DVL calibration method based on robust invariant extended Kalman filter. IEEE Transactions on Vehicular Technology, 2022, 71(9): 9422−9434 doi: 10.1109/TVT.2022.3182017 [68] 郭瑜, 徐博, 余志强, 等. 基于不变误差定义的INS/USBL紧组合导航算法. 中国惯性技术学报, 2023, 31(8): 750−759 doi: 10.13695/j.cnki.12-1222/o3.2023.08.002Guo Y, Xu B, Yu Z Q, et al. Invariant error definition-based tightly integrated navigation algorithm for INS/USBL. Journal of Chinese Inertial Technology, 2023, 31(8): 750−759 doi: 10.13695/j.cnki.12-1222/o3.2023.08.002 [69] Tang Hong-Qiong, Xu Jiang-Ning, Chang Lu-Bin, Shi Wen-Ce, He Hong-Yang. Invariant Error-Based Integrated Solution for SINS/DVL in Earth Frame: Extension and Comparison. IEEE Transactions on Instrumentation and Measurement, 2023. doi: 10.1109/TIM.2022.3225043 [70] Brossard M, Barrau A, Bonnabel S. Exploiting symmetries to design EKFs with consistency properties for navigation and SLAM. IEEE Sensors Journal, 2018, 19(4): 1572−1579 [71] Zhang T, Wu K Z, Song J, Huang S, Dissanayake G. Convergence and consistency analysis for a 3-D invariant-EKF SLAM. IEEE Robotics and Automation Letters, 2017, 2(2): 733−740 doi: 10.1109/LRA.2017.2651376 [72] Wu K Z, Zhang T, Su D, Huang S D, Dissanayake G. An invariant-EKF VINS algorithms for improving consistency. In: Proceedings of the 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS). Vancouver, BC, Canada: IEEE, 2017. 1578-1585 [73] Caruso D, Eudes A, Sanfourche M, Vissière D, Besnerais G L. Magneto-visual-inertial Dead-reckoning: Improving Estimation consistency by invariance. In: Proceedings of the 2019 IEEE 58th Conference on Decision and Control (CDC). Nice, France: IEEE, 2019. 7923-7930 [74] Heo S, Park C G. Consistent EKF-based visual-inertial odometry on matrix Lie group. IEEE Sensors Journal, 2018, 18(9): 3780−3788 doi: 10.1109/JSEN.2018.2808330 [75] Heo S, Juang J H, Park C G. Consistent EKF-based visual-inertial navigation using points and lines. IEEE Sensors Journal, 2018, 18(18): 7638−7649 doi: 10.1109/JSEN.2018.2858276 [76] Tsao S H, Jan S S. Observability analysis and consistency improvements for visual-inertial odometry on the matrix Lie group of extended poses. IEEE Sensors Journal, 2021, 21(6): 8341−8353 doi: 10.1109/JSEN.2020.3046718 [77] Jung J H, Heo S, Park C G. Observability analysis of IMU intrinsic parameters in stereo visual-inertial odometry. IEEE Robotics and Automation Letters, 2020, 69(10): 7530−7541 [78] Yang Y, Chen C, Lee W, et al. Decoupled right invariant error states for consistent visual-inertial navigation. IEEE Robotics and Automation Letters, 2022, 7(2): 1627−1634 doi: 10.1109/LRA.2021.3140054 [79] Luo Y, Hu J L, Guo C. Right invariant $SE_2(3)$-EKF for relative navigation in learning-based visual inertial odometry. In: Proceedings of the 2022 5th International Symposium on Autonomous Systems (ISAS). Hangzhou, China: IEEE, 2022. 1-7 [80] Guo C, Hu J L, Luo Y R. Learning-based monocular visual-inertial odometry with $SE_2(3)$-EKF. Journal of Field Robotics, 2024, 41(6): 1780−1796 doi: 10.1002/rob.22349 [81] Jin Y Q, Zhang W A, Sun H, Yu L. Learning-aided inertial odometry with nonlinear state estimator on manifold. IEEE Transactions on Intelligent Transportation Systems, 2023, 24(9): 9792−9803 doi: 10.1109/TITS.2023.3273391 [82] Altawaitan A, Stanley J, Ghosal S, Duong T, Atanasov N. Learned IMU bias prediction for invariant visual inertial odometry. IEEE Robotics and Automation Letters, 2025, 10(11): 11872−11879 doi: 10.1109/LRA.2025.3617733 [83] Zhang Z Q, Song Y, Huang S D, Xiong R, Wang Y. Toward consistent and efficient map-based visual-inertial localization: Theory framework and filter design. IEEE Transactions on Robotics, 2023, 39(4): 2892−2911 doi: 10.1109/TRO.2023.3272847 [84] Liu Chang-Wu, Jiang Chen, Wang Hao-Wen. InGVIO: a consistent invariant filter for fast and high-accuracy GNSS-visual-inertial odometry. IEEE Robotics and Automation Letters, 2023, 8(3): 1850−1857 doi: 10.1109/LRA.2023.3243520 [85] Song Y, Zhang Z Q, Wu J, Wang Y, Zhao L, Huang S D. A right invariant extended Kalman filter for object based SLAM. IEEE Robotics and Automation Letters, 2021, 7(2): 13167−1323 [86] Jung J H, Park C G. Object-based visual-inertial navigation system on matrix Lie group. In: Proceedings of the 2022 International Conference on Robotics and Automation (ICRA). Philadelphia: IEEE, 2022. 9499-9505 [87] Zhang H, Xiao R, Li J, Yan C, Tang H. A high-precision LiDAR-inertial odometry via invariant extended Kalman filtering and efficient surfel mampping. IEEE Transactions on Instrumentation and Measurement, 2023, 73. DOI: 10.1109/TIM.2024.3382751 [88] Shi P, Zhu Z, Sun S, Zhao X, Tan M. Invariant extended Kalman filtering for tightly coupled LiDAR–inertial odometry and mapping. IEEE/ASME Transactions on Mechatronics, 2023, 28(4): 2213−2224 doi: 10.1109/TMECH.2022.3233363 [89] Fang K D, Song R, Wang H H. Invariant-DLIO: Direct LiDAR–inertial odometry based on invariant Kalman filtering. IEEE Sensors Journal, 2025, 25(11): 20572−20583 doi: 10.1109/JSEN.2025.3558916 [90] Zhang C, Zhang J, Liu Q, Qin J. SI-LIO: high-precision tightly-coupled LiDAR-inertial odometry via single-iteration invariant extended Kalman filter. IEEE Robotics and Automation Letters, 2025, 10(1): 564−571 doi: 10.1109/LRA.2024.3497757 [91] Scherzinger B M, Reid D B. Modified strapdown inertial navigator error models. In: Proceedings of 1994 IEEE Position, Location and Navigation Symposium-PLANS'94. Las Vegas, NV, USA: IEEE, 1994: 426-430. [92] Scherzinger B M. Inertial navigator error models for large heading uncertainty. In: Proceedings of 1996 IEEE Position, Location and Navigation Symposium-PLANS'96. Atlanta, GA, USA: IEEE, 1996: 477-484. [93] Kong Xiao-Ying, Nebot E M, Durrant-Whyte H. Development of a nonlinear psi-angle model for large misalignment errors and its application in INS alignment and calibration. In: Proceedings 1999 IEEE International Conference on Robotics and Automation. Detroit, MI, USA: IEEE, 1999: 1430-1435. [94] Cui Jia-Rui, Wang Mao-Song, Wu Wen-Qi, et al. Lie group based nonlinear state errors for MEMS-IMU/GNSS/magnetometer integrated navigation. Journal of Navigation, 2021, 74(4): 887−900 doi: 10.1017/S037346332100014X [95] Luo Ya-Rong, Wang Meng-Yuan, Guo Chi, Guo Wen-Fei. Research on invariant extended Kalman filter based 5G/SINS integrated navigation simulation. China Satellite Navigation Conference (CSNC 2021) Proceedings. Singapore: Springer, 2021. 455-466 [96] Du S Y, Huang Y L, Lin B Q, Qian J H, Zhang Y G. A Lie group manifold-based nonlinear estimation algorithm and its application to low-accuracy SINS/GNSS integrated navigation. IEEE Transactions on Instrumentation and Measurement, 2022, 71. DOI: 10.1109/TIM.2022.3159950 [97] Chang Lu-Bin, Di Jing-Bo, Qin Fang-Jun. Inertial-based integration with transformed INS mechanization in Earth frame. IEEE/ASME Transactions on Mechatronics, 2022, 27(3): 1738−1749 doi: 10.1109/TMECH.2021.3090428 [98] Ou-Yang Wei, Wu Y X. A trident quaternion framework for inertial based navigation Part I: Rigid motion representation and computation. IEEE Transactions on Aerospace Electronic Systems, 2021, 58(3): 2409−2420 [99] Ou-Yang Wei, Wu Y X. A trident quaternion framework for inertial based navigation Part Ⅱ: Error models and application to initial alignment. IEEE Transactions on Aerospace Electronic Systems, 2021, 58(3): 2421−2437 [100] Luo Ya-Rong, Guo Chi, Chen Yi-Chao. Filter and piecewise smoother on the matrix Lie group. GPS Solutions, 2023, 27(4): 163 doi: 10.1007/s10291-023-01460-2 [101] 罗亚荣, 郭文飞, 齐书峰, 郭迟. 不变RTS平滑算法及其在5G/SINS组合导航中的应用研究. 武汉大学学报(信息科学版), 2024, 49(5): 747−755 doi: 10.13203/j.whugis20210587Luo Ya-Rong, Guo Wen-Fei, Qi Shu-Feng. Invariant RST smoother and its application in 5G/SINS integrated navigation system. Geomatics and Information Science of Wuhan University, 2024, 49(5): 747−755 doi: 10.13203/j.whugis20210587 [102] van Der Lann N, Cohen M, Arsenault J, et al. The invariant Rauch-Tung-Striebel smoother. IEEE Robotics and Automation Letters, 2020, 5(4): 5067−5074 doi: 10.1109/LRA.2020.3005132 [103] 李昕, 孟硕林, 黄观文, 等. GNSS/SINS定位稳健 $SE_2(3)$-EKF方法. 测绘学报, 2023, 52(10): 1640−1649 doi: 10.11947/j.AGCS.2023.20220526Li Xin, Meng Shuo-Lin, Huang Guan-Wen, et al. Robust GNSS/SINS positioning based on the $SE_2(3)$-EKF framework. Acta Geodaetica et Cartographica Sinica, 2023, 52(10): 1640−1649 doi: 10.11947/j.AGCS.2023.20220526 [104] Li Xin, Huang Guan-Wen, Ma S, Li H. Adaptive Invariant Extended Kalman Filter-Based Tightly-Coupled SINS/RTK-Integrated Positioning for Rotor Unmanned Aerial Vehicle. IEEE Transactions on Instrumentation Measurement, 2024. DOI: 10.1109/TIM.2024.3449937 [105] Wang Mao-Song, Cui Jia-Rui, Wu Wen-Qi. Left/Right Invariant Lie Group Error for SINS/GNSS Tightly Coupled Vehicular Integrated Navigation. IEEE Transactions on Instrumentation Measurement, 2025, 74(6): 8975−8988 doi: 10.1109/tvt.2025.3542441 [106] Ye Xiao-Yu, Song Fu-Jun, Zhang Zong-Yu, Zhang Rui, Zeng Qing-Hua. Semi-Aerodynamic Model-Aided Invariant Kalman Filtering for UAV Full-State Estimation. IEEE Sensors Journal, 2024, 24(16): 25920−25939 doi: 10.1109/JSEN.2024.3414995 [107] Oursland J. The Design and Implementation of an Invariant Extended Kalman Filter for IMU-UWB Sensor Fusion. South Dakota School of Mines and Technology, 2024. [108] 崔加瑞, 张礼廉, 王茂松, 吴文启. 基于改进李群卡尔曼滤波的惯性/里程计组合导航方法. 控制理论与应用, 2023, 40(12): 2179−2186Cui Jia-Rui, Zhang Li-Lian, Wang Mao-Song, Wu Wen-Qi. An improved LG-EKF for SINS/ODO integrated navigation. Control Theory & Applications, 2023, 40(12): 2179−2186 [109] Cui Jia-Rui, Wang Mao-Song, Liu Ruo-Chen, Wu Wen-Qi. Enhanced LG-EKF Backtracking Framework for Body-Velocity-Aided Vehicular Integrated Navigation. IEEE Transactions on Vehicular Technology, 2024, 73(10): 14212−14223 doi: 10.1109/TVT.2024.3405809 [110] 郭迟, 陈毅超, 罗亚荣. 一种世界坐标系下的GNSS/SINS松组合导航矩阵李群滤波算法. 中国惯性技术学报, 2023, 52(10): 1640−1649 doi: 10.13695/j.cnki.12-1222/o3.2024.03.005Guo Chi, Chen Yi-Chao, Luo Ya-Rong. Matrix Lie group filtering algorithm for GNSS/SINS loosely integrated navigation in the world frame. Journal of Chinese Inertial Technology, 2023, 52(10): 1640−1649 doi: 10.13695/j.cnki.12-1222/o3.2024.03.005 [111] Hwang J H, Cha J, Park C G. A novel federated structure of invariant EKF using left-/right-invariant observations. IEEE Sensors Journal, 2022, 22(21): 20645−20654 doi: 10.1109/jsen.2022.3179833 [112] Han J, Ouyang W, Zhu M R, Wu Y X. Covariance switch-based invariant extended Kalman filter for multi-source fusion. In: Proceedings of International Coference on Guidance Navigation and Control (ICGNC). Singapore: Springer, 2024. 340-351 [113] Luo Y R, Lu F, Guo C, Liu J N. Matrix Lie group-based extended Kalman filtering for inertial-integrated navigation in the navigation frame. IEEE Transactions on Instrumentation Measurement, 2023. DOI: 10.1109/TIM.2023.3329103 [114] Barrau A, Bonnabel S. The geometry of navigation problems. IEEE Transactions on Automatic Control, 2023, 68(2): 689−704 doi: 10.1109/TAC.2022.3144328 [115] Xia C X, Li X X, He F Y, Li S Y, Zhou Y X. Accurate and rapidly-convergent GNSS/INS/LiDAR tightly-coupled integration via invariant EKF based on two-frame group. IEEE Transactions on Automation Science and Engineering, 2025, 22: 7992−8005 doi: 10.1109/TASE.2024.3476069 [116] Chauchat P, Barrau A, Bonnabel S. Invariant smoothing on Lie groups. In: Proceedings of the 2018 IEEE/RSJ In- ternational Conference on Intelligent Robots and Systems (IROS). Madrid, Spain: IEEE, 2018. 1703-1710 [117] Walsh A, Arsenault J, Forbes J R. Invariant sliding window filtering for attitude and bias estimation. In: Proceedings of the 2019 American Control Conference (ACC). Philadelphia, PA, USA: IEEE, 2019. 3161-3166 [118] Huai J, Lin Y, Zhuang Y, Shi M. Consistent Right-Invariant Fixed-Lag Smoother with Application to Visual Inertial SLAM. Proceedings of the AAAI Conference on Artificial Intelligence, 2021, 35(7): 6084−6092. doi: 10.1609/aaai.v35i7.16758 [119] Brossard M, Barrau A, Chauchat P, Bonnabel S. Associating uncertainty to extended poses for on Lie group IMU preintegration with rotating Earth. IEEE Transactions on Robotics, 2022, 38(2): 998−1015 doi: 10.1109/TRO.2021.3100156 [120] Li X, Chirikjian G S. Lie-theoretic multi-robot localization. Riemannian Computing in Computer Vision, 2016165−186 [121] Shin G, Sim H, Nam S, Kim Y, Heo J, Kim K K, Multi-robot relative pose estimation in SE(2) with observability analysis: a comparison of extended Kalman filtering and robust pose graph optimization. IEEE Transactions on Intelligent Vehicles, 2024: 1-23 [122] Xu J, Zhu P X, Zhou Y Z, Ren W. Distributed invariant extended Kalman filter using Lie groups: algorithm and experiments. IEEE Transactions on Control Systems Technology, 2023, 31(6): 2777−2789 doi: 10.1109/TCST.2023.3290299 [123] Li H, Li X, Huang S, Yang C, Wu J. Covariance Intersection-based Invariant Kalman Filtering for Distributed Pose Estimation. In proceeding of the 2024 IEEE 63rd Conference on Decision and Control (CDC), Milan, Italy: IEEE, 2024. 1993-1998. [124] Li H Y, Zeng Q C, Li H R, Zhang Y L, Wu J F. Distributed invariant Kalman filter for object-level multi-robot pose SLAM. In: Proceedings of the IEEE Conference on Robotics and Automation (ICRA). Altanta, GA, USA: IEEE, 2025. 15835-15841 [125] Zhang Cong, Qin Jia-Hu, Yan Cheng-Zhen, Shi Yang, Wang Yao-Nan, Li Man. Towards invariant extended Kalman filter-based resilient distributed state estimation for moving robots over mobile sensor networks under deception attacks. Automatica, 2024, 159: 111408 doi: 10.1016/j.automatica.2023.111408 [126] Luo Y, Lu W, Guo C, Li M. D-GVIO: A Buffer-Driven and Efficient Decentralized GNSS-Visual-Inertial State Estimator for Multi-Agent Systems. arXiv preprint arXiv: 2603.01404, 2026. [127] Zhou Y Z, Liu Y F, Zhu P X, Wang X. Distributed Invariant Kalman Filter for Cooperative Localization using Matrix Lie Groups. arXiv preprint arXiv: 2405.04000, 2024. [128] Jin Y Q, Sun H, Zhang W A. Resilient sequential fusion on Lie groups for consistent collaborative localization. IEEE Transactions on Control Systems Technology, 2025, 33(6): 2320 doi: 10.1109/TCST.2025.3589462 [129] Mahony R, Trumpf J, Hamel T. Observers for kinematic systems with symmetry. IFAC Proceedings Volumes, 2013, 46(23): 617−633 doi: 10.3182/20130904-3-FR-2041.00212 [130] Trumpf J, Mahony R, Hamel T. On the structure of kinematic systems with complete symmetry. In proceedings of 2018 IEEE conference on decision and control (CDC). IEEE, 2018. 1276-1280. [131] Mahony R, Hamel T, Trumpf J. Equivariant systems theory and observer design[J]. arxiv preprint arxiv: 2006.08276, 2020. [132] Fornasier A, Ge Y X, van Goor P, Mahony R, Weiss S. Equivariant symmetires for inertial navigation systems. Automatica, 2025, 181: 112495 doi: 10.1016/j.automatica.2025.112495 [133] Luo Ya-Rong, Guo Chi, Liu Jing-Nan. Equivariant filtering framework for inertial-integrated navigation. Satellite Navigation, 2023, 2(1): 30 doi: 10.1186/s43020-021-00061-z [134] 罗亚荣, 卢文韬, 郭迟, 刘经南. GNSS/INS组合导航左手对称性等变滤波模型及算法. 测绘学报, 2025, 54(8): 1389−1403 doi: 10.11947/j.AGCS.2025.20240473Luo Ya-Rong, Lu Wen-Tao, Guo Chi, Liu Jing-Nan. Left-handed symmetry equivariant filtering model and algorithm for GNSS/INS integrated navigation. Acta Geodaetica et Cartographica Sinica, 2025, 54(8): 1389−1403 doi: 10.11947/j.AGCS.2025.20240473 [135] Luo Ya-Rong, Chen Yi-Chao, Tao An-Bo, Guo Chi. Left equivariant filter with autonomous error dynamics for inertial integrated navigation systems with biases. In: Proceedings of the 36th China Control and Decision Conference (CCDC). Xi'an, China: IEEE, 2025. 341-348 [136] Loianno G, Watterson M, Kuman V. Visual inertial odometry for quadrotors on SE(3). In: Proceedings of the 2016 IEEE Conference on Robotics and Automation (ICRA). Stockholm: IEEE, 2016. 1544-1551 [137] Cogliati A, Mastrolia P. Cartan, Schouten and the search for connection. Historia Mathematica, 2018, 45(1): 39−74 doi: 10.1016/j.hm.2017.09.001 [138] Mueller M W, Hehn M D, D'Andera R. Covariance correction step for Kalman filtering with an atittude. Journal of Guidance, Control, and Dynamics, 2016, 40(9): 2301−2306 [139] Gill R, Muller M W, D'Andera R. Full-order solution to the attitude reset problem for Kalman filtering with an atittude. Journal of Guidance, Control, and Dynamics, 2020, 43(7): 1232−1246 doi: 10.2514/1.G004134 [140] Markley F L, Cheng Yang, Crassidis J L. Error-covariance reset in the multiplicative extended Kalman filter for attitude estimation. Journal of Guidance, Control, and Dynamics, 2023, 46(10): 1860−1873 doi: 10.2514/1.G007653 [141] Ge Yi-Xiao, van Goor P, Mahony R. Equivariant filter design for discret-time systems. In: Proceedings of the 2022 IEEE Conference on Desicion and Control (CDC). Cancun: IEEE, 2022. 1243-1250 [142] Ge Yi-Xiao, van Goor P, Mahony R. A geometric perspective on fusing Gaussian distributions on Lie groups. IEEE Control Systems Letters, 2024, 8: 844−849 doi: 10.1109/LCSYS.2024.3405485 [143] Pennec X. Bi-invariant means on Lie groups with Cartan-Schouten connection. Geometric Science of Infromation, Berlin: Springer, 2013. 59-67 [144] Mahony R, Trumpf J. Equivariant filter design for kinematic systems on Lie groups. IFAC-PapersOnLine, 2021, 54(9): 253−260 doi: 10.1016/j.ifacol.2021.06.148 [145] Calinon S. Gaussians on Riemannian manifolds: applications for robot learning and adaptive control. IEEE Robotics & Automation Magazine, 2020, 27(2): 33−45 doi: 10.1109/MRA.2020.2980548 [146] 罗亚荣, 郭迟, 欧阳威, 刘经南. 顾及零偏几何性质的GNSS/SINS组合导航方法. 测绘学报, 2025, 54(1): 26−39 doi: 10.11947/j.AGCS.2025.20240232Luo Ya-Rong, Guo Chi, Ou-Yang Wei, Liu Jing-Nan. GNSS/SINS integrated navigation method considering the geometric property of biases state. Acta Geodaetica et Cartographica Sinica, 2025, 54(1): 26−39 doi: 10.11947/j.AGCS.2025.20240232 [147] Lu W, Luo Y R, Guo C, Jiang W P. An iterated equivariant filter and its application in tightly coupled SINS/GNSS integrated navigation. IEEE Transactions on Instrumentation Measurement, 2025, DOI: 10.1109/TIM.2025.3565028 [148] Liu Chang-Wu, Shen Yuan. On the existence of linear observed systems on manifolds with connection. IEEE Control Systems Letters, 2024, 8: 2607−2612 doi: 10.1109/LCSYS.2024.3506222 [149] Mahony R, van Goor P, Henein M, et al. Equivariant visual odometry in the wild. In: Proceedings of the 59th IEEE Conference on Desicion and Control (CDC). Jeju, Korea (South): IEEE, 2020. 1314-1319 [150] van Goor P, Mahony R, Hamel T, Trumpf J. A geometric observer design for visual localisation and mapping. In 2019 IEEE 58th Conference on Decision and Control (CDC), IEEE, 2019. 2543-2549. [151] van Goor P, Mahony R, Hamel T, Trumpf J. An observer design for visual simultaneous localisation and mapping with output equivariance. IFAC-PapersOnLine, 2020, 53(2): 9560−9565 doi: 10.1016/j.ifacol.2020.12.2438 [152] van Goor P, Mahony R, Hamel T, et al. Constructive observer design for visual simultaneous localisation and mapping. Automatica, 2021, 132: 109803 doi: 10.1016/j.automatica.2021.109803 [153] Mahony R, Hamel T. A geometric nonlinear observer for simultaneous localisation and mapping. In: Proceedings of 2017 IEEE 56th Annual Conference on Decision and Control (CDC). IEEE, 2017. 2408-2415. [154] van Goor P, Hamel T, Mahony R. Equivariant filter(EqF): a general filter design for systems on homogenerous spaces. In: Proceedings of the 59th IEEE Conference on Desicion and Control (CDC). Jeju, Korea (South): IEEE, 2020. 5401-5408 [155] van Goor P, Hamel T, Mahony R. Equivariant filter (EqF). IEEE Transactions on Automatic Control, 2023, 68(6): 3501−3512 doi: 10.1109/TAC.2022.3194094 [156] van Goor P, Mahony R. An equivariant filter for visual inertial odometry. In: Proceedings of the International Conference on Robotics and Automation (ICRA). Xi'an, China: IEEE, 2021. 14432-14438 [157] van Goor P, Mahony R. EqVIO: An equivariant filter for visual-inertial odometry. IEEE Transactions on Robotics, 2023, 39(5): 3567−3585 doi: 10.1109/TRO.2023.3289587 [158] Chen Z H, Miao Z Q, Wang C C, Wang Y N. Robust stereo VIO based on an equivariant filter for complex lighting environments. In: Proceedings of the 2022 International Conference on Robotics and Biomimetics (ROBIO). Jinghong: IEEE, 2022. 2128-2133 [159] Ge Yi-Xiao, Pearce A, van Goor P, Mahony R. Equivariant filter design for range-only SLAM. In: Proceedings of the 2025 IEEE International Conference on Robotics and Automation (ICRA). Atlanta, GA, USA: IEEE, 2025. 5040-5046 [160] Fornasier A, Ng Y, Mahony R, et al. Equivariant filter design for inertial navigation systems with input measurement biases. In: Proceedings of the 2022 IEEE International Conference on Robotics and Automation (ICRA). Philadelphia: IEEE, 2022. 4333-4339 [161] Fornasier A, Ng Y, B rommer C, et al. Overcoming bias: equivariant filter design for biased attitude estimation with online calibration. IEEE Robotics and Automation Letters, 2022, 7(4): 12118−12125 doi: 10.1109/LRA.2022.3210867 [162] Tao A B, Luo Y R, Xia C X, Guo C, Li X X. Equivariant filter for tightly coupled LiDAR-Inertial odometry. In: Proceedings of the IEEE Conference on Robotics and Automation (ICRA). Altanta, GA, USA: IEEE, 2025. 2147-2153 [163] Fornasier A, van Goor P, Allak E, et al. MSCEqF: a multi state constraint equivariant filter for vision-aided inertial navigation. IEEE Robotics and Automation Letters, 2024, 9(1): 731−738 doi: 10.1109/LRA.2023.3335775 [164] Fornasier A, Ge Yi-Xiao, van Goor P, et al. An equivariant approach to robust state estimation for the ArduPilot autopilot system. In: Proceedings of the 2024 IEEE International Conference on Robotics and Automation (ICRA). Yokohama: IEEE, 2024. 11956-11962 [165] 胡建朗, 罗亚荣, 郭迟, 刘经南. 一种结合等变滤波和视觉观测网络的混合视觉惯性里程计. 导航定位与授时, 2024, 11(6): 28−46 doi: 10.19306/j.cnki.2095-8110.2024.06.003Hu Jian-Lang, Luo Ya-Rong, Guo Chi, Liu Jing-Nan. A hybrid visual-onertial odometry combining equivariant filter and visual observation network. Navigation Posiioning and Timing, 2024, 11(6): 28−46 doi: 10.19306/j.cnki.2095-8110.2024.06.003 [166] Serrano G, Guerreiro B J, Lawrence P, Cunha R. Equivariant Filter for Relative Attitude and Target Angular Velocity Estimation. arXiv preprint arXiv: 2506.06016.24 [167] Burri M, Nikolic J, Gohl P, Schneider T, Rehder J, Omari S, Achtelik M W, Siegwart R. The EuRoC micro aerial vehicle datasets. The International Journal of Robotics Research, 2016, 35(10): 1157−1163 doi: 10.1177/0278364915620033 [168] Geiger A, Lenz P, Stiller C, Urtasun R. Vision meets robotics: The KITTI dataset. The International Journal of Robotics Research, 2013, 32(11): 1231−1237 doi: 10.1177/0278364913491297 [169] Delmerico J, Cieslewski T, Rebecq H, Faessler M, Scaramuzza D. Are We Ready for Autonomous Drone Racing? The UZH-FPV Drone Racing Dataset. In: Proceedings of the 2019 IEEE International Conference on Robotics and Automation (ICRA). Montreal, QC, Canada: IEEE, 2019. 6713-6719 -
下载:
计量
- 文章访问数: 14
- HTML全文浏览量: 18
- 被引次数: 0
