MF-IEKF: A Multiplicative Federated Invariant Extended Kalman Filter for INS/GNSS

Highlights

What are the main findings?

• Based on the two left-invariant errors on the SE₂(3) Lie group, a corrected error on the Lie algebra is proposed for the multiplicative corrected left-invariant extended Kalman filter (MCL-IEKF).
• Leveraging the decoupling of position and attitude in the right-invariant observation matrix of GNSS, a multiplicative federated IEKF (MF-IEKF) is developed by combining left- and right-invariant errors.

What are the implications of the main findings?

• MCL-IEKF improves state estimation accuracy compared to classical multiplicative left- (right-)IEKF.
• Under large misalignment angle conditions, MF-IEKF achieves faster convergence in position and velocity compared to the MCL-IEKF.

Abstract

The integration of an inertial navigation system (INS) with the Global Navigation Satellite System (GNSS) is crucial for suppressing the error drift of the INS. However, traditional fusion methods based on the extended Kalman filter (EKF) suffer from geometric inconsistency, leading to biased estimates—a problem markedly exacerbated under large initial misalignment angles. The invariant extended Kalman filter (IEKF) embeds the state in the Lie group SE₂(3) to establish a more consistent framework, yet two limitations remain. First, its standard update fails to synergize complementary error information within the left-invariant formulation, capping estimation accuracy. Second, velocity and position states converge slowly under extreme misalignment. To address these issues, a multiplicative federated IEKF (MF-IEKF) was proposed. A geometrically consistent state propagation model on SE₂(3) is derived from multiplicative IMU pre-integration. Two parallel, mutually inverse left-invariant error sub-filters (ML₁-IEKF and ML₂-IEKF) cooperate to improve overall accuracy. For large-misalignment scenarios, a short-term multiplicative right-invariant sub-filter is introduced to suppress initial position and velocity errors. Extensive Monte Carlo simulations and KITTI dataset experiments show that MF-IEKF achieves higher navigation accuracy and robustness than ML₁-IEKF.

1. Introduction

Inertial navigation systems (INSs) are indispensable in aerospace, autonomous driving, and robotics, owing to their full autonomy, high-rate output, and provision of complete motion information [1,2,3]. However, propagating attitude, velocity, and position through temporal integration of gyroscope and accelerometer data leads to the inherent accumulation and divergence of navigation errors, resulting in significant positional drift [4,5,6]. To suppress this drift and enhance accuracy, multi-sensor fusion, especially with the Global Navigation Satellite System (GNSS), has become indispensable.

Among existing fusion algorithms, frameworks based on the Extended Kalman Filter (EKF) [7,8] and its Error-State Kalman Filter (ESKF) variant [9,10] remain the most widely adopted. Nevertheless, these conventional methods suffer from fundamental limitations. Conventional methods model position and velocity in Euclidean space while representing attitude on the Lie group SO(3) or with quaternions. This mismatch in parameterization disrupts the intrinsic geometric structure. When state estimates deviate significantly, the local linearization employed by EKF/ESKF introduces substantial errors, jeopardizing filter convergence [11]. More critically, these methods exhibit geometric inconsistency [12]: the linearization process during measurement updates can induce spurious information gain in unobservable directions [13,14], thereby corrupting the state estimate. This phenomenon is well-established in Simultaneous Localization and Mapping (SLAM) [15,16,17] and visual-inertial odometry (VIO) systems [18,19].

To address this fundamental issue of geometric inconsistency, the invariant filtering framework, rooted in Riemannian manifold and Lie group theory, has emerged [20,21,22,23,24]. A key realization of this framework is the invariant extended Kalman filter (IEKF), which constructs a geometrically consistent filtering structure by embedding the entire system state into the matrix Lie group SE₂(3). In this setting, the motion model satisfies the group-affine property [25], which results in error dynamics governed by a linear autonomous differential equation, independent of the true state trajectory [22]. This structural decoupling enables the IEKF to theoretically overcome the convergence pitfalls of traditional EKF methods that stem from linearization-point dependence. Consequently, the IEKF maintains superior estimation accuracy and long-term stability, even under large initial misalignment [26,27].

The IEKF can be categorized into the left-IEKF (L-IEKF) and the right-IEKF (R-IEKF) according to the error definition. The system matrix of L-IEKF is completely independent of the state trajectory, often leading to higher estimation accuracy, whereas that of R-IEKF excludes rapidly varying states, yielding superior temporal stability [20]. Consequently, the observation model must be aligned with either the left- or right-invariant form depending on its physical nature (global or local) [28]. This alignment is crucial for maintaining filter consistency throughout the process and for achieving optimal estimation [28,29,30,31]. Guided by this principle, IEKF has been successfully applied in globally referenced inertial-integrated navigation [30,32] and locally referenced VIO systems [21,33,34,35].

Recognizing the complementary advantages of left- and right-invariant models, researchers have begun to explore their joint utilization. For instance, Ref. [31] proposed a federated IEKF that processes left- and right-invariant observation models in parallel and dynamically selects the output according to the scenario, representing a preliminary exploration of multi-model collaboration. However, the intrinsic synergistic mechanisms between these error models were not thoroughly analyzed. In contrast, Ref. [28] established a connection between left- and right-invariant errors for attitude estimation using the adjoint matrix of the Lie group, improving accuracy. Yet, this collaborative framework was not extended to full state estimation encompassing velocity and position. These works indicate that exploring a deep fusion of left- and right-invariant models is a clear and valuable direction.

Despite initial attempts at fusion architectures, existing Lie group-based navigation methods still face two fundamental challenges in delivering high-precision, high-robustness performance. The first is the challenge of model fidelity and generalizability. Many studies rely on simplified models, such as assuming a flat Earth and neglecting Coriolis effects [36], which deviate from real-world navigation conditions. To address this, Ref. [36] proposed an equivariant filtering framework that accounts for Coriolis forces and introduced two new left- and right-invariant error models. Recent research strives to enhance geometric consistency under more realistic models, e.g., constructing precise kinematics and error models on the SE(3) group in the Earth-centered inertial (ECI) or Earth-centered, Earth-fixed (ECEF) reference frames [37,38]. These works deepen the theoretical foundation and improve attitude—particularly heading—estimation under large misalignment angles. However, their focus remains on refining the model within a single reference frame and on theoretical analysis. The second challenge is computational efficiency and structural consistency. Although many methods define states on Lie groups, their kinematic equations often adopt decoupled computational propagation [21,28,29,30,31,32,33,34,35,39], offering computational efficiency comparable to that of the traditional EKF without fully leveraging the structural advantages of Lie group methods. To address this, Ref. [40] introduced an auxiliary velocity and unified state propagation on the SE₂(3) group, proposing an efficient multiplicative pre-integration theory that was later generalized into a unified mathematical framework by [41]. Ref. [42] applied this approach to INS/GNSS integration but did not effectively mitigate the dramatic growth of velocity and position errors caused by large misalignment angles during initial alignment. Recently, Ref. [43] proposed an Iterative Equivariant Filter (I-EqF) that enhances the robustness of tightly coupled SINS/GNSS navigation under nonlinearities and large misalignment through iterative updates and covariance resetting, representing progress in optimizing a single-filter algorithm.

Building on the aforementioned developments, current research has advanced invariant filtering technology along several fronts: model construction has become more rigorous [36,37,38]; numerical methods for single filters have been refined [43]; and valuable attempts have been made to fuse left- and right-invariant models [28,31]. Yet a unified framework that organically integrates two key considerations remains elusive: at the architectural level, how to transcend simple parallel or switching mechanisms to achieve deep synergy between left- and right-invariant error estimations;, in the presence of large initial misalignment, how to exploit the right-invariant model’s insensitivity to attitude error to accelerate the convergence of velocity and position states. Addressing these challenges motivates the present work.

To synergistically leverage the complementary strengths of different invariant error models and effectively handle large initial misalignment, we propose a novel multiplicative federated IEKF (MF-IEKF) framework as shown in Figure 1. The core contribution is an architectural innovation that fuses the advantages of multiple invariant models through a deep synergistic mechanism. Specifically:

• Leveraging multiplicative pre-integration theory [40,41], we derive a state propagation equation that evolves on the SE₂(3) Lie group, ensuring geometric consistency and computational efficiency.
• Adopting the four invariant error models proposed in [36], we construct two parallel left-invariant error sub-filters using GNSS observations. By exploiting the inverse relationship between their error estimates, we perform collaborative correction that significantly improves steady-state accuracy.
• During the initial phase, we introduce a multiplicative right-invariant sub-filter that exploits the attitude-error decoupling property of right-invariant observation models to accelerate velocity and position convergence, thereby effectively mitigating the slow initial convergence exhibited by left-invariant models under large misalignment.
• Rigorous validation via extensive Monte Carlo simulations and experiments on public datasets demonstrates the efficacy of the proposed MF-IEKF. Compared with the multiplicative left-IEKF (ML₁-IEKF) baseline, our method achieves consistent improvements of at least 5.26% in attitude, 8.72% in velocity, and 15.28% in position estimation.

2. IMU Model on Lie Group

This section develops a linearized inertial measurement unit (IMU) kinematic model within the Earth-Centered Earth-Fixed (ECEF) reference frame. The following coordinate systems are formally defined: e-frame (ECEF), b-frame (body-fixed coordinate system), i-frame (inertial reference frame), and n-frame (local navigation frame).

2.1. IMU-Based Kinematic Equations

The kinematic difference equations for the attitude, velocity, and position in the e-frame are given by

$${\dot{\mathbf{R}}}_b^e={\mathbf{R}}_b^e{\mathit{\omega }}_{ib}^b{}^{\wedge }-{{\mathit{\omega }}_{ie}^e}^{\wedge }{\mathbf{R}}_b^e$$

(1)

$${\dot{\mathit{v}}}_{eb}^e={\mathbf{R}}_b^e{\mathit{a}}_{ib}^b-2{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{v}_{eb}^e+{\mathit{g}}_{ib}^e$$

(2)

$${\dot{\mathit{p}}}_{eb}^e={\mathit{v}}_{eb}^e$$

(3)

where ${\mathbf{R}}_b^e\in {ℝ}^{3\times 3}$ is the attitude matrix from b-frame to e-frame; the symbol ${(\cdot )}_{eb}^e$ in the velocity ${\mathit{v}}_{eb}^e$ and position ${\mathit{p}}_{eb}^e$ denotes the b-frame with respect to the e-frame in the e-frame; ${\mathit{a}}_{ib}^b$ and ${\mathit{\omega }}_{ib}^b\in {ℝ}^{3\times 1}$ represent the acceleration and angular rate relative to the i-frame expressed in the b-frame; ${\mathit{\omega }}_{ie}^e\in {ℝ}^{3\times 1}$ is the angular rate of the Earth rotation in the e-frame; ${\mathit{g}}_{ib}^e$ is the gravity vector. The term $-2{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{v}_{eb}^e$ are so called Coriolis acceleration, where the symbol ${(\cdot )}^{\wedge }\in {ℝ}^{3\times 3}$ is the skew-symmetric matrix operator. For a vector $\mathit{\omega }={\left[{\mathit{\omega }}_x,{\mathit{\omega }}_y,{\mathit{\omega }}_z\right]}^{\mathrm{T}}$, its skew-symmetric matrix is defined as:

$${\mathit{\omega }}^{\wedge }=\left[\begin{array}{ccc}0& -{\mathit{\omega }}_z& {\mathit{\omega }}_y\\ {\mathit{\omega }}_z& 0& -{\mathit{\omega }}_x\\ -{\mathit{\omega }}_y& {\mathit{\omega }}_x& 0\end{array}\right].$$

(4)

To establish a unified framework on the manifold, the paper employs an auxiliary velocity [40].

$${\mathit{v}}_{ib}^e={\mathit{v}}_{eb}^e+{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{p}_{ib}^e$$

(5)

where ${\mathit{p}}_{ib}^e$ is the b-frame with respect to the i-frame in the e-frame. Given the colocation of origins between the e-frame and i-frame, there exists the equivalence relation ${\mathit{p}}_{ib}^e={\mathit{p}}_{eb}^e$. Based on (3) and (5), the differential equation of ${\mathit{p}}_{ib}^e$ can be written as

$${\dot{\mathit{p}}}_{ib}^e={\dot{\mathit{p}}}_{eb}^e={\mathit{v}}_{eb}^e={\mathit{v}}_{ib}^e-{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{p}_{ib}^e$$

(6)

${\mathbf{R}}_b^e$, ${\mathit{v}}_{ib}^e$ and ${\mathit{p}}_{ib}^e$ are embedded into the matrix Lie group ${\mathrm{SE}}_2(3)$ [22], the state X can be represented as

$$\mathbf{X}=\left[\begin{array}{ccc}{\mathbf{R}}_b^e& {\mathit{v}}_{ib}^e& {\mathit{p}}_{ib}^e\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]\in {ℝ}^{5\times 5}$$

(7)

Based on (1)–(6), the kinematic difference equation of state matrix X can be rewritten as

$$\dot{\mathbf{X}}=g(\mathbf{X})=\left[\begin{array}{cc}{\dot{\mathbf{R}}}_b^e& \begin{array}{cc}{\dot{\mathit{v}}}_{ib}^e& {\dot{\mathit{p}}}_{ib}^e\end{array}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 2}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{R}}_b^e{\mathit{\omega }}_{ib}^b{}^{\wedge }-{{\mathit{\omega }}_{ie}^e}^{\wedge }{\mathbf{R}}_b^e& \begin{array}{cc}{\mathbf{R}}_b^e{\mathit{a}}_{ib}^b-{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{v}_{ib}^e+{\mathit{G}}_{ib}^e& {\mathit{v}}_{ib}^e-{\mathit{\omega }}_{ie}^e{}^{\wedge }\mathit{p}_{ib}^e\end{array}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 2}\end{array}\right]$$

(8)

where ${\mathit{G}}_{ib}^e={({\mathit{\omega }}_{ie}^e{}^{\wedge })}^2{\mathit{p}}_{ib}^e+{\mathit{g}}_{ib}^e$ is the gravitational vector, it can be derived through vector superposition of centrifugal acceleration with the gravity vector [36].

It is easily verified that (8) can be decomposed to [25]

$$g(\mathbf{X})=\mathbf{X}U+f(\mathbf{X})+W\mathbf{X}$$

(9)

where the matrix U is related to the input, and the form is

$$U=\left[\begin{array}{cc}{{\mathit{\omega }}_{ib}^b}^{\wedge }& \begin{array}{cc}{\mathit{a}}_{ib}^b& {\mathbf{0}}_{3\times 1}\end{array}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 2}\end{array}\right]$$

(10)

The matrix W is with respect to the Earth rotation and gravity vector in the e-frame, and it has

$$W=\left[\begin{array}{cc}-{{\mathit{\omega }}_{ie}^e}^{\wedge }& \begin{array}{cc}{\mathit{G}}_{ib}^e& {\mathbf{0}}_{3\times 1}\end{array}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 2}\end{array}\right]$$

(11)

$f(\cdot )$ is a drift field related to velocity, as defined by

$$f:\mathbf{X}=\left[\begin{array}{ccc}{\mathbf{R}}_b^e& {\mathit{v}}_{ib}^e& {\mathit{p}}_{ib}^e\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]\mapsto f(\mathbf{X})=\left[\begin{array}{cc}{\mathbf{0}}_{3\times 3}& \begin{array}{cc}{\mathbf{0}}_{3\times 1}& {\mathit{v}}_{ib}^e\end{array}\\ {\mathbf{0}}_{2\times 3}& {\mathbf{0}}_{2\times 2}\end{array}\right]$$

(12)

It can be easily checked that $g(\mathbf{X})$ has group-affine property, and satisfies the following equation

$$g({\mathbf{X}}_1{\mathbf{X}}_2)=g({\mathbf{X}}_1){\mathbf{X}}_2+{\mathbf{X}}_{1}g({\mathbf{X}}_2)-{\mathbf{X}}_{1}g({\mathbf{I}}_{5\times 5}){\mathbf{X}}_2$$

(13)

where ${\mathbf{X}}_1,{\mathbf{X}}_2\in {\mathrm{SE}}_2(3)$ are the implementation of (6), and ${\mathbf{I}}_{5\times 5}$ is the group identity element in the ${\mathrm{SE}}_2(3)$. Obviously, $f(\cdot )$ also has the group-affine property

$$f({\mathbf{X}}_1{\mathbf{X}}_2)=f({\mathbf{X}}_1){\mathbf{X}}_2+{\mathbf{X}}_{1}f({\mathbf{X}}_2)-{\mathbf{X}}_{1}f({\mathbf{I}}_{5\times 5}){\mathbf{X}}_2$$

(14)

since $f\left({\mathbf{I}}_{5\times 5}\right)={\mathbf{0}}_{5\times 5}$, (23) can be updated as

$$f({\mathbf{X}}_1{\mathbf{X}}_2)=f({\mathbf{X}}_1){\mathbf{X}}_2+{\mathbf{X}}_{1}f({\mathbf{X}}_2)$$

(15)

2.2. Decomposition of IMU State Matrix on the Li Group in Continuous Time

As the group affine property of the $f(\mathbf{X})$, the state matrix X at arbitrary time t in the e-frame can be decomposed into a product function of the initial state ${\mathbf{X}}_0$ as [40]

$${\mathbf{X}}_t={\mathbf{L}}_t{\mathbf{\Phi }}_t({\mathbf{X}}_0){\mathbf{{\rm Y}}}_t$$

(16)

where the concrete definition of ${\mathbf{\Phi }}_t(\cdot )$ can be given as [40]

$${\mathbf{\Phi }}_t:\mathbf{X}=\left[\begin{array}{ccc}{\mathbf{R}}_b^e& {\mathit{v}}_{ib}^e& {\mathit{p}}_{ib}^e\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]\mapsto \left[\begin{array}{ccc}{\mathbf{R}}_b^e& {\mathit{v}}_{ib}^e& {\mathit{p}}_{ib}^e+t{\mathit{v}}_{ib}^e\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]=\mathbf{X}+t\cdot f(\mathbf{X})$$

(17)

Obviously, ${\mathbf{\Phi }}_t(\cdot )$ only depends on the time t. It is easy to verify the group automorphism property

$${\mathbf{\Phi }}_t({\mathbf{X}}_1{\mathbf{X}}_2)={\mathbf{\Phi }}_t({\mathbf{X}}_1){\mathbf{\Phi }}_t({\mathbf{I}}_{5\times 5}){\mathbf{\Phi }}_t({\mathbf{X}}_2)={\mathbf{\Phi }}_t({\mathbf{X}}_1){\mathbf{\Phi }}_t({\mathbf{X}}_2)$$

(18)

According to the definition of -, it can be concluded that ${\mathbf{\Phi }}_t({\mathbf{I}}_{5\times 5})={\mathbf{I}}_{5\times 5}$.

The concrete form of ${\mathbf{L}}_t$ in (16) can be given as

$${\mathbf{L}}_t=\left[\begin{array}{ccc}{\mathbf{L}}_t^R& {\mathbf{L}}_t^v& {\mathbf{L}}_t^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(19)

Simultaneously, ${\mathbf{L}}_t$ is the solution of differential Equation (20)

$${\dot{\mathbf{L}}}_t=\left[\begin{array}{ccc}-{{\mathit{\omega }}_{ie}^e}^{\wedge }{\mathbf{L}}_t^R& {\mathit{G}}_{ib}^e-{{\mathit{\omega }}_{ie}^e}^{\wedge }{\mathbf{L}}_t^v& {\mathbf{L}}_t^v-{{\mathit{\omega }}_{ie}^e}^{\wedge }{\mathbf{L}}_t^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]=W{\mathbf{L}}_t+f({\mathbf{L}}_t)$$

(20)

where W is defined in (11). Based on the differential equation in (20), ${\mathbf{L}}_t$ represents the global increment in the e-frame that undergoes slow variation due to the action of the Earth rotation. In addition, the initial value is ${\mathbf{L}}_0={\mathbf{I}}_{5\times 5}$.

Meanwhile, the concrete form of ${\mathbf{{\rm Y}}}_t$ in (16) can be written as

$${\mathbf{{\rm Y}}}_t=\left[\begin{array}{ccc}{\mathbf{{\rm Y}}}_t^R& {\mathbf{{\rm Y}}}_t^v& {\mathbf{{\rm Y}}}_t^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(21)

and ${\mathbf{{\rm Y}}}_t$ is the solution of the differential Equation (22)

$${\dot{\mathbf{{\rm Y}}}}_t=\left[\begin{array}{ccc}{\mathbf{{\rm Y}}}_t^R{{\mathit{\omega }}_{ib}^b}^{\wedge }& {\mathbf{{\rm Y}}}_t^R{\mathit{a}}_{ib}^b& {\mathbf{{\rm Y}}}_t^v\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]=U{\mathbf{{\rm Y}}}_t+f({\mathbf{{\rm Y}}}_t)$$

(22)

where U is defined in (10). Based on (22), ${\mathbf{{\rm Y}}}_t$ is associated with the high-frequency input from the IMU, namely the acceleration ${\mathit{a}}_{ib}^b$ and angular velocity ${\mathit{\omega }}_{ib}^b$. It reflects the local increment during the motion process in the b-frame, and it can be used to pre-integrate. Similarly to ${\mathbf{L}}_0$, the initial value is also ${\mathbf{{\rm Y}}}_0={\mathbf{I}}_{5\times 5}$. It should be noted that both ${\mathbf{L}}_t$ and ${\mathbf{{\rm Y}}}_t$ depend on the initial state ${\mathbf{X}}_0$.

The state matrix ${\mathbf{X}}_t$ in (16) can be expanded as

$${\mathbf{X}}_t=\left[\begin{array}{ccc}{\mathbf{L}}_t^R{R}_0{\mathbf{{\rm Y}}}_t^R& {\mathbf{L}}_t^R(R_0{\mathbf{{\rm Y}}}_t^v+{\mathit{v}}_0)+{\mathbf{L}}_t^v& {\mathbf{L}}_t^R(R_0{\mathbf{{\rm Y}}}_t^p+{\mathit{p}}_0+t{\mathit{v}}_0)+{\mathbf{L}}_t^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(23)

where ${\mathbf{R}}_0$, ${\mathit{v}}_0$ and ${\mathit{p}}_0$ with their frame notations omitted here due to space constraints, correspond to ${\mathbf{R}}_{b,0}^e$, ${\mathit{v}}_{eb,0}^e$ and ${\mathit{p}}_{eb,0}^e$, separately.

Hereby, the paper establishes the equations for the continuous-time state matrix ${\mathbf{X}}_t$ in the e-frame.

2.3. The State Matrix on Li Group in Discrete Time

In practical implementations, state estimation systems inherently process discretized sensor inputs, such as high-frequency acceleration and angular velocity measurements sampled at discrete intervals. This operational constraint necessitates the discretization of the continuous formulation presented in (16).

The state ${\mathbf{X}}_t$ at the time k + 1 can be given as

$${\mathbf{X}}_{k+1}={\mathbf{L}}_{k+1}{\mathbf{\Phi }}_{k+1}({\mathbf{X}}_0){\mathbf{{\rm Y}}}_{k+1}$$

(24)

where ${\mathbf{L}}_{k+1}$ and ${\mathbf{{\rm Y}}}_{k+1}$ were defined in (19) and (21). According to (17), ${\mathbf{\Phi }}_{k+1}({\mathbf{X}}_0)$ can be written as

$${\mathbf{\Phi }}_{k+1}({\mathbf{X}}_0)=\left[\begin{array}{ccc}{\mathbf{R}}_0& {\mathit{v}}_0& {\mathit{p}}_0+t_k{\mathit{v}}_{ib}^e\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(25)

The initial objective focuses on deriving the temporal evolution of the global increment ${\mathbf{L}}_k$ across consecutive time steps. Through discretization of the continuous formulation in (20), the discrete-time representations of global increments ${\mathbf{L}}_{k+1}^R$, ${\mathbf{L}}_{k+1}^v$, and ${\mathbf{L}}_{k+1}^p$ at discrete time $t_{k+1}$ are expressed through the following relationships

$${\mathbf{L}}_{k+1}={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{L}}_k)$$

(26)

where

$${\mathbf{L}}_{\Delta t}=\left[\begin{array}{ccc}{\mathbf{L}}_{\Delta t}^R& {\mathbf{L}}_{\Delta t}^v& {\mathbf{L}}_{\Delta t}^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(27)

$${\mathbf{\Phi }}_{\Delta t}({\mathbf{L}}_k)=\left[\begin{array}{ccc}{\mathbf{L}}_k^R& {\mathbf{L}}_k^v& \Delta t{\mathbf{L}}_k^v+{\mathbf{L}}_k^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(28)

$$\begin{array}{c}{\mathbf{L}}_{\Delta t}^R={\Gamma }_0\left(-{\mathit{\omega }}_{ie}^e\Delta t\right)\\ {\mathbf{L}}_{\Delta t}^v={\Gamma }_1\left(-{\mathit{\omega }}_{ie}^e\Delta t\right)\Delta t{\mathit{G}}_{ib}^e\\ {\mathbf{L}}_{\Delta t}^p={\Gamma }_2\left(-{\mathit{\omega }}_{ie}^e\Delta t\right)\Delta t^2{\mathit{G}}_{ib}^e\end{array}$$

(29)

The detailed derivation of ${\mathbf{L}}_{\Delta t}$ can be found in Appendix A. The definition of function ${\Gamma }_m\left(\phi \right)$ is [29]

$${\Gamma }_m\left(\phi \right)\triangleq {\displaystyle \sum _{n=0}^{\infty }\frac{1}{\left(n+m\right)!}{\left({\phi }^{\wedge }\right)}^n},\phi \in {ℝ}^{3\times 1},m=0,1,2\dots$$

(30)

The above derivation quantifies the variation in the global increment across consecutive time intervals. Next, we now turn to the evolution of the local increment ${\mathbf{{\rm Y}}}_t$ over consecutive time intervals. Based on (22), ${\mathbf{{\rm Y}}}_t$ at time $t_{k+1}$ can be expressed as

$${\mathbf{{\rm Y}}}_{k+1}={\mathbf{\Phi }}_{\Delta t}({\mathbf{{\rm Y}}}_k){\mathbf{{\rm Y}}}_{\Delta t}$$

(31)

where

$${\mathbf{\Phi }}_{\Delta t}({\mathbf{{\rm Y}}}_k)=\left[\begin{array}{ccc}{\mathbf{{\rm Y}}}_k^R& {\mathbf{{\rm Y}}}_k^v& \Delta t{\mathbf{{\rm Y}}}_k^v+{\mathbf{{\rm Y}}}_k^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(32)

$${\mathbf{{\rm Y}}}_{\Delta t}=\left[\begin{array}{ccc}{\mathbf{{\rm Y}}}_{\Delta t}^R& {\mathbf{{\rm Y}}}_{\Delta t}^v& {\mathbf{{\rm Y}}}_{\Delta t}^p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(33)

where ${\mathbf{{\rm Y}}}_{\Delta t}^R$, ${\mathbf{{\rm Y}}}_{\Delta t}^v$ and ${\mathbf{{\rm Y}}}_{\Delta t}^p$ are the local increments between two consecutive discrete-time intervals $\Delta t$, and based on (22), ${\mathbf{{\rm Y}}}_{\Delta t}$ can be obtained as

$$\begin{array}{c}{\mathbf{{\rm Y}}}_{\Delta t}^R={\Gamma }_0\left({\mathit{\omega }}_{ib}^b\Delta t\right)\\ {\mathbf{{\rm Y}}}_{\Delta t}^v={\Gamma }_1({\mathit{\omega }}_{ib}^b\Delta t)\Delta t{\mathit{a}}_{ib}^b\\ {\mathbf{{\rm Y}}}_{\Delta t}^p={\Gamma }_2({\mathit{\omega }}_{ib}^b\Delta t)\Delta t^2{\mathit{a}}_{ib}^b\end{array}$$

(34)

The detailed derivation of ${\mathbf{{\rm Y}}}_{\Delta t}$ can be found in Appendix A, where ${\Gamma }_m\left(\phi \right)$ was defined in (30); the inputs ${\mathit{a}}_{ib}^b$ and ${\mathit{\omega }}_{ib}^b$ are assumed to be constant during each discrete-time interval $\Delta t$ and take the initial values at $t_k$. This assumption is reasonable and common when processing the IMU data.

By substituting (26) and (31) into (24), the discrete-time state equation can be derived as follows

$${\mathbf{X}}_{k+1}={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}\left({\mathbf{X}}_k\right){\mathbf{{\rm Y}}}_{\Delta t}$$

(35)

Proof:

$$\begin{array}{cc}{\mathbf{X}}_{k+1}& ={\mathbf{L}}_{k+1}{\mathbf{\Phi }}_{k+1}({\mathbf{X}}_0){\mathbf{{\rm Y}}}_{k+1}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{L}}_k){\mathbf{\Phi }}_{k+\Delta t}({\mathbf{X}}_0){\mathbf{\Phi }}_{\Delta t}({\mathbf{{\rm Y}}}_k){\mathbf{{\rm Y}}}_{\Delta t}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{L}}_k){\mathbf{\Phi }}_{\Delta t}({\mathbf{\Phi }}_k({\mathbf{X}}_0)){\mathbf{\Phi }}_{\Delta t}({\mathbf{{\rm Y}}}_k){\mathbf{{\rm Y}}}_{\Delta t}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{L}}_k{\mathbf{\Phi }}_k({\mathbf{X}}_0){\mathbf{{\rm Y}}}_k){\mathbf{{\rm Y}}}_{\Delta t}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{X}}_k){\mathbf{{\rm Y}}}_{\Delta t}\hfill \end{array}$$

(36)

where ${\mathbf{\Phi }}_t\left(\mathbf{X}\right)$ possesses another cumulative property with respect to time and the proof is presented as following

$$\begin{array}{cc}{\mathbf{\Phi }}_{t_i}({\mathbf{\Phi }}_{t_j}(\mathbf{X}))& ={\mathbf{\Phi }}_{t_i}(\mathbf{X}+t_{j}f(\mathbf{X}))\hfill \\ & =\mathbf{X}+t_{j}f(\mathbf{X})+t_{i}f(\mathbf{X}+f(\mathbf{X}))\hfill \\ & =\mathbf{X}+(t_i+t_j)f(\mathbf{X})={\mathbf{\Phi }}_{t_i+t_j}(\mathbf{X})\hfill \end{array}$$

(37)

As demonstrated in (35), we have established the discrete time propagation equation for the state matrix $\mathbf{X}$. This formulation exhibits intrinsic multiplicative properties that enable unified computation within the Lie group framework, in contrast to traditional EKFs that rely on additive error formulations. The updated state ${\mathbf{X}}_{k+1}$ comprises three geometrically interpretable components:

• Global increment ${\mathbf{L}}_{\Delta t}$ induced by Earth’s rotation.
• Prior state ${\mathbf{\Phi }}_{\Delta t}({\mathbf{X}}_k)$.
• Local increment ${\mathbf{{\rm Y}}}_{\Delta t}$ generated by IMU high-frequency inputs.

3. Invariant Error State in Earth Frame

3.1. Four Invariant Errors in E-Frame

Similarly to the state matrix X, the error state is also defined on the Lie group ${\mathrm{SE}}_2(3)$.

Definition 1

(invariant error [22]). The left- and right-invariant error matrices between the true state $\mathbf{X}\in {\mathrm{SE}}_2\left(3\right)$ and estimation $\widehat{\mathbf{X}}\in {\mathrm{SE}}_2\left(3\right)$ are defined as

Right-invariant error matrix:

$${\eta }^{r1}=\widehat{\mathbf{X}}{\mathbf{X}}^{-1}$$

(38)

Left-invariant error matrix:

$${\eta }^{l1}={\mathbf{X}}^{-1}\widehat{\mathbf{X}}$$

(39)

Invariance Property: The term “invariant” signifies a key geometric property of these definitions. Consider an arbitrary fixed transformation $C\in {\mathrm{SE}}_2\left(3\right)$. If both states are transformed by the same left-multiplication (i.e., $\mathbf{X}\to C\mathbf{X}$ and $\widehat{\mathbf{X}}\to C\widehat{\mathbf{X}}$), the computed error remains unchanged.

X and the estimate $\widehat{\mathbf{X}}$ by the same transformation C does not alter the error. Alternative definitions:

Right-invariant error matrix:

$${\eta }^{r1}=\widehat{\mathbf{X}}{\mathbf{X}}^{-1}=(C\widehat{\mathbf{X}}){(C\mathbf{X})}^{-1}$$

(40)

Left-invariant error matrix:

$${\eta }^{l1}={\mathbf{X}}^{-1}\widehat{\mathbf{X}}={(C\mathbf{X})}^{-1}(C\widehat{\mathbf{X}})$$

(41)

The introduction of the arbitrary matrix C in the above equalities is solely to demonstrate this invariance. It proves that the errors ${\eta }^{r1}$ and ${\eta }^{l1}$ depend only on the relative difference between $\widehat{\mathbf{X}}$ and X, and not on their absolute coordinates in the state space. This property is fundamental for the development of invariant filters, such as the MF-IEKF proposed in this work, which are known to exhibit improved convergence characteristics, especially under large misalignment conditions.

Exploiting this frame-invariance, one can equivalently define the errors inversely [30], leading to an alternative set of invariant errors:

Alternative right-invariant error matrix:

$${\eta }^{r2}=\mathbf{X}{\widehat{\mathbf{X}}}^{-1}=(C\mathbf{X}){(C\widehat{\mathbf{X}})}^{-1}$$

(42)

Alternative left-invariant error matrix:

$${\eta }^{l2}={\widehat{\mathbf{X}}}^{-1}\mathbf{X}={(C\widehat{\mathbf{X}})}^{-1}(C\mathbf{X}).$$

(43)

It is evident that error states ${\eta }^{r1}={({\eta }^{r2})}^{-1}$ and ${\eta }^{l1}={({\eta }^{l2})}^{-1}$ represent two opposing characterizations on the manifold.

3.2. Noise Model and Approximation

IMU measurements comprise three principal components: true kinematic values, sensor biases, and stochastic noise. These components can be mathematically modeled through the following

$${\tilde{\mathit{a}}}_{ib}^b={\mathit{a}}_{ib}^b+{\mathit{b}}_a+{\mathit{n}}_a$$

(44)

$${\tilde{\mathit{\omega }}}_{ib}^b={\mathit{\omega }}_{ib}^b+{\mathit{b}}_{\mathit{\omega }}+{\mathit{n}}_{\mathit{\omega }}$$

(45)

where $\tilde{\mathit{a}}$ and $\tilde{\mathit{\omega }}$ are the acceleration and gyroscope measurements expressed in the b-frame; ${\mathit{n}}_a$ and ${\mathit{n}}_{\mathit{\omega }}$ modeled as white Gaussian noise processes, satisfy ${\mathit{n}}_a\sim \mathcal{N}(0,{\mathit{\sigma }}_a^2)$ and ${\mathit{n}}_{\mathit{\omega }}\sim \mathcal{N}(0,{\mathit{\sigma }}_{\mathit{\omega }}^2)$, respectively; ${\mathit{b}}_a$ and ${\mathit{b}}_{\mathit{\omega }}$ denote the acceleration and gyroscope biases, whose temporal evolution follows Gaussian Markov processes.

$$\dot{\mathit{b}}=\left[\begin{array}{c}{\dot{\mathit{b}}}_a\\ {\dot{b}}_{\mathit{\omega }}\end{array}\right]=\left[\begin{array}{cc}-\mathrm{diag}({\displaystyle \frac{1}{{\tau }_a}})& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& -\mathrm{diag}({\displaystyle \frac{1}{{\tau }_{\mathit{\omega }}}})\end{array}\right]\left[\begin{array}{c}{\mathit{b}}_a\\ {\mathit{b}}_{\mathit{\omega }}\end{array}\right]+\left[\begin{array}{c}{n}_{b_a}\\ {\mathit{n}}_{b_{\mathit{\omega }}}\end{array}\right]={\mathbf{F}}_b\mathit{b}+{\mathit{n}}_b$$

(46)

The noise in the IMU is not negligible, and the effect of such noise can be characterized as a right perturbation on the local increment

$${\mathbf{{\rm Y}}}_{\Delta t}={\widehat{\mathbf{{\rm Y}}}}_{\Delta t}\exp (-{\mathit{N}}_{\Delta t})={\widehat{\mathbf{{\rm Y}}}}_{\Delta t}\exp (-{\mathbf{G}}_k{\mathit{n}}_k)$$

(47)

where ${\mathit{N}}_{\Delta t}$ is the perturbation and satisfies ${\mathit{N}}_{\Delta t}\sim \mathcal{N}({\mathbf{0}}_{9\times 1},{\mathbf{G}}_k\mathrm{cov}({\mathit{n}}_k){\mathbf{G}}_k^{\mathrm{T}})$; ${\mathit{n}}_k={\left[\begin{array}{cc}{\mathit{n}}_{\mathit{\omega }}^{\mathrm{T}}& {\mathit{n}}_a^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}$ defined in (44) and (45); and ${\mathbf{G}}_k\in {ℝ}^{9\times 6}$ is the Jacobi matrix with respect to the ${\mathit{n}}_k$, and can be written as

$${\mathbf{G}}_k=\left[\begin{array}{cc}{\Gamma }_0^{-1}{\Gamma }_1\Delta t& {\mathbf{0}}_{3\times 3}\\ -{\Gamma }_0^{-1}{\Gamma }_1{\hat{\mathit{a}}}_{ib}^b{}^{\wedge }\Gamma _2^{\mathrm{T}}\Delta t^2& {\Gamma }_0^{-1}{\Gamma }_1\Delta t\\ -{\Gamma }_0^{-1}{\Gamma }_2{\hat{\mathit{a}}}_{ib}^b{}^{\wedge }\Gamma _3^{\mathrm{T}}\Delta t^3& {\Gamma }_0^{-1}{\Gamma }_2\Delta t^2\end{array}\right]$$

(48)

where the detailed derivation of ${\mathbf{G}}_k$ can be found in Appendix B.

${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}$ corresponds to the local increment that encompasses uncertainty. Based on (34), ${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}$ can be expressed as

$${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}=\left[\begin{array}{ccc}{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R& {\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^v& {\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^p\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(49)

where

$$\begin{array}{c}{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R={\Gamma }_0({\hat{\mathit{\omega }}}_{ib}^b\Delta t)\\ {\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^v={\Gamma }_1({\hat{\mathit{\omega }}}_{ib}^b\Delta t){\hat{\mathit{a}}}_{ib}^b\Delta t\\ {\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^p={\Gamma }_2({\hat{\mathit{\omega }}}_{ib}^b\Delta t){\hat{\mathit{a}}}_{ib}^b\Delta t^2\end{array}$$

(50)

where ${\hat{\mathit{\omega }}}_{ib}^b={\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{b}}_{\mathit{\omega }}$, ${\hat{\mathit{a}}}_{ib}^b={\tilde{\mathit{a}}}_{ib}^b-{\mathit{b}}_a$.

3.3. Right- and Left-Invariant Error Kinematic Equations Based the Lie Algebra in Discrete Time

This section maps the error state matrices $\eta$ in the Lie group ((40)–(43)) to Lie algebras $\mathit{x}={[\begin{array}{ccc}{\mathit{x}}_{\phi }^{\mathrm{T}}& {\mathit{x}}_v^{\mathrm{T}}& {\mathit{x}}_p^{\mathrm{T}}\end{array}]}^{\mathrm{T}}\in {ℝ}^9$, where ${\mathit{x}}_{\phi }$, ${\mathit{x}}_v$, ${\mathit{x}}_p$ correspond to the rotation, velocity, and position errors, respectively. These mappings exhibit the following relationship

$$\eta =\exp (\mathit{x})=\mathrm{Exp}({\mathit{x}}^{\wedge })$$

(51)

Moreover, based on the definition of ${\mathbf{\Phi }}_t(\cdot )$ in (26), the log-linearity property can be straightforwardly verified as

$${({\mathbf{\Phi }}_t(\mathbf{X}))}^{-1}={\mathbf{\Phi }}_t({\mathbf{X}}^{-1})$$

(52)

and

$${\mathbf{\Phi }}_t(\exp (\mathit{x}))={\mathbf{\Phi }}_t(\mathrm{Exp}({\mathit{x}}^{\wedge }))=\mathrm{Exp}({\mathbf{F}}_t\mathit{x})$$

(53)

where ${\mathbf{F}}_t$ can be written as

$${\mathbf{F}}_t=\left[\begin{array}{ccc}{\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& {\mathbf{I}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathbf{0}}_{3\times 3}& t{\mathbf{I}}_{3\times 3}& {\mathbf{I}}_{3\times 3}\end{array}\right]$$

(54)

The adjoint operator of the state matrix X defined in (7) can be conveniently expressed as

$$\mathbf{A}{d}_{\mathbf{X}}:=\left[\begin{array}{ccc}\mathbf{R}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\\ {\mathit{v}}^{\wedge }\mathbf{R}& \mathbf{R}& {\mathbf{0}}_{3\times 3}\\ {\mathit{p}}^{\wedge }\mathbf{R}& {\mathbf{0}}_{3\times 3}& \mathbf{R}\end{array}\right]\in {ℝ}^{9\times 9}$$

(55)

Since (55) is a coordinate-agnostic mathematical definition, the subscripts and superscripts for R, v, and p are omitted.

And, it exhibits a useful property [40] as follows:

$$\mathbf{X}\exp (\mathit{x}){\mathbf{X}}^{-1}=\exp (\mathbf{A}{d}_{\mathbf{X}}\mathit{x})$$

(56)

The right-invariant error state matrix ${\eta }_{k+1}^{r1}$ in (40) can be further deduced as

$$\begin{array}{cc}{\eta }_{k+1}^{r1}& ={\widehat{\mathbf{X}}}_{k+1}{\mathbf{X}}_{k+1}^{-1}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\widehat{\mathbf{X}}}_k){\widehat{\mathbf{{\rm Y}}}}_{\Delta t}{\mathbf{X}}_{k+1}^{-1}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\eta }_k^{r1}{\mathbf{X}}_k){\mathbf{{\rm Y}}}_{\Delta t}\exp {(-{\mathit{N}}_{\Delta k})}^{-1}{\mathbf{X}}_{k+1}^{-1}\hfill \\ & ={\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\eta }_k^{r1}){\mathbf{L}}_{\Delta t}^{-1}\cdot {\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{X}}_k){\mathbf{{\rm Y}}}_{\Delta t}\exp {(-{\mathit{N}}_{\Delta k})}^{-1}{\mathbf{X}}_{k+1}^{-1}\hfill \\ & =\exp \left(\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{r1}\right)\exp (\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r1}}{\mathit{N}}_{\Delta k})\hfill \end{array}$$

(57)

Applying the BCH formula [44], the kinematic equation governing the prior right-invariant error in its Lie algebra form is given by

$${\mathit{x}}_{k+1}^{r1}{}^{-}\approx \mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{r1}{}^{+}+\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r1}}{\mathit{N}}_{\Delta k}$$

(58)

where superscript “−” denotes the prior error state, and $\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}=\left[\begin{array}{ccc}{\mathbf{L}}_{\Delta t}^R& 0_{3\times 3}& 0_{3\times 3}\\ {\mathbf{L}}_{\Delta t}^v{}^{\wedge }\mathbf{L}_{\Delta t}^R& {\mathbf{L}}_{\Delta t}^R& 0_{3\times 3}\\ {\mathbf{L}}_{\Delta t}^p{}^{\wedge }\mathbf{L}_{\Delta t}^R& 0_{3\times 3}& {\mathbf{L}}_{\Delta t}^R\end{array}\right]$, ${\mathbf{L}}_{\Delta t}^R$, ${\mathbf{L}}_{\Delta t}^v$ and ${\mathbf{L}}_{\Delta t}^p$ can be obtained from (29) and (40).

Obviously, another prior right-invariant error state ${\eta }_{k+1}^{r2}$ in (41) is the inverse of ${\eta }_{k+1}^{r1}$ in (40). Therefore, the error equations on the Lie group and Lie algebra can be given as

$${\eta }_{k+1}^{r2}=\exp (-\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r2}}{\mathit{N}}_{\Delta k})\exp (-\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{r2})$$

(59)

$${\mathit{x}}_{k+1}^{r2}{}^{-}\approx -\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{r2}-\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r2}}{\mathit{N}}_{\Delta k}$$

(60)

Similarly, for the two left-invariant errors in (42) and (43), the kinematic equations of the prior error states and vectors in discrete time can be formulated as

$$\begin{array}{cc}{\eta }_{k+1}^{l1}& ={\mathbf{X}}_{k+1}^{-1}{\widehat{\mathbf{X}}}_{k+1}\hfill \\ & ={({\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\mathbf{X}}_k){\mathbf{{\rm Y}}}_{\Delta t})}^{-1}{\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\widehat{\mathbf{X}}}_k){\widehat{\mathbf{{\rm Y}}}}_{\Delta t}\hfill \\ & ={\mathbf{{\rm Y}}}_{\Delta t}^{-1}{\mathbf{\Phi }}_{\Delta t}{({\mathbf{X}}_k)}^{-1}{\mathbf{L}}_{\Delta t}^{-1}{\mathbf{L}}_{\Delta t}{\mathbf{\Phi }}_{\Delta t}({\widehat{\mathbf{X}}}_k){\widehat{\mathbf{{\rm Y}}}}_{\Delta t}\hfill \\ & =\exp {(-{\mathit{N}}_{\Delta k})}^{-1}({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}){\mathbf{\Phi }}_{\Delta t}({\mathbf{X}}_k^{-1}{\widehat{\mathbf{X}}}_k){\widehat{\mathbf{{\rm Y}}}}_{\Delta t}\hfill \\ & =\exp ({\mathit{N}}_{\Delta k})\exp \left(\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{l1}\right)\hfill \end{array}$$

(61)

$${\mathit{x}}_{k+1}^{l1}{}^{-}\approx \mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{l1}{}^{+}+{\mathit{N}}_{\Delta k}$$

(62)

$${\eta }_{k+1}^{l2}{}^{-}=\exp (-\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{l2})\exp (-{\mathit{N}}_{\Delta k})$$

(63)

$${\mathit{x}}_{k+1}^{l2}{}^{-}\approx -\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}{\mathit{x}}_k^{l2}{}^{+}-{\mathit{N}}_{\Delta k}$$

(64)

where $\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}=\left[\begin{array}{ccc}{({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}& 0_{3\times 3}& 0_{3\times 3}\\ -{({({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^v)}^{\wedge }{({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}& {({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}& 0_{3\times 3}\\ -{({({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^p)}^{\wedge }{({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}& 0_{3\times 3}& {({\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R)}^{-1}\end{array}\right]$, ${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^R$, ${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^v$, and ${\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^p$ can be obtained from (50).

Combined with (46), the discretization of the bias error state can be written as

$$\delta {\mathit{b}}_{k+1}=\mathrm{Exp}({\mathbf{F}}_b\Delta t)\delta {\mathit{b}}_k+{\mathit{n}}_{b_k}$$

(65)

Let a new symbol $\mathit{\xi }$ denote the error state containing x and $\delta \mathit{b}$, $\mathit{\xi }={[\begin{array}{cc}{\mathit{x}}^{\mathrm{T}}& \delta {\mathit{b}}^{\mathrm{T}}\end{array}]}^{\mathrm{T}}\in {ℝ}^{15\times 1}$. By combining the error state equation and (65), the propagation equations for the two right-invariant errors (${\mathit{\xi }}^{r1}$ and ${\mathit{\xi }}^{r2}$) in the e-frame, which consider accelerometer and gyroscope bias errors, are as follows

$$\begin{array}{c}{\mathit{\xi }}_{k+1}^{r1}{}^{-}\approx {\mathbf{A}}_k^{r1}{\mathit{\xi }}_k^{r1}+{\mathbf{B}}_k^{r1}{\mathit{n}}_k^{\prime }\\ {\mathit{\xi }}_{k+1}^{r2}{}^{-}\approx {\mathbf{A}}_k^{r2}{\mathit{\xi }}_k^{r2}+{\mathbf{B}}_k^{r2}{\mathit{n}}_k^{\prime }\end{array}$$

(66)

where $\mathbf{A}\in {ℝ}^{15\times 15}$ is the transition matrix for the propagation of adjacent error states, and $\mathbf{B}\in {ℝ}^{15\times 12}$ is the transition matrix for the noise components, These matrices can be written as

$$\begin{array}{c}{\mathbf{A}}_k^{r1}=\left[\begin{array}{cc}\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}& \mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r1}}{\mathbf{G}}_k\\ {\mathbf{0}}_{6\times 9}& \mathrm{Exp}({\mathbf{F}}_b\Delta t)\end{array}\right],{\mathbf{B}}_k^{r1}=\left[\begin{array}{cc}\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r1}}{\mathbf{G}}_k& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{6\times 6}& {\mathbf{I}}_{6\times 6}\end{array}\right],\\ {\mathbf{A}}_k^{r2}=\left[\begin{array}{cc}-\mathbf{A}{d}_{{\mathbf{L}}_{\Delta t}}{\mathbf{F}}_{\Delta t}& -\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r2}}{\mathbf{G}}_k\\ {\mathbf{0}}_{6\times 9}& \mathrm{Exp}({\mathbf{F}}_b\Delta t)\end{array}\right],{\mathbf{B}}_k^{r2}=\left[\begin{array}{cc}\mathbf{A}{d}_{{\mathbf{X}}_{k+1}^{r2}}{\mathbf{G}}_k& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{6\times 6}& {\mathbf{I}}_{6\times 6}\end{array}\right],\\ {\mathit{n}}_k^{\prime }={\left[\begin{array}{cccc}{{\mathit{n}}_{\mathit{\omega }}}^{\mathrm{T}}& {{\mathit{n}}_a}^{\mathrm{T}}& {{\mathit{n}}_{b_{\mathit{\omega }}}}^{\mathrm{T}}& {{\mathit{n}}_{b_a}}^{\mathrm{T}}\end{array}\right]}^{\mathrm{T}}.\end{array}$$

It is clear that the error state transition matrix A in (66) is independent of the current trajectory, even when bias is taken into account. The covariances matrix of the propagation can be given as

$$\begin{array}{c}{\mathbf{P}}_{k+1}^{r1}{}^{-}={\mathbf{A}}_k^{r1}{\mathbf{P}}_k^{r1}{\mathbf{A}}_k^{r1}{}^{\mathrm{T}}+{\mathbf{B}}_k^r{\mathbf{Q}}_k^r{{\mathbf{B}}_k^r}^{\mathrm{T}}\\ {\mathbf{P}}_{k+1}^{r2}{}^{-}={\mathbf{A}}_k^{r2}{\mathbf{P}}_k^{r2}{\mathbf{A}}_k^{r2}{}^{\mathrm{T}}+{\mathbf{B}}_k^r{\mathbf{Q}}_k^r{{\mathbf{B}}_k^r}^{\mathrm{T}}\end{array}$$

(67)

where ${\mathbf{Q}}_k$ represents the noise covariance matrix.

Similarly, the left-invariant errors ${\mathit{\xi }}^l$ covariance models can be written as

$$\begin{array}{c}{\mathit{\xi }}_{k+1}^{l1}{}^{-}\approx {\mathbf{A}}_k^{l1}{\mathit{\xi }}_k^{l1}+{\mathbf{B}}_k^l{n}_k^{\prime }\\ {\mathit{\xi }}_{k+1}^{l2}{}^{-}\approx {\mathbf{A}}_k^{l2}{\mathit{\xi }}_k^{l2}+{\mathbf{B}}_k^l{n}_k^{\prime }\end{array}$$

(68)

$$\begin{array}{c}{\mathbf{P}}_{k+1}^{l1}{}^{-}={\mathbf{A}}_k^{l1}{\mathbf{P}}_k^{l1}{\mathbf{A}}_k^{l1}{}^{\mathrm{T}}+{\mathbf{B}}_k^l{\mathbf{Q}}_k^l{{\mathbf{B}}_k^l}^{\mathrm{T}}\\ {\mathbf{P}}_{k+1}^{l2}{}^{-}={\mathbf{A}}_k^{l2}{\mathbf{P}}_k^{l2}{\mathbf{A}}_k^{l2}{}^{\mathrm{T}}+{\mathbf{B}}_k^l{\mathbf{Q}}_k^l{{\mathbf{B}}_k^l}^{\mathrm{T}}\end{array}$$

(69)

where the state transition matrices and noise transition matrix be expressed as

$$\begin{array}{c}{\mathbf{A}}_k^{l1}=\left[\begin{array}{cc}\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}& {\mathbf{G}}_k\\ {\mathbf{0}}_{6\times 9}& \mathrm{Exp}({\mathbf{F}}_b\Delta t)\end{array}\right],{\mathbf{B}}_k^l=\left[\begin{array}{cc}{\mathbf{G}}_k& {\mathbf{0}}_{9\times 6}\\ {\mathbf{0}}_{6\times 6}& {\mathbf{I}}_{6\times 6}\end{array}\right],\\ {\mathbf{A}}_k^{l2}=\left[\begin{array}{cc}-\mathbf{A}{d}_{{\widehat{\mathbf{{\rm Y}}}}_{\Delta t}^{-1}}{\mathbf{F}}_{\Delta t}& -{\mathbf{G}}_k\\ {\mathbf{0}}_{6\times 9}& \mathrm{Exp}({\mathbf{F}}_b\Delta t)\end{array}\right]\end{array}$$

3.4. GNSS Observation Update

In the INS-based prediction process, we have defined four error states and their discrete-time error propagation equations. During the GNSS observation process, the right- and left-invariant positional error models can be written as

$$\begin{array}{c}{\mathit{z}}_{k+1}=H_{k+1}^{r1}{\mathit{\xi }}_{k+1}^{r1}{}^{-}+{\mathit{v}}_{obser}^e\\ {\mathit{z}}_{k+1}=H_{k+1}^{r2}{\mathit{\xi }}_{k+1}^{r2}{}^{-}+{\mathit{v}}_{obser}^e\end{array}$$

(70)

$$\begin{array}{c}{\mathit{z}}_{k+1}=H_{k+1}^{l1}{\mathit{\xi }}_{k+1}^{l1}{}^{-}+{\mathit{v}}_{obser}^e\\ {\mathit{z}}_{k+1}=H_{k+1}^{l2}{\mathit{\xi }}_{k+1}^{l2}{}^{-}+{\mathit{v}}_{obser}^e\end{array}$$

(71)

where $H^{(\cdot )}$ is the Jacobian matrix associated with the GNSS observation model.

$$\begin{array}{c}{H}^{l1}=[\begin{array}{ccccc}{\mathbf{R}}_b^e{\mathit{l}}^{\wedge }& {\mathbf{0}}_{3\times 3}& -{\mathbf{R}}_b^e{J}^{l1}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}]\\ H^{l2}=[-\begin{array}{ccccc}{\mathbf{R}}_b^e{\mathit{l}}^{\wedge }& {\mathbf{0}}_{3\times 3}& {\mathbf{R}}_b^e{J}^{l2}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}]\end{array}$$

(72)

$$\begin{array}{c}{H}^{r1}=[-\begin{array}{ccccc}{({\mathit{p}}_{ib}^e+{\mathbf{R}}_b^e\mathit{l})}^{\wedge }& {\mathbf{0}}_{3\times 3}& J^{r1}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}]\\ H^{r2}=[\begin{array}{ccccc}{({\mathit{p}}_{ib}^e+{\mathbf{R}}_b^e\mathit{l})}^{\wedge }& {\mathbf{0}}_{3\times 3}& -J^{r2}& {\mathbf{0}}_{3\times 3}& {\mathbf{0}}_{3\times 3}\end{array}]\end{array}$$

(73)

The detailed derivations are presented in Appendix C. l is the lever arm vector. ${\mathit{v}}_{obser}^e$ denotes the observation noise, which satisfies ${\mathit{v}}_{obser}^e\sim \mathcal{N}(0,\mathbf{V})$. The calculation of the Kalman gain $K_{k+1}^{(\cdot )}$, error ${{\mathit{\xi }}_{k+1}^{(\cdot )}}^{+}$ and covariance ${{\mathbf{P}}_{k+1}^{(\cdot )}}^{+}$ at time $t_{k+1}$ can be expressed as

$$\begin{array}{c}{K}_{k+1}^{(\cdot )}={{\mathbf{P}}_{k+1}^{(\cdot )}}^{-}{H_{k+1}^{(\cdot )}}^{\mathrm{T}}{(H_{k+1}^{(\cdot )}{{\mathbf{P}}_{k+1}^{(\cdot )}}^{-}{H}_{k+1}^{(\cdot )}{}^{\mathrm{T}}+{\mathbf{V}}_{k+1})}^{-1}\\ {\mathit{\xi }}_{k+1}^{(\cdot )}{}^{+}=K_{k+1}^{(\cdot )}{\mathit{z}}_{k+1}^{(\cdot )}\\ {\mathbf{P}}_{k+1}^{(\cdot )}{}^{+}=(\mathbf{I}-K_{k+1}^{(\cdot )}{H}_{k+1}^{(\cdot )}){{\mathbf{P}}_{k+1}^{(\cdot )}}^{-}\end{array}$$

(74)

where superscript ^(⋅) represents the different error models from (40)–(43) and “+” denotes the posterior state. It should be noted that the error vector ${{\mathit{\xi }}_{k+1}^{(\cdot )}}^{+}$ includes error state and biases $\mathit{\xi }={[\begin{array}{cc}{\mathit{x}}^{\mathrm{T}}& \delta {\mathit{b}}^{\mathrm{T}}\end{array}]}^{\mathrm{T}}$. We can restore the true state in the e-frame, by the exponential mapping from the Lie group to Lie algebra, and the right-invariant errors are defined as

$$\begin{array}{c}{\mathbf{X}}_{k+1}^{r1}={\eta }_{k+1}^{r1}{\widehat{\mathbf{X}}}_{k+1}^{r1}=\exp {({\mathit{x}}_{k+1}^{r1}{}^{+})}^{-1}{\widehat{\mathbf{X}}}_{k+1}^{r1}\\ {\mathbf{X}}_{k+1}^{r2}={\widehat{\mathbf{X}}}_{k+1}^{r2}{\eta }_{k+1}^{r2}{}^{-1}=\exp ({\mathit{x}}_{k+1}^{r2}{}^{+}){\widehat{\mathbf{X}}}_{k+1}^{r2}\end{array}$$

(75)

The definitions of two left-invariant errors are given by

$$\begin{array}{c}{\mathbf{X}}_{k+1}^{l1}={\widehat{\mathbf{X}}}_{k+1}^{l1}{\eta }_{k+1}^{l1}={\widehat{\mathbf{X}}}_{k+1}^{l1}\exp {(x_{k+1}^{l1}{}^{+})}^{-1}\\ {\mathbf{X}}_{k+1}^{l2}={\eta }_{k+1}^{l2}{\widehat{\mathbf{X}}}_{k+1}^{l2}{}_{k+1}{}^{-1}={\widehat{\mathbf{X}}}_{k+1}^{l2}\exp (x_{k+1}^{l2}{}^{+})\end{array}$$

(76)

where

$$\exp ({\mathit{x}}^{(\cdot )})=\left[\begin{array}{ccc}\exp ({\mathit{x}}_{\phi })& J^{(\cdot )}{\mathit{x}}_v& J^{(\cdot )}{\mathit{x}}_p\\ {\mathbf{0}}_{1\times 3}& 1& 0\\ {\mathbf{0}}_{1\times 3}& 0& 1\end{array}\right]$$

(77)

where $\exp ({\mathit{x}}_{\phi })={\Gamma }_0({\mathit{x}}_{\phi })$ is the attitude error matrix in the Lie group; $J{\mathit{x}}_v$ and $J{\mathit{x}}_p$ represent the velocity error and position error, respectively; and $J^{(\cdot )}$ is the left Jacobian matrix, and $J^{(\cdot )}={\Gamma }_1({\mathit{x}}_{\phi })$.

3.5. The New Errors Based on Multiple Invariant Errors

Based on the definitions of the four invariant error matrices in (40)–(43), multiplying ${\eta }_t^{r1}$ by ${\eta }_t^{r2}$ and ${\eta }_t^{l1}$ by ${\eta }_t^{l2}$ yields the following expression

$$\begin{array}{c}{\eta }_k^{l1}{\eta }_k^{l2}\triangleq {\mathbf{I}}_{5\times 5}\\ {\eta }_k^{r1}{\eta }_k^{r2}\triangleq {\mathbf{I}}_{5\times 5}\end{array}$$

(78)

According the mapping relationship between Lie groups and Lie algebras given in (51), (78) can be mapped to the Lie algebra, resulting in an equivalent error vector identity as follows

$$\begin{array}{c}{\mathit{x}}_{k+1}^{l1}\triangleq -{\mathit{x}}_{k+1}^{l2}\\ {\mathit{x}}_{k+1}^{r1}\triangleq -{\mathit{x}}_{k+1}^{r2}\end{array}$$

(79)

However, in practical operation, numerical errors, linearization and discretization approximations, and model mismatch prevent the theoretical symmetry (79) from holding exactly. To exploit this theoretical constraint and thus enhance estimation accuracy, we propose a lightweight post-processing correction strategy. Following each measurement update, we correct the error vector to mitigate random deviations induced by numerical errors while preserving the core filter structure. Since it is impractical to determine a priori which of ${\mathit{x}}_{k+1}^1$ and ${\mathit{x}}_{k+1}^2$ is superior, we adopt a symmetric fusion strategy: for the left-invariant error, the corrected vector ${\mathit{x}}_{k+1}^{l1\ast }$ retains the sign of ${\mathit{x}}_{k+1}^{l1}$ while its magnitude is set to the weighted average of their absolute values,

$${\mathit{x}}_{k+1}^{l1}{}^{*}=sign({\mathit{x}}_{k+1}^{l1}{}^{+})\odot \left(w\left|{{\mathit{x}}_{k+1}^{l1}}^{+}\right|+(1-w)\left|{{\mathit{x}}_{k+1}^{l2}}^{+}\right|\right)$$

(80)

where $sign(\cdot )$ denotes the sign function, $\odot$ represents the element-wise multiplication operator, and $w$ is a user-defined weight coefficient (here set to $w$ = 0.5). As the correction magnitude is typically much smaller than the uncertainty range encoded in the covariance matrix P (experimentally, $‖{{\mathit{x}}_{k+1}^{l1}}^{*}‖/{\mathbf{P}}_{k+1}$ averages about 0.08), its impact on covariance consistency is negligible.

The core significance of this method is that it converts the theoretical symmetry of left-invariant errors (79) into a lightweight, practical correction tool. By fusing the magnitude information of both errors, we effectively curb random deviations without modifying the core filter structure, leading to improved long-term stability and accuracy. Experiments show that this lightweight post-processing step markedly reduces the root-mean-square error of state estimation, underscoring the practical benefits of exploiting geometric constraints to improve filter robustness.

The proposed correction, as a lightweight post-processing step, enhances practicality while introducing inherent trade-offs between optimality and computational efficiency. First, algorithmic simplicity and real-time constraints preclude a concurrent covariance update, causing a deliberate departure from the classical Kalman optimality framework and constituting a moderate compromise on theoretical rigor. Second, the method presupposes predominantly stochastic error deviations; performance degrades under pronounced systematic biases or severe dynamic mismatches. These limitations define the method’s applicability and motivate future work on approximate covariance updates or adaptive fusion weights.

Nevertheless, in the context of INS/GNSS integrated estimation under large misalignment angles, the corrected error vector remains subject to the inherent limitations of left-invariant errors. Specifically, the observation matrix H^l defined in (72) contains the attitude matrix ${\mathbf{R}}_b^e$. This introduces attitude-induced errors into the position and velocity estimates and consequently degrades the estimation performance under large misalignment conditions. Although the multiplicative right-IEKF generally exhibits lower overall accuracy and robustness compared to the multiplicative left-IEKF [40], under large misalignment angles, it demonstrates faster convergence speed and higher estimation accuracy for position and velocity during the initial stage. As the matrix H^r (73) does not contain the attitude matrix, thereby mitigating the influence of attitude errors caused by large misalignments. Based on this analysis, this study further combines the advantages of the corrected left-invariant error ${{\mathit{x}}_{k+1}^{l1}}^{*}$ and the right-invariant error ${\mathit{x}}_{k+1}^{r1}$ to propose a multiplicative federated IEKF (MF-IEKF). The new method first employs the multiplicative right₁-IEKF in the initial phase and then switches to the multiplicative corrected left-IEKF (MCL-IEKF) to continue the task. The federated invariant error can be written as

$${\mathit{x}}_{k+1}^{\mathbf{F}}(i)=\left\{\begin{array}{cc}{\mathit{x}}_{k+1}^{r1}(i)& t_{k+1}<t_{switch}\\ {\mathit{x}}_{k+1}^{l1}{}^{*}(i)& t_{k+1}>t_{switch}\end{array}\right.$$

(81)

where $t_{switch}$ denotes the switching time, also called the degradation time of MR₁-IEKF, and is set to 10 s in this study. Although MR₁-IEKF excels at the outset, ML-IEKF achieves superior performance over the entire duration. Consequently, the optimal switching instant coincides with the crossover point where both estimators exhibit equivalent accuracy. The choice of $t_{switch}$ in our experiments resulted from a comprehensive trade-off and is not necessarily the global optimum; this choice does not undermine the demonstrated superiority of MF-IEKF.

Finally, the MF-IEKF for the INS/GNSS can be expressed as Algorithm 1.

Algorithm 1: MF-IEKF for the INS/GNSS

Input: ${\mathbf{X}}_0\left({\mathbf{R}}_{b,}^e{}_0,{\mathit{v}}_{ib,}^e{}_0,{{\mathit{p}}_{ib}^e}_{,0}\right)$, $\left({\mathit{b}}_{\mathit{\omega },0},{\mathit{b}}_{a,0}\right)$, $\left({\tilde{\mathit{\omega }}}_{ib,k}^b,{\tilde{\mathit{a}}}_{ib,k}^b\right)$, ${\mathbf{P}}_0$

Output: ${\mathbf{X}}_k\left({\mathbf{R}}_{b,}^e{}_k,{\mathit{v}}_{ib,}^e{}_k,{{\mathit{p}}_{ib}^e}_{,k}\right)$.

1: 1. Initialization:

2: ${\mathbf{X}}_0=\mathrm{E}\left[\left({\mathbf{X}}_0\right)\right]$, ${\mathbf{P}}_0=\mathrm{E}\left[\left({\mathbf{X}}_0-{\widehat{\mathbf{X}}}_0\right){\left({\mathbf{X}}_0-{\widehat{\mathbf{X}}}_0\right)}^{\mathrm{T}}\right]$

3: 2. State prediction at $t_{k+1}$:

4:   Priori invariant error and covariance propagation equation:

5:   ${\mathit{\xi }}_{k+1}^{(\cdot )}{}^{-}\approx {\mathbf{A}}_k^{(\cdot )}{\mathit{\xi }}_k^{(\cdot )}+{\mathbf{B}}_k^{(\cdot )}{n_k}^{\prime }$ 6:   ${\mathbf{P}}_{k+1}^{(\cdot )}{}^{-}={\mathbf{A}}_k^{(\cdot )}{\mathbf{P}}_k^{(\cdot )}{\mathbf{A}}_k^{(\cdot )}{}^{\mathrm{T}}+{\mathbf{B}}_k^{(\cdot )}{\mathbf{Q}}_k^{(\cdot )}{{\mathbf{B}}_k^{(\cdot )}}^{\mathrm{T}}$

7:   ${\mathbf{A}}_{k+1}^{(\cdot )},{\mathbf{B}}_{k+1}^{(\cdot )}$ matrices can be refer to Equations (66) and (68).

8: 3. State update at $t_{k+1}$:

9:    While GNSS observation arrive:

10:    Kalman gain, posteriori invariant error and covariance calculation:

11:   $K_{k+1}^{(\cdot )}={\mathbf{P}}_{k+1}^{-}{H_{k+1}^{(\cdot )}}^{\mathrm{T}}{(H_{k+1}^{(\cdot )}{{\mathbf{P}}_{k+1}^{(\cdot )}}^{-}{H}_{k+1}^{(\cdot )}{}^{\mathrm{T}}+{\mathbf{V}}_{k+1})}^{-1}$ 12:   ${\mathit{\xi }}_{k+1}^{(\cdot )}{}^{+}=K_{k+1}^{(\cdot )}{\mathit{z}}_{k+1}^{(\cdot )}$ 13:   ${\mathbf{P}}_{k+1}^{(\cdot )}{}^{+}=(\mathbf{I}-{{\rm K}}_{k+1}^{(\cdot )}{H}_{k+1}^{(\cdot )}){{\mathbf{P}}_{k+1}^{(\cdot )}}^{-}$

14:   If $t_{k+1}<t_{switch}$, process sub-filter MR1-IEKF, the error calculation:

15:   ${\mathit{\xi }}_{k+1}{}^{+}={{\mathit{\xi }}_{k+1}^{r1}}^{+}$

16:   else, process sub-filter MCL-IEKF, the error calculation:

17:   ${\mathit{\xi }}_{k+1}{}^{+}={\mathit{\xi }}_{k+1}^{l1*}{}^{+}=\left[\begin{array}{cc}{{\mathit{x}}_{k+1}^{l1}}^{*}& \delta {\mathit{b}}_{k+1}^{\mathrm{T}}\end{array}\right]$

18:   ${\mathit{x}}_k^{l1*}$ can be refer to Equation (80).

19: 4. Global state update:

20:   If $t_{k+1}<t_{switch}$, ${\mathbf{X}}_{k+1}={\mathbf{X}}_{k+1}^{r1}=\exp {({\mathit{x}}_{k+1}^{r1}{}^{+})}^{-1}{\widehat{\mathbf{X}}}_{k+1}^{r1}$

21:   else ${\mathbf{X}}_{k+1}={\mathbf{X}}_{k+1}^{l1}={\widehat{\mathbf{X}}}_{k+1}^{l1}\exp {(x_{k+1}^{l1*}{}^{+})}^{-1}$

22:   return ${\mathbf{X}}_{k+1}$, ${{\mathbf{P}}_{k+1}^{(\cdot )}}^{+}$

23:   End while

4. Experiments

This section evaluates the accuracy of the algorithms based on different error models through INS/GNSS simulation experiments and open source dataset experiments. A comparison was conducted among the following five algorithms:

The method based on the right- invariant error defined in (40), denoted as MR₁-IEKF.

The method based on the left-invariant error defined in (58) is denoted as ML₁-IEKF, which corresponds to the framework presented in [42].

The method based on the left-invariant error defined in (62), denoted as ML₂-IEKF.

The method based on the corrected left-invariant error defined in (80), denoted as MCL-IEKF.

The method federated invariant error defined in (81), denoted as MF-IEKF. Note that the MCL-IEKF is a sub-filter of the MF-IEKF.

4.1. Numerical Simulations

IMU and GNSS data used in the simulations were generated by the open-source platform gnss-ins-sim [45]. The toolbox supports high-, medium-, and low-grade IMU configurations, together with a low-precision GNSS receiver model. To balance the validity of algorithm performance evaluation with the reasonableness of simulation conditions, a medium-grade IMU model was selected for this study. Its parameters are specified as follows: a gyroscope bias of 3.5$°/\mathrm{h}$ and an angular random walk of 0.25$°/\sqrt{\mathrm{h}}$; an accelerometer bias of 5 mg and a velocity random walk of 3 $\mathrm{mg}/\sqrt{\mathrm{Hz}}$. A low-precision GNSS receiver was simulated with positional noise standard deviations of σ = (5, 5, 7) m. The INS and GNSS output rates are 100 Hz and 10 Hz, respectively. The initial plat-form misalignment angles were set to [30°, 30°, 10°]. Figure 2 illustrates the reference trajectory, which includes acceleration, constant-speed cruising, and turning segments to emulate realistic vehicle dynamics.

To obtain statistically meaningful results, 500 independent Monte Carlo trials were conducted. Each trial lasted 105 s, sufficient to capture the full transition from large initial misalignment to filter convergence. All estimators shared identical initial conditions: a zero error-state vector and an identity covariance matrix.

To quantify the estimation accuracy of the algorithms in position, velocity, and attitude, the root mean square error (RMSE) was calculated, respectively, using the following formula

$$\mathrm{RMSE}(k)=\sqrt{{\displaystyle \frac{1}{n}}{\displaystyle \sum _{i=1}^n{(y_{k,i}-{\widehat{y}}_{k,i})}^2}}$$

(82)

where k denotes the time step, n is the number of Monte Carlo simulations, n = 500, and $y_{k,i}$ ${\widehat{y}}_{k,i}$ represent the reference value and estimated value in the i-th simulation at k-th time step, respectively.

Figure 3, Figure 4 and Figure 5 present the three-axis RMSE curves for attitude, velocity, and position, respectively, obtained from the five algorithms over 500 Monte Carlo runs. As shown in Figure 3, the attitude RMSE distributions exhibit minor variations across the algorithms, requiring further quantitative analysis. Figure 4 reveals that the velocity RMSE curves of the two ML₁- and ML₂-IEKF are significantly higher overall than that of MCL-IEKF and MF-IEKF. Magnified views of the y- and z-axes clearly indicate that the RMSE of MCL-IEKF, which combines two ML-IEKFs, is noticeably higher than that of MF-IEKF, which incorporates MR₁-IEKF. The position RMSE distributions in Figure 5 further support these findings. In the enlarged views of the x- and y-axes, the RMSE of MCL-IEKF remains markedly higher than that of MF-IEKF. Moreover, the magnified sections in Figure 4 and Figure 5 demonstrate that both MR₁-IEKF and MF-IEKF achieve significantly faster convergence in velocity and position estimation compared to the ML-IEKF-based methods, which aligns with the theoretical analysis in Section 3 of the paper.

To facilitate a direct quantification and comparison of algorithm performance, the mean of RMSEs (MRMSE) is also computed as follows

$$\mathrm{MRMSE}=\sqrt{{\displaystyle \frac{1}{n}}{\displaystyle \sum _{k=1}^n\left(\frac{1}{3}{\displaystyle \sum _{i=1}^3{\left({\mathrm{RMSE}}_i(k)\right)}^2}\right)}}$$

(83)

where n denotes the total time step, and i = 1, 2, 3 corresponding to the X-Y-Z axes, respectively.

The MRMSE values in

Table 1. The MRMSEs of state in the Monte Carlo experiments.

	MR1-IEKF	ML1-IEKF	ML2-IEKF	MCL-IEKF	MF-IEKF
Attitude (rad)	0.077	0.076	0.074	0.066	0.065
velocity (m/s)	2.012	4.186	4.660	1.590	1.530
position (m)	7.67	5.809	5.911	5.037	4.921

quantitatively confirm this qualitative assessment. The proposed MF-IEKF achieves superior accuracy across all metrics. As a sub-filter, the MCL-IEKF itself demonstrates significant improvements over the baseline ML₁-IEKF, with MRMSE reductions of 13.15% in attitude, 62.01% in velocity, and 13.28% in position. Furthermore, the complete MF-IEKF framework, which incorporates MR₁-IEKF, achieves additional performance gains, ultimately reducing attitude, velocity, and position errors by 13.86%, 63.43%, and 15.28%, respectively, compared to the ML₁-IEKF baseline.

4.2. Open Source Dataset Experiments

To validate the proposed algorithm under real-world conditions, the widely used KITTI odometry dataset [46] was employed. The data were collected by a vehicle equipped with an OXTS RT3003 high-precision integrated navigation system, which provides time-synchronized IMU (100 Hz) and GNSS (10 Hz) measurements. The associated sensor noise parameters are as follows: gyroscope bias of 1.5$°/\mathrm{h}$ and angular random walk of 0.15$°/\sqrt{\mathrm{h}}$; accelerometer bias of 0.1 mg and velocity random walk of 0.05 $\mathrm{mg}/\sqrt{\mathrm{Hz}}$; GNSS position measurement noise standard deviations of (0.02, 0.02, 0.05) m.

Sequence 2011_10_03_drive_0027 (duration ≈ 8 min) was selected for evaluation. This sequence covers typical urban, residential, and suburban driving environments, presenting rich motion dynamics and high representativeness; its trajectory is illustrated in Figure 6. To ensure a fair comparison, all filters were initialized under identical conditions: a zero error-state vector and an identity covariance matrix. The state update cycle was strictly synchronized with the timestamps of the dataset.

$$\mathrm{AE}(k)=\left|y_k-{\widehat{y}}_k\right|$$

(84)

Figure 6 depicts the ground truth trajectory used in the dataset-based experiments. Figure 7, Figure 8 and Figure 9 present the absolute error (AE) distribution curves, as defined in (84), for attitude, velocity, and position estimates across the evaluated algorithms. As observed in Figure 7, the MF-IEKF achieves a notably lower AE in pitch angle compared to the other methods. The magnified view of velocity estimates in Figure 8 further demonstrates that the MF-IEKF maintains the lowest AE throughout the entire time period. Similarly, the enlarged section of position estimates in Figure 9 reveals a consistent trend, confirming that MF-IEKF has a faster convergence speed in the initial stage compared to ML-IEKF.

It should be noted that the sensors used in this dataset experiment exhibit high precision, which enables both ML₁-IEKF and ML₂-IEKF to also achieve reasonably accurate estimates. Under these conditions, the performance advantage of MCL-IEKF and MF-IEKF remains relatively limited outside the regions highlighted in the magnified views. Therefore, to enable an objective assessment of algorithm performance, a quantitative comparison based on the RMSE is necessary. The RMSE is calculated using the following formula

$$\mathrm{RMSE}=\sqrt{{\displaystyle \frac{1}{3}}{\displaystyle \sum _{i=1}^3\left(\frac{1}{n}{\displaystyle \sum _{k=1}^n{(y_{i,k}-{\widehat{y}}_{i,k})}^2}\right)}}$$

(85)

where n denotes the total time step, $y_k$ and ${\widehat{y}}_k$ represent the reference value and estimated value at k-th time step, and i = 1, 2, 3 corresponding to the X-Y-Z axes, respectively.

The RMSE results are summarized in

Table 2. The RMSEs of state in the open source dataset experiments.

	MR1-IEKF	ML1-IEKF	ML2-IEKF	MCL-IEKF	MF-IEKF
Attitude (rad)	0.026	0.019	0.019	0.018	0.018
velocity (m/s)	9.380	0.344	0.342	0.324	0.314
position (m)	4.865	1.003	1.069	0.943	0.642

. Compared to the baseline ML₁-IEKF, the MCL-IEKF sub-filter achieves reductions in RMSE of 5.26% in attitude, 5.81% in velocity, and 5.98% in position. Building upon MCL-IEKF, the MF-IEKF delivers further enhancements, reducing the RMSE by an additional 3% in velocity and 31.63% in position relative to MCL-IEKF. This improvement is attributed to the incorporation of MR₁-IEKF results during the initial phase. Notably, the MR₁-IEKF exhibits markedly greater errors in velocity and position estimation. This stems from its inherent alignment with a local coordinate frame, in contrast to the ML-IEKF, which is naturally suited to the global frame of GNSS observations [42]. This performance disparity is further amplified under high-dynamic conditions. Overall, compared to ML₁-IEKF, the MF-IEKF achieves total error reductions of 5.26% in attitude, 8.72% in velocity, and 35.69% in position. These results conclusively demonstrate the effectiveness of the proposed federated framework.

4.3. Summary and Analysis of Experiment Results

This section presents a comprehensive evaluation of five IEKF-based algorithms through rigorous numerical simulations and real-world dataset validation. The proposed MF-IEKF demonstrates superior performance in both controlled Monte Carlo simulations and KITTI dataset experiments. In simulated environments with large initial misalignments, the framework achieves statistically significant improvements in velocity and position estimation accuracy and convergence speed, attributable to its synergistic use of invariant filters. These quantitative advantages are further validated in real-world scenarios using the KITTI dataset, where MF-IEKF maintains excellent performance even under high-precision sensor conditions. This consistency verifies the algorithm’s effectiveness across different platforms and noise characteristics. The experimental results collectively confirm that the federated architecture, which strategically leverages both left- and right-invariant error models, effectively enhances the precision and reliability of INS/GNSS integrated navigation in challenging scenarios.

5. Conclusions

An MF-IEKF framework is proposed for INS/GNSS integration. It enhances estimation accuracy compared to baseline ML₁-IEKF methods, demonstrating faster convergence in velocity and position estimation, particularly under large misalignment angles. The framework is built upon multiplicative pre-integration theory and introduces two core contributions: First, it operates ML₁-IEKF and ML₂-IEKF in parallel, leveraging the inverse relationship between their left-invariant error vectors mapped onto the Lie algebra to perform collaborative error correction. This yields the MCF-IEKF sub-filter, which improves steady-state accuracy. Second, during the initial phase, it incorporates an MR₁-IEKF sub-filter to effectively suppress velocity and position errors induced by large misalignment angles, significantly enhancing both the convergence speed and accuracy of velocity and position estimates throughout the initial alignment process. Extensive Monte Carlo simulations and experiments on public datasets demonstrate that compared to the conventional ML₁-IEKF, our proposed MF-IEKF framework achieves accuracy improvements of at least 5.26% in attitude, 8.72% in velocity, and 15.28% in position estimation, confirming its effectiveness and superiority in complex dynamic scenarios, particularly under large initial misalignment conditions. This research provides a viable solution for high-performance inertial-integrated navigation, and future work will focus on deeper integration of this framework with multi-source sensors (such as vision and LiDAR) and further theoretical analysis of the convergence and stability of the federated architecture.