Equivariant filtering framework for inertial-integrated navigation

Abstract

This paper proposes an Equivariant Filtering (EqF) framework for the inertial-integrated state estimation. As the kinematic system of the inertial-integrated navigation can be naturally modeled on the matrix Lie group SE₂(3), the symmetry of the Lie group can be exploited to design an equivariant filter which extends the invariant extended Kalman filtering on the group-affine system and overcomes the inconsitency issue of the traditional extend Kalman filter. We firstly formulate the inertial-integrated dynamics as the group-affine systems. Then, we prove the left equivariant properties of the inertial-integrated dynamics. Finally, we design an equivariant filtering framework on the earth-centered earth-fixed frame and the local geodetic navigation frame. The experiments show the superiority of the proposed filters when confronting large misalignment angles in Global Navigation Satellite Navigation (GNSS)/Inertial Navigation System (INS) loosely integrated navigation experiments.

Introduction

As dynamic systems on Riemannian manifold are common in the robotics and avionics, the development of robust and accurate state estimation algorithms for autonomous navigation system has gained a great interest in robotics and avionics industries, especially when the states of the vehicles are evolving on Lie group. There are various attempts to design Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF) for a nonlinear system of which state space is in a manifold ${\mathcal {M}}$. In general, Riemannian manifolds lack some mathematical tools such as multiplication (Menegaz et al. 2018). Meanwhile, the most common dynamics of a system can be modeled on the matrix Lie group as it provides better intuitive understanding and simple manipulation by the matrix representation of the Lie group and Lie algebra. Consequently, the Lie group and Lie algebra theory is commonly used in the construction of system dynamics. The Lie group symmetries of the system models are exploited to design observers and filters for attitude (Ng et al. 2019) and pose estimation (Hua et al. 2015), tracking of homographies (Hua et al. 2020), velocity aided attitude estimation (Bonnable et al. 2009), and Visual Simultaneous Localization And Mapping (VSLAM) (Zlotnik 2018; Mahony et al. 2020b). It is shown that any kinematic system on a Lie group can be embedded in a natural manner into an equivariant kinematic system and the equivariant properties of the system embedding can be applied to design an equivariant filter for any kinematic system on a Lie group.

The SE₂(3) based EKF framework proposed recently exploited the explicit algebraic invariance condition termed group-affine property that characterizes the inertial-integrated navigation systems (Luo et al. 2021). Mahony and Trumpf (2020) have shown that the equivariant embedding of group-affine systems only requires a finite dimensional input vector space extension. Furthermore, the research on the equivariant observer design for any kinematic system on a homogeneous space was studied in Mahony et al. (2020a). The equivariant filter differs from the traditional EKF by deriving the coordinate-free error dynamics and linearizing the error state transition matrix around a fixed equilibrium which leads to the state-independent error state transition matrix. Moreover, the state-independent property overcomes the inconsistency issue, caused by linearizing the error dynamics along the system’s trajectory, of the traditional EKF. Therefore, it is promising to embed the inertial-integrated navigation system into an equivariant system so that an equivariant filter can be designed as a kinematic system on a Lie group. Equivariant system theory has been used for the full second order kinematic systems on $\mathrm {T}\mathbf {SO}(3)$ (Ng et al. 2019) and $\mathrm {T}\mathbf {SE}(3)$ (Ng et al. 2020), where the second order means both the first and second derivatives of position are considered in the kinematic system.

Motivated by the equivariant observer design for an equivariant kinematic system, we propose an equivariant filter framework for the inertial-integrated navigation by leveraging the Lie group structure of SE₂(3), which can be regarded as equivariant filter design for the second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$. Firstly, we propose an equivariant filter framework for the inertial-integrated navigation system which embeds the attitude, velocity, and position into the matrix Lie group SE₂(3). Secondly, we consider the left equivariant system and its associated properties as well as the relationships with the group-affine systems when confronting the full second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$. Then, we detail the derivations of the equivariant filter on the navigation frame and the earth frame. Finally, we show the benefit of the Kalman filter based on the left invariant error for the case of large attitude misalignment through a simulation experiment and two field experiments.

The remainder of this paper is organized as follows. Preliminaries are presented in “Preliminaries”. Section “Left equivariant systems” introduces theory for the left equivariant system. Section “Full second order kinematic systems for inertial-integrated navigation” gives the full second order kinematic system in both navigation frame and earth frame. The equivariant filtering for the second order kinematics systems on $\mathrm {T}\mathbf {SE}_2(3)$ is provided in “Full second order kinematic systems for inertial-integrated navigation” section. Then, the equivariant filteri design for inertial-integrated navigation system is shown in Equivariant filtering for second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$ section”. Conclusion and future work are given in “Equivariant filtering design for the inertial-integrated navigation” section.

Preliminaries

The kinematics of a vehicle are described by its velocity, position and attitude, which are expressed on a manifold space and identified in different frames. The velocity and position can be represented by vectors and the attitude in the 3-dimensional space can be represented by a Direction Cosine Matrix (DCM). These three quantities can be jointly reformulated as an element of the SE₂(3) matrix Lie group. The SE₂(3) matrix Lie group is also called the group of direct spatial isometries (Barrau 2017) and represents the space of matrices that apply a rigid body rotation and 2 translations to points in ${\mathbb {R}}^3$. Moreover, the group SE₂(3) has the structure of the semidirect product of SO(3) group by ${\mathbb {R}}^3\times {\mathbb {R}}^3$ and can be expressed as $SE_2(3)=SO(3)\ltimes \underbrace{{\mathbb {R}}^3\times {\mathbb {R}}^3}_2$ (Luo et al. 2020). The relationship between the Lie algebra and the associated vector is described by a linear isomorphism $\Pi$: ${\mathbb {R}}^9 \rightarrow \mathfrak {se}_2(3)$. More details about SE₂(3) matrix Lie group can be found in Barrau (2017) and Luo et al. (2020). The uncertainties on matrix Lie group SE₂(3) can be represented by left multiplication and right multiplication which lead to the left-invariant error and right-invariant error, respectively. The invariant property can be verified by the left group action and right group action.

Meanwhile, expressed in the c frame, the vector $v_{ab}^c$ describes the vector from point a to point b. The direction cosine matrix $C_d^f$ represents the rotation from the d frame to the f frame. The commonly used reference frames in inertial-integrated navigation systems are Earth-Centered-Inertial (ECI) frames (i-frame), Earth-Centered-Earth-Fixed (ECEF) frames (e-frame), north-east-down navigation frames (n-frame), and forward-transversal-down body frames (b-frame) (Shin 2005).

The “plumb-bob gravity” (Savage 2000) is given as

$$\begin{aligned} g=G-(\omega _{ie}\times )^2r \end{aligned}$$

(1)

where g is the gravity vector; G is the gravitational vector; r is the position vector from earth center to the vehicle. This formula can be expressed in ECI frame, ECEF frame, and navigation frame. The gravity perturbation in ECEF frame (Groves 2013) can be written as

$$\begin{aligned} \delta g_{ib}^e\triangleq {\tilde{g}}_{ib}^e-g_{ib}^e\approx -\frac{\mu }{||r_{in}^e||^3}\delta r_{ib}^e \end{aligned}$$

(2)

where $\mu$ is defined in Groves (2013).

The gravity perturbation in navigation frame can be written as  Shin (2005)

$$\begin{aligned} \delta g_{in}^n\triangleq {\tilde{g}}_{in}^n-g_{in}^n \approx \begin{bmatrix} 0&0&\frac{2g_{l}^n}{\sqrt{R_MR_N}+h}\delta r_D \end{bmatrix}^T \end{aligned}$$

(3)

where $\sqrt{R_MR_N}$ is the Gaussian mean Earth radius of curvature; $g_l^n$ is the calculation formula of WGS84 ellipsoid local normal gravity acceleration (Shin 2005); $\delta r_D$ is perturbation of the error position vector $\delta r_{en}^n$ in the down direction of NED frame.

Considering the biases, scale factors, and non-orthogonalities of the accelerometers and gyroscopes, the uncertainty of the sensors (Shin 2005) can be expressed as

$$\begin{aligned} \begin{aligned} \delta f_{ib}^b&=b_a+diag(f_{ib}^b)s_a+\Gamma _a\gamma _a\\ \delta \omega _{ib}^b&=b_g+diag(\omega _{ib}^b)s_g+\Gamma _g\gamma _g \end{aligned} \end{aligned}$$

(4)

where $b_a$ and $b_g$ are the residual biases of the accelerometers and gyroscopes, respectively; $s_a$ and $s_g$ are the scale factors of the accelerometers and gyroscopes, respectively; $\gamma _a$ and $\gamma _g$ are the non-orthogonalities of the accelerometer triad and gyroscope triad, respectively; diag(a) represents the diagonal matrix form of a 3-dimensional vector a; and $\Gamma _a$ and $\Gamma _g$ can be found in (Shin 2005). The random constant, the random walk, and the first-order Gauss-Markov models are typically used in modeling the inertial sensor errors (Shin 2005).

Finally, we summarize the commonly used frames in the inertial navigation and provide the detailed navigation equations in both the NED frame and the ECEF frame. The position error state in radian is usually very small, which can cause numerical instability in Kalman filtering calculation. Therefore, it is better to represent the position error vector in the XYZ coordinate system. The position vector differential equation in the NED frame can be calculated as

$$\begin{aligned} {\dot{r}}_{en}^n=-\omega _{en}^n\times r_{en}^n+v_{en}^n \end{aligned}$$

(5)

The differential equation of the velocity vector in the NED frame is given by

$$\begin{aligned} {\dot{v}}_{en}^n=C_b^nf_{ib}^b-[(2\omega _{ie}^n+\omega _{en}^n)\times ]v_{en}^n+g_{in}^n \end{aligned}$$

(6)

where $\omega _{ie}^n$ is the earth rotation vector expressed in the navigation frame; $f_{ib}^b$ is the specific force vector in navigation frame; $\omega _{en}^n=\omega _{in}^n-\omega _{ie}^n$ is the angular rate vector of the navigation frame relative to the ECEF frame expressed in the navigation frame which is also called the transport rate.

The attitude in the NED frame can be represented by the direction cosine matrix $C_b^n$ and its differential equation is given by

$$\begin{aligned} {\dot{C}}_{b}^n=C_b^n(\omega _{ib}^b\times )-(\omega _{in}^n\times )C_b^n \end{aligned}$$

(7)

When the attitude, velocity, and position in NED frame are represented as $C_b^n$, $v_{in}^n$, and $r_{in}^n$, their differential equations are also considered. They are given as

$$\begin{aligned} \begin{aligned} {\dot{C}}_{b}^n&=C_b^n(\omega _{ib}^b\times )-(\omega _{in}^n\times )C_b^n\\ {\dot{v}}_{in}^n&=-\omega _{in}^n\times v_{in}^n+C_b^nf_{ib}^b+G_{in}^n\\ {\dot{r}}_{in}^n&=-\omega _{in}^n\times r_{in}^n+v_{in}^n \end{aligned} \end{aligned}$$

(8)

When the attitude, velocity, and position in ECEF frame are represented as $C_b^e$, $v_{eb}^e$, and $r_{eb}^e$, their differential equations are given as

$$\begin{aligned} \begin{aligned} {\dot{C}}_{b}^e&=C_b^e(\omega _{ib}^b\times )-(\omega _{ie}^e\times )C_b^e\\ {\dot{v}}_{eb}^e&=-2\omega _{ie}^e\times v_{eb}^e+C_b^ef_{ib}^b+g_{ib}^e\\ {\dot{r}}_{eb}^e&=v_{eb}^e \end{aligned} \end{aligned}$$

(9)

When the attitude, velocity, and position in ECEF frame are represented as $C_b^e$, $v_{ib}^e$, and $r_{ib}^e$, their differential equations are given as

$$\begin{aligned} \begin{aligned} {\dot{C}}_{b}^e&=C_b^e(\omega _{ib}^b\times )-(\omega _{ie}^e\times )C_b^e\\ {\dot{v}}_{ib}^e&=-\omega _{ie}^e\times v_{ib}^e+C_b^ef_{ib}^b+G_{ib}^e\\ {\dot{r}}_{ib}^e&=-\omega _{ie}^e\times r_{ib}^e+v_{ib}^e \end{aligned} \end{aligned}$$

(10)

Lie group and Lie algebra

Let G denote the Lie group, ${\mathfrak {g}}$ the associated Lie algebra, ${\mathcal {M}}$ the smooth manifold, and ${\mathfrak {X}}({\mathcal {M}})$ the linear infinite dimensional vector space of all the vector fields over the smooth manifold ${\mathcal {M}}$. A group action $\psi$ of Lie group G on a smooth manifold ${\mathcal {M}}$ is a smooth mapping which can be either left or right. The left Lie group action is chosen here as it will be used in the subsequent left equivariant systems. Although the right group action can be obtained by some simple manipulations and is wildly used in the filtering based visual inertial navigation system, it has no consistency with the left equivariant systems for the absolute positioning measurement case. A left group action verifies:

$$\begin{aligned} \psi :G\times {\mathcal {M}}\rightarrow {\mathcal {M}} \end{aligned}$$

(11)

with $\psi (A,\psi (B,x))=\psi (AB,x), \forall A,B\in G$ and $\psi (I,v)=v, \forall v\in V$.

For the left group action, the inverse $\psi _{A^{-1}}, \forall A\in G$ is smooth, which induces families of diffeomorphisms $\psi _A:{\mathcal {M}}\rightarrow {\mathcal {M}}$ for $A\in G$ by $\psi _{A}(x):=\psi (A,x)$, and nonlinear projection $\psi _{x}:G\rightarrow {\mathcal {M}}$ for $x\in {\mathcal {M}}$ by $\psi _{x}(A):=\psi (A,x)$. $\left\{ \psi _{A}|A\in G \right\}$ is called Lie group of transformations.

When ${\mathcal {M}}=G$, $\forall g\in G$, the smooth mapping $L_{g}:h\rightarrow gh,\forall h\in G$ is called the left translation generated by Lie group element g. It is worth noting that $L_g:G\rightarrow G$ is a diffeomorphism and $L_{gh}=L_{g}L_{h}$. Given $\forall g\in G$ and $\Lambda \in {\mathfrak {g}}$, the left group action $L_{g}$ induces $dL_{g}\Lambda =g\Lambda$. The left group action $\psi$ is linear if it induces linear smooth mapping $\psi _{A}$.

Lemma 1

Define a smooth mapping $d_{\star }L:G\times {\mathfrak {X}}(G)\rightarrow {\mathfrak {X}}(G)$, given $\forall Z\in G$ and $F\in {\mathfrak {X}}(G)$,

$$\begin{aligned} d_{\star }L(Z,F):=dL_{Z}\cdot F\circ L_{Z^{-1}}\in {\mathfrak {X}}(G) \end{aligned}$$

(12)

Then $d_{\star }L$ is a linear left group action on the vector space ${\mathfrak {X}}(G)$.

Proof

Note that for $A\in G$, $L_{A}$ is a diffeomorphism with smooth inverse $L_{A^{-1}}$. $\forall A,B\in G, F\in {\mathfrak {X}}(G)$, we have

$$\begin{aligned} & d_{{ \star }} L(A,d_{{ \star }} L(B,F)) = dL_{A} \cdot (dL_{B} \cdot F^\circ L_{{B^{{ - 1}} }} )^\circ L_{{A^{{ - 1}} }} \\ & \; = dL_{{AB}} \cdot F^\circ L_{{(AB)^{{ - 1}} }} = d_{{ \star }} L(AB,F) \\ \end{aligned}$$

(13)

Since $L_{g}$ is a left translation on G. The identity property of the group action is straightforward and this demonstrates $d_{\star }L$ is a group action. Linearity follows since

$$\begin{aligned} & d_{\star }L(A,\alpha _1F_1+\alpha _2F_2)\\&\quad=dL_{A}\cdot (A,\alpha _1F_1+\alpha _2F_2)\circ L_{A^{-1}} \\&\quad=\alpha _1dL_A\cdot F_1\circ L_{A^{-1}}+\alpha _2ddL_A\cdot F_2\circ L_{A^{-1}}\\&\quad=\alpha _1d_{\star }L(A,F_1)+\alpha _2d_{\star }L(A,F_2) \end{aligned}$$

(14)

for all $A\in G$, $F_1,F_2\in {\mathfrak {X}}(G)$ and $\alpha _1,\alpha _2\in {\mathbb {R}}$. $\square$

An algebraic object is introduced which is closely related to the kinematic system function ${\mathcal {F}}$ and is important in understanding invariant system structures.

Definition 1

Let ${\mathcal {F}}:V\rightarrow {\mathfrak {X}}(G)$ be a kinematic system on a Lie group G over a input vector space V. The lift is the function $\Lambda : G\times V\rightarrow {\mathfrak {g}}$ defined by

$$\begin{aligned} \Lambda (X,v):={\mathcal {F}}_{v}(X)X^{-1}=dR_{X^{-1}}{\mathcal {F}}_{v}(X),\forall X\in G,v\in V \end{aligned}$$

(15)

The connection between the input vector space V and the Lie algebra ${\mathfrak {g}}$ can be obtained by the algebraic structure provided by the lift.

Left equivariant systems

A left equivariant system is the one in which the left translation of the system function is the same as evaluating the function at the translated base point along with a possible group action transformation of the input space V (Mahony et al. 2020a).

Definition 2

(Left Equivariant System) A kinematic system ${\mathcal {F}}: V\rightarrow {\mathfrak {X}}(G)$ is said to be left equivariant if there exists a left group action $\psi : G\times V\rightarrow V$ such that

$$\begin{aligned} dL_A{\mathcal {F}}_v(X)={\mathcal {F}}_{\psi _{A}(v)}(L_A(X)),\forall A,X\in G, v\in V \end{aligned}$$

(16)

By Definition 2 we assume that the kinematic system ${\mathcal {F}}:V\rightarrow {\mathfrak {X}}(G)$ is left equivariant, which means that

$$\begin{aligned} \begin{aligned} {\mathcal {F}}_{\psi _{A}(v)}(X)&={\mathcal {F}}_{\psi _{A}(v)}(L_A(A^{-1}X))\\&=dL_A{\mathcal {F}}_v(A^{-1}X) =dL_A\cdot {\mathcal {F}}_v \circ L_{A^{-1}}(X) =d_{\star }L_{A}{\mathcal {F}}_{v}(X) \end{aligned} \end{aligned}$$

(17)

where the second line follows from the definition in Eq. (16). It is worth noting that the left input group action is uniquely determined by the left group action $d_{\star }L$ on the vector field. Look at the equivariant kinematic system in Eq. (17) from another point of view, the vector subspace $im{\mathcal {F}}\subset {\mathfrak {X}}(G)$ is invariant under the left group action $d_{\star }L$.

The equivariance can be expressed by an algebraic property of the lift $\Lambda$ defined in Definition 1, which is formulated in the following lemma:

Lemma 2

Let ${\mathcal {F}}:V\rightarrow {\mathfrak {X}}(G)$ be a left equivariant kinematic system on a Lie group G over a input vector space V with lift $\Lambda$. The lift $\Lambda$ satisfies

$$\begin{aligned} Ad_{A^{-1}}\Lambda (AX,\psi _{A}(v))=\Lambda (X,v),\forall A,X\in G,v\in V \end{aligned}$$

(18)

Proof

Given $A,X\in G$ and $v\in V$,

$$\begin{aligned} \begin{aligned} Ad_{A^{-1}}\Lambda (AX,\psi _{A}(v))=&Ad_{A^{-1}}{\mathcal {F}}_{\psi _{A}(v)}(AX)((AX)^{-1})\\ =&dR_{A}dL_{A^{-1}}{\mathcal {F}}_{\psi _{A}(v)}(AX)dL_{X^{-1}}dL_{A^{-1}} =dL_{A^{-1}}{\mathcal {F}}_{\psi _{A}(v)}(L_{A}X)dL_{X^{-1}}\\ =&dL_{A^{-1}} dL_{A}{\mathcal {F}}_{v}dL_{X^{-1}} ={\mathcal {F}}_{v}X^{-1}=\Lambda (X,v) \end{aligned} \end{aligned}$$

(19)

$\square$

Full second order kinematic systems for inertial-integrated navigation

For a state evolving over time, differential equation is commonly used to describe it. Therefore, this section gives the full second order kinematic systems for inertial-integrated navigation in different frames.

Full second order kinematic system in navigation frame

The velocity vector $v_{en}^n$, position vector $r_{en}^n$, and attitude matrix $C_b^n$ can be embedded as the element of the SE₂(3) matrix Lie group, that is

$$\begin{aligned} {\mathcal {X}}=\begin{bmatrix} C_b^n &{} v_{en}^n &{} r_{en}^n\\ 0_{1\times 3} &{} 1 &{} 0\\ 0_{1\times 3} &{} 0&{} 1 \end{bmatrix}\in SE_2(3) \end{aligned}$$

(20)

The inverse of the element can be written as follows

$$\begin{aligned} {\mathcal {X}}^{-1}=\begin{bmatrix} C_n^b &{} -C_n^bv_{en}^n &{} -C_n^br_{en}^n\\ 0_{1\times 3} &{}1 &{}0\\ 0_{1\times 3} &{}0&{}1 \end{bmatrix}=\begin{bmatrix} C_n^b &{} -v_{en}^b &{} -r_{en}^b\\ 0_{1\times 3} &{}1 &{}0\\ 0_{1\times 3} &{}0&{}1 \end{bmatrix}\in SE_2(3) \end{aligned}$$

(21)

Therefore, the differential equation of the ${\mathcal {X}}$ can be calculated as

$$\begin{aligned} \begin{aligned} \frac{d}{dt}{\mathcal {X}} & = f_{u_t}({\mathcal {X}})=\frac{d}{dt}\begin{bmatrix} C_b^n &{} v_{en}^n &{} r_{en}^n\\ 0_{1\times 3} &{} 1 &{} 0\\ 0_{1\times 3} &{} 0&{} 1 \end{bmatrix} =\begin{bmatrix} {\dot{C}}_b^n &{} {\dot{v}}_{en}^n &{} {\dot{r}}_{en}^n\\ 0_{1\times 3} &{} 0 &{} 0\\ 0_{1\times 3} &{} 0&{} 0 \end{bmatrix}\\ =&\begin{bmatrix} C_b^n(\omega _{ib}^b\times )-(\omega _{in}^n\times )C_b^n &{} C_b^nf_{ib}^b-\left[ (2\omega _{ie}^n+\omega _{en}^n)\times \right] v_{en}^n+g_{in}^n &{} -\omega _{en}^n\times r_{en}^n+v_{en}^n\\ 0_{1\times 3} &{} 0 &{} 0\\ 0_{1\times 3} &{} 0&{} 0 \end{bmatrix}\\ \triangleq&\,{\mathcal {X}}W_1+W_2{\mathcal {X}} \end{aligned} \end{aligned}$$

(22)

where $u_t$ is an input sequence; $f_{u_t}$ is a time-varying vector field on matrix Lie group SE₂(3); $W_1$ and $W_2$ are

$$\begin{aligned} W_1=\begin{bmatrix} \omega _{ib}^b\times &{} f_{ib}^b &{} 0_{3\times 1}\\ 0_{1\times 3} &{} 0 &{} 0\\ 0_{1\times 3} &{} 0&{} 0 \end{bmatrix},W_2=\begin{bmatrix} -\omega _{in}^n\times &{} g_{in}^n-\omega _{ie}^n\times v_{en}^n &{} v_{en}^n+\omega _{ie}^n\times r_{en}^n\\ 0_{1\times 3} &{} 0 &{} 0\\ 0_{1\times 3} &{} 0&{} 0 \end{bmatrix} \end{aligned}$$

(23)

It is easy to verify that the dynamical equation $f_{u_t}({\mathcal {X}})$ is group-affine and the group-affine system owns the log-linear property of the corresponding error propagation (Barrau 2017):

$$\begin{aligned} & f_{{u_{t} }} ({\mathcal{X}}_{A} ){\mathcal{X}}_{B} +{\mathcal{X}}_{A} f_{{u_{t} }} ({\mathcal{X}}_{B} ) -{\mathcal{X}}_{A} f_{{u_{t} }} (I_{d} ){\mathcal{X}}_{B} \\ = & ({\mathcal{X}}_{A} W_{1} + W_{2}{\mathcal{X}}_{A} ){\mathcal{X}}_{B} +{\mathcal{X}}_{A} ({\mathcal{X}}_{B} W_{1} + W_{2}{\mathcal{X}}_{B} ) -{\mathcal{X}}_{A} (W_{1} + W_{2} ){\mathcal{X}}_{B} \\ & \quad ={\mathcal{X}}_{A}{\mathcal{X}}_{B} W_{1} + W_{2}{\mathcal{X}}_{A}{\mathcal{X}}_{B} \triangleq f_{{u_{t} }} ({\mathcal{X}}_{A}{\mathcal{X}}_{B} ) \\ \end{aligned}$$

(24)

The group-affine system means that the error kinematics are explicitly independent of the state if the biases of the gyroscope and the accelerometer are ignored (Barrau 2017; Mahony and Trumpf 2020).

Full second order kinematic system with transformed INS mechanization in navigation frame

The velocity vector $v_{in}^n$, position vector $r_{in}^n$, and attitude matrix $C_b^n$ can be treated as the element of the SE₂(3) matrix Lie group. When the state defined on the matrix Lie group is given as

$$\begin{aligned} {\mathcal {X}}=\begin{bmatrix} C_b^n &{} v_{in}^n &{} r_{in}^n\\ 0_{1\times 3} &{} 1 &{} 0\\ 0_{1\times 3} &{} 0 &{}1 \end{bmatrix} \end{aligned}$$

(25)

where $C_b^n$ is the direction cosine matrix from the body frame to the navigation frame; $v_{in}^n$ is the velocity of body relative to the ECI frame expressed in the navigation frame. Meanwhile, $v_{in}^n=v_{en}^n+\omega _{ie}^n\times r_{en}^n$ and $r_{in}^n=r_{en}^n$. The velocity transformation $v_{in}^n=v_{en}^n+\omega _{ie}^n\times r_{en}^n$ means the inertial navigation system mechanization is based on the new transformed velocity. In fact, there is very little difference between the mechanization on the ECEF frame and the ECI frame for positioning on earth’s surface as it just changes the origin of the velocity vector and position vector from the ECEF frame to the ECI frame.

Combined with the “plumb-bob gravity” equation in navigation frame, i.e., $g_{in}^n=G_{in}^n-(\omega _{ie}^n\times )^2r_{en}^n$, the dynamic equation for the state ${\mathcal {X}}$ can be deduced as follows

$$\begin{aligned} \frac{d}{{dt}}{\mathcal{X}} &= f_{{u_{t} }} ({\mathcal{X}}) \\ = &\, \frac{d}{{dt}}\left[ {\begin{array}{*{20}l} {C_{b}^{n} } \hfill & {v_{{in}}^{n} } \hfill & {r_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] = \left[ {\begin{array}{*{20}l} {\dot{C}_{b}^{n} } \hfill & {\dot{v}_{{in}}^{n} } \hfill & {\dot{r}_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}l} {C_{b}^{n} (\omega _{{ib}}^{b} \times ) - (\omega _{{in}}^{n} \times )C_{b}^{n} } \hfill & { - \omega _{{in}}^{n} \times v_{{in}}^{n} + C_{b}^{n} f_{{ib}}^{b} + G_{{in}}^{n} } \hfill & { - \omega _{{in}}^{n} \times r_{{in}}^{n} + v_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}l} {C_{b}^{n} } \hfill & {v_{{in}}^{n} } \hfill & {r_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}l} {\omega _{{ib}}^{b} \times } \hfill & {f_{{ib}}^{b} } \hfill & {0_{{3 \times 1}} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] + \left[ {\begin{array}{*{20}l} { - \omega _{{in}}^{n} \times } \hfill & {G_{{in}}^{n} } \hfill & {v_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}l} {C_{b}^{n} } \hfill & {v_{{in}}^{n} } \hfill & {r_{{in}}^{n} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] \\ &= \, {\mathcal{X}}W_{1} + W_{2} {\mathcal{X}} \\ \end{aligned}$$

(26)

It is easy to verify that the dynamical equation $f_{u_t}({\mathcal {X}})$ is group-affine and the group-affine system owns the log-linear property of the corresponding error propagation (Barrau , Bonnabel 2017; Luo et al. 2021).

Full second order kinematic system in earth frame

When the system state is defined as

$$\begin{aligned} {\mathcal {X}}=\begin{bmatrix} C_b^e &{} v_{eb}^e &{} r_{eb}^e\\ 0_{1\times 3} &{} 1 &{} 0\\ 0_{1\times 3} &{} 0&{} 1 \end{bmatrix}\in SE_2(3) \end{aligned}$$

(27)

where $C_b^e$ is the direction cosine matrix from the body frame to the ECEF frame; $v_{eb}^e$ is the velocity of body frame relative to the ECEF frame expressed in the ECEF frame; $r_{eb}^e$ is the position of body frame relative to the ECEF frame expressed in the ECEF frame.

Then the dynamic equation of the state ${\mathcal {X}}$ can be deduced as follows

$$\begin{aligned} \frac{d}{{dt}}{\mathcal{X}} &= f_{{u_{t} }} ({\mathcal{X}}) \\ = &\, \frac{d}{{dt}}\left[ {\begin{array}{*{20}l} {C_{b}^{e} } \hfill & {v_{{eb}}^{e} } \hfill & {r_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] = \left[ {\begin{array}{*{20}l} {\dot{C}_{b}^{e} } \hfill & {\dot{v}_{{eb}}^{e} } \hfill & {\dot{r}_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}l} {C_{b}^{e} (\omega _{{ib}}^{b} \times ) - (\omega _{{ie}}^{e} \times )C_{b}^{e} } \hfill & {( - 2\omega _{{ie}}^{e} \times )v_{{eb}}^{e} + C_{b}^{e} f_{{ib}}^{b} + g_{{ib}}^{e} } \hfill & {v_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}l} {C_{b}^{e} } \hfill & {v_{{eb}}^{e} } \hfill & {r_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}l} {\omega _{{ib}}^{b} \times } \hfill & {f^{b} } \hfill & {0_{{3 \times 1}} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right] \\ & \quad + \left[ {\begin{array}{*{20}l} { - \omega _{{ie}}^{e} \times } \hfill & {g_{{ib}}^{e} - \omega _{{ie}}^{e} \times v_{{eb}}^{e} } \hfill & {v_{{eb}}^{e} + \omega _{{ie}}^{e} \times r_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 0 \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}l} {C_{b}^{e} } \hfill & {v_{{eb}}^{e} } \hfill & {r_{{eb}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] \triangleq {\mathcal{X}}W_{1} + W_{2} {\mathcal{X}} \\ \end{aligned}$$

(28)

It is easy to verify that the dynamical equation has the group-affine property.

Full second order kinematic system with transformed INS mechanization in earth frame

When the system state is defined as

$$\begin{aligned} {\mathcal {X}}=\begin{bmatrix} C_b^e &{} v_{ib}^e &{} r_{ib}^e\\ 0_{1\times 3} &{} 1 &{} 0\\ 0_{1\times 3} &{} 0&{} 1 \end{bmatrix}\in SE_2(3) \end{aligned}$$

(29)

where $C_b^e$ is the direction cosine matrix from the body frame to the ECEF frame; $v_{ib}^e$ is the velocity of body frame relative to the ECI frame expressed in the ECEF frame; $r_{ib}^e$ is the position of body frame relative to the ECI frame expressed in the ECEF frame. Meanwhile, $v_{ib}^e=v_{eb}^e+\omega _{ie}^e\times r_{eb}^e$ and $r_{ib}^e=r_{eb}^e$.

Firstly, the inverse of the state which is also on the matrix Lie group SE₂(3) is given as

$$\begin{aligned} {\mathcal {X}}^{-1}=\begin{bmatrix} C_e^b &{} -v_{ib}^b &{} -r_{ib}^b\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}\in SE_2(3) \end{aligned}$$

(30)

Then, combined with the “plumb-bob gravity” equation in earth frame: $g_{ib}^e=G_{ib}^e-(\omega _{ie}^e\times )^2r_{eb}^e$, the dynamic equation of the state ${\mathcal {X}}$ can be deduced as follows

$$\begin{aligned} \begin{aligned} \frac{d}{dt}{\mathcal {X}} & = \,f_{u_t}({\mathcal {X}})=\frac{d}{dt}\begin{bmatrix} C_b^e &{} v_{ib}^e &{} r_{ib}^e\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}=\begin{bmatrix} {\dot{C}}_b^e &{} {\dot{v}}_{ib}^e &{} {\dot{r}}_{ib}^e\\ 0_{1\times 3}&{}0&{}0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix}\\ =&\begin{bmatrix} C_b^e(\omega _{ib}^b\times )-(\omega _{ie}^e\times )C_b^e &{} (-\omega _{ie}^e\times )v_{ib}^e+C_b^ef_{ib}^b+G_{ib}^e &{} (-\omega _{ie}^e\times )r_{ib}^e+v_{ib}^e\\ 0_{1\times 3}&{}0&{}0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix}\\ =&\begin{bmatrix} C_b^e &{} v_{ib}^e &{} r_{ib}^e\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}\begin{bmatrix} \omega _{ib}^b\times &{} f_{ib}^b &{} 0_{3\times 1}\\ 0_{1\times 3} &{}0&{}0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix}+\begin{bmatrix} -\omega _{ie}^e\times &{} G_{ib}^e &{} v_{ib}^e\\ 0_{1\times 3} &{}0&{}0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix}\begin{bmatrix} C_b^e &{} v_{ib}^e &{} r_{ib}^e\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}\\ =&\,{\mathcal {X}}W_1+W_2{\mathcal {X}} \end{aligned} \end{aligned}$$

(31)

It is easy to verify that the dynamical Eq. (31) satisfies the group-affine property.

Equivariant filtering for second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$

Taking all the differential equations in different frames into consideration, the second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$ can be formulated as

$$\begin{aligned} {\mathcal {F}}_{v}(X)={\mathcal {F}}_{W_1,W_2}(X)=XW_1+W_2X \end{aligned}$$

(32)

Note that this equation is coordinate-free and enables us to derive different filters by choosing different local coordinates.

Define the function $\Lambda :{\mathcal {M}}\times V\rightarrow {\mathfrak {g}}$ as

$$\begin{aligned} \Lambda (X,v)=\Lambda (X,(W_1,W_2)):=XW_1X^{-1}+W_2 \end{aligned}$$

(33)

It is easy to verify that $\Lambda$ is a lift according to Eq. (15).

Define the group action of the Lie group G on the input vector space V as $\psi :G\times V\rightarrow V$, the explicit expression is given as

$$\begin{aligned} \psi _A(v)=\psi (A,v)=\psi (A,(W_1,W_2)):=(W_1,Ad_{A}W_2) \end{aligned}$$

(34)

Given $A,B\in G$, then we have:

$$\begin{aligned} \begin{aligned} \psi _{B}\left( \psi _{A}(v)\right)&=\psi _{B}\left( \psi _{A}(v)\right) =\psi \left( B, \psi _{A}(v) \right) =\psi \left( B, (W_1,Ad_{A}W_2) \right) \\&=(W_1,Ad_{B}Ad_{A}W_2)=(W_1,Ad_{BA}W_2)=\psi (BA,(W_1+W_2))=\psi _{BA}(v) \end{aligned} \end{aligned}$$

(35)

and

$$\begin{aligned} \psi (I,v)=\psi _{I}(v)=(W_1,Ad_{I}W_2)=(W_1,W_2)=v \end{aligned}$$

(36)

where I is the identity element of Lie group G. The above two equations show the left group action properties of $\psi$.

To verify that the lift $\Lambda$ is equivariant by Eq. (18), compute

$$\begin{aligned} \begin{aligned} Ad_{A^{-1}}\Lambda (AX,\psi _{A}(v)) &= Ad_{A^{-1}}\Lambda (AX,(W_1,Ad_{A}W_2))\\ &= Ad_{A^{-1}}\left( AXW_1(AX)^{-1}+Ad_{A}W_2 \right) =XW_1X^{-1}+W_2=\Lambda (X,v) \end{aligned} \end{aligned}$$

(37)

In general, the group G and ${\mathcal {M}}$ are the matrix Lie group SE₂(3) in the inertial-integrated navigation system. More specifically, the state can be translated by the left group action among four states. For the full states mentioned in Eqs. (20), (25), (27), and (29), A can be defined as

$$\begin{aligned} A_1&=\begin{bmatrix} C_b^e &{} 0_{3\times 1}&{} 0_{3\times 1}\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix},A_2=\begin{bmatrix} I_{3\times 3} &{} \omega _{ie}^n\times r_{en}^n &{} 0_{3\times 1}\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix},\\ A_3&=\begin{bmatrix} I_{3\times 3} &{} \omega _{ie}^e\times r_{eb}^e &{} 0_{3\times 1}\\ 0_{1\times 3}&{}1&{}0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix} \end{aligned}$$

(38)

where $A_1$ is the transformation from Eq. (20)–(27); $A_2$ is the transformation from Eq. (20)–(25); $A_3$ is the transformation from Eq. (27)–(29).

Equivariant Filtering Design for the Inertial-Integrated Navigation

As the second order kinematics are shown to be equivariant, the equivariant filtering can be designed for the inertial-integrated navigation systems and four types of kinematics can be used to design the equivariant filtering algorithms accordingly. To save space, only one of them is given here, and the rest can be derived similarly. Moreover, the error on the Lie group can be selected from ${\tilde{X}}^{-1}X$, $X^{-1}{\tilde{X}}$, ${\tilde{X}}X^{-1}$, and $X{\tilde{X}}^{-1}$, where the first two are left invariant and the last two are right invariant. In the end, we only choose the right invariant error ${\tilde{X}}X^{-1}$ with the second order kinematics with transformed INS mechanization in the earth frame. More algorithms can be obtained by combining a kinematic system with an invariant error (Luo et al. 2021).

The right invariant error ${\eta ^R}$ is defined as

$$\begin{aligned} \eta ^{R} \triangleq \widetilde{{\mathcal{X}}}{\mathcal{X}}^{{ - 1}} = & \left[ {\begin{array}{*{20}c} {\tilde{C}_{b}^{e} } & {\tilde{v}_{{ib}}^{e} } & {\tilde{r}_{{ib}}^{e} } \\ {0_{{1 \times 3}} } & 1 & 0 \\ {0_{{1 \times 3}} } & 0 & 1 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {C_{e}^{b} } & { - v_{{ib}}^{b} } & { - r_{{ib}}^{b} } \\ {0_{{1 \times 3}} } & 1 & 0 \\ {0_{{1 \times 3}} } & 0 & 1 \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}c} {\tilde{C}_{b}^{e} C_{e}^{b} } & {\tilde{v}_{{ib}}^{e} - \tilde{C}_{b}^{e} v_{{ib}}^{b} } & {\tilde{r}_{{ib}}^{e} - \tilde{C}_{b}^{e} r_{{ib}}^{b} } \\ {0_{{1 \times 3}} } & 1 & 0 \\ {0_{{1 \times 3}} } & 0 & 1 \\ \end{array} } \right] \\ \end{aligned}$$

(39)

Similarity, the new error state defined on the matrix Lie group SE₂(3) can be expressed as

$$\begin{aligned} \begin{aligned} {\exp _G(\phi ^e\times ) }&{\triangleq }{ {\tilde{C}}_b^eC_e^b= C_{e}^{e'}\approx I_{3\times 3}-\phi ^e\times }\\ J\rho _{v}^e&{\triangleq }{\tilde{v}}_{ib}^e -{\tilde{C}}_b^ev_{ib}^b={\tilde{v}}_{ib}^e- v_{ib}^e+v_{ib}^e-{\tilde{C}}_b^eC_e^bv_{ib}^e\\&=\delta v_{ib}^e+(I_{3\times 3}-\exp _G(\phi ^e\times )) v_{ib}^e =\delta v_{ib}^e+\phi ^e\times v_{ib}^e\\ J\rho _{r}^e&{\triangleq }{\tilde{r}}_{ib}^e-{\tilde{C}}_b^er_{ib}^b={\tilde{r}}_{ib}^e- r_{ib}^e+r_{ib}^e-{\tilde{C}}_b^eC_e^br_{ib}^e\\&=\delta r_{ib}^e+(I_{3\times 3}-\exp _G(\phi ^e\times )) r_{ib}^e =\delta r_{ib}^e+\phi ^e\times r_{ib}^e \end{aligned} \end{aligned}$$

(40)

where $e'$ is the estimated earth frame.

Meanwhile, the right invariant error satisfies that

$$\begin{aligned} \begin{aligned} \eta ^R&=\begin{bmatrix} \exp _G(\phi ^e\times ) &{} J\rho _{v}^e &{} J\rho _{r}^e\\ 0_{1\times 3} &{} 1&{} 0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}=\exp _G\left( \begin{bmatrix} (\phi ^e\times ) &{} \rho _{v}^e &{} \rho _{r}^e\\ 0_{1\times 3} &{} 0&{} 0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix}\right) \\&=\exp _G\left( \Pi \begin{bmatrix} \phi ^e \\ \rho _{v}^e \\ \rho _{r}^e \end{bmatrix} \right) =\exp _G(\Pi (\rho ^e)) \end{aligned} \end{aligned}$$

(41)

where $\phi ^e$ is the attitude error state; $J\rho _v^e$ is the new definition of velocity error state; $J\rho _r^e$ is the new definition of position error state; $\Pi$ is a linear isomorphism between the Lie algebra and the associated vector; $\phi ^e\times$ denotes the skew-symmetric matrix generated from a 3D vector $\phi ^e\in {\mathbb {R}}^3$; $\exp _G$ denotes the matrix exponential mapping; J is the left Jacobian matrix of the 3D orthogonal rotation matrices group SO(3) and is given as

$$\begin{aligned} J=J_l(\phi )=\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\phi _{\wedge })^n=I_{3\times 3}+\frac{1-\cos \theta }{\theta ^2}\phi _{\wedge }+\frac{\theta -\sin \theta }{\theta ^3}\phi _{\wedge }^2,\theta =||\phi || \end{aligned}$$

(42)

$\rho ^e=\begin{bmatrix} {\phi ^e}^T&{\rho _{v}^e}^T&{\rho _{r}^e}^T \end{bmatrix}^T$ is a 9-dimensional state error vector defined on the Euclidean space corresponding to the error state $\eta ^R$ which is defined on the matrix Lie group. J can be approximated as $J\approx I_{3\times 3}$ if $||\phi ||$ is small enough.

$\exp _G(\phi \times )$ is the Rodriguez formula of the rotation vector and can be calculated by

$$\begin{aligned} C_b^e=\,\exp _G(\phi \times )=\cos ||\phi || I_{3\times 3}+\frac{1-\cos ||\phi ||}{||\phi ||^2}\phi {\phi }^T+\frac{\sin ||\phi ||}{||\phi ||}(\phi \times ) \end{aligned}$$

(43)

Remark 1

Note that the direction cosine matrix $C_b^e$ represents the rotation from b frame to e frame where e frame is fixed. Therefore, the rotation $C_{e'}^e$ can be approximated as $C_{e'}^e\approx I_{3\times 3}+\phi ^e\times$ but $C_{e}^{e'}$ in Eq. (40) can be approximated as $C_{e}^{e'}\approx I_{3\times 3}-\phi ^e\times$ as we assume that the e frame is fixed. When the error state is defined as $\eta ^R={\mathcal {X}}\tilde{{\mathcal {X}}}^{-1}$, the attitude error state will be defined as $C_b^e{\tilde{C}}_e^b\approx C_{e'}^e\approx I_{3\times 3}+\phi ^e\times$.

Remark 2

Although the exponential map is not obligatory in local coordinate mapping, it is the mapping that are easier to compute.

Although the different equation on matrix Lie group SE₂(3) are given, we are interested in the numerical integration in local coordinates. Therefore, we need to transfer the differential equations of error state in matrix Lie group to 3 differential equations in the 9-dimensional local coordinate space. The differential equation of the attitude error state is given as

$$\begin{aligned} &\frac{d}{dt}({\tilde{C}}_b^eC_e^b)=\,\dot{{\tilde{C}}}_b^eC_e^b+{\tilde{C}}_b^e{\dot{C}}_e^b\\&\quad =\,\left[ {\tilde{C}}_b^e({\tilde{\omega }}_{ib}^b\times )-({\tilde{\omega }}_{ie}^e\times ){\tilde{C}}_b^e\right] C_e^b\\ &\quad\quad+{\tilde{C}}_b^e\left[ C_e^b(\omega _{ie}^e\times )-(\omega _{ib}^b\times )C_e^b\right] \\&\quad =\,{\tilde{C}}_b^e(\tilde{{\omega }}_{ib}^b\times )C_e^b-({\omega }_{ie}^e\times ){\tilde{C}}_b^eC_e^b\\ &\quad\quad+{\tilde{C}}_b^eC_e^b(\omega _{ie}^e\times )-{\tilde{C}}_b^e(\omega _{ib}^b\times )C_e^b\\&\quad \approx\, {\tilde{C}}_b^e(\delta {\omega }_{ib}^b\times )C_e^b-({\omega }_{ie}^e\times )(I_{3\times 3}-\phi ^e\times )\\ &\quad\quad+(I_{3\times 3}-\phi ^e\times )(\omega _{ie}^e\times )\\&\quad =\,{\tilde{C}}_b^e(\delta \omega _{ib}^b\times ){{\tilde{C}}_e^b{\tilde{C}}_b^eC_e^b}\\ &\quad\quad+({\omega }_{ie}^e\times )(\phi ^e\times )-(\phi ^e\times )(\omega _{ie}^e\times )\\&\quad \approx\, (({\tilde{C}}_b^e\delta \omega _{ib}^b)\times )(I_{3\times 3}-\phi ^e\times )-((\phi ^e\times \omega _{ie}^e)\times )\\&\quad \approx \,({\tilde{C}}_b^e\delta \omega _{ib}^b)\times -(\phi ^e\times \omega _{ie}^e)\times\\ &\quad =({\tilde{C}}_b^e(\delta b_g^b+w_g^b))\times -(\phi ^e\times \omega _{ie}^e)\times \end{aligned}$$

(44)

where the second order small quantity $\left( ({\tilde{C}}_b^e\delta \omega _{ib}^b)\times \right) (\phi ^e\times )$ is neglected. Therefore, the Eq. (44) can be simplified as

$$\begin{aligned} {\dot{\phi }}^e=\phi ^e\times \omega _{ie}^e-{\tilde{C}}_b^e\delta b_g^b-{\tilde{C}}_b^ew_g^b={-\omega _{ie}^e\times \phi ^e}-{\tilde{C}}_b^e\delta b_g^b-{\tilde{C}}_b^ew_g^b \end{aligned}$$

(45)

The differential equation of the velocity error state is given as

$$\begin{aligned} \begin{aligned} \frac{d}{dt}(J\rho _{v}^e)=&\,\frac{d}{dt}({\tilde{v}}_{ib}^e-{\tilde{C}}_b^eC_e^bv_{ib}^e)=\dot{{\tilde{v}}}_{ib}^e -{\tilde{C}}_b^eC_e^b{\dot{v}}_{ib}^e-\frac{d}{dt}({\tilde{C}}_b^eC_e^b)v_{ib}^e\\ =&\,\left[ (-{\tilde{\omega }}_{ie}^e\times ){\tilde{v}}_{ib}^e+{\tilde{C}}_b^e{{\tilde{f}}_{ib}^b}+{\tilde{G}}_{ib}^e\right] -{\tilde{C}}_b^eC_e^b\left[ (-\omega _{ie}^e\times )v_{ib}^e+C_b^e{f_{ib}^b}+G_{ib}^e\right] \\&-\left( {\tilde{C}}_b^e(\delta \omega _{ib}^b\times ){\tilde{C}}_e^b{\tilde{C}}_b^eC_e^b-({\omega }_{ie}^e\times ){\tilde{C}}_b^eC_e^b+{\tilde{C}}_b^eC_e^b(\omega _{ie}^e\times )\right) v_{ib}^e\\ =&\,{{\tilde{C}}_b^e\delta {f_{ib}^b}}-({\omega }_{ie}^e\times )({\tilde{v}}_{ib}^e-{\tilde{C}}_b^eC_e^bv_{ib}^e) -\left( ({\tilde{C}}_b^e\delta \omega _{ib}^b)\times \right) {{\tilde{C}}_b^eC_e^b v_{ib}^e}+{\tilde{G}}_{ib}^e-{\tilde{C}}_b^eC_e^bG_{ib}^e\\ \approx&\,{\tilde{C}}_b^e\delta {f_{ib}^b}-({\omega }_{ie}^e\times )J\rho _v^e -({\tilde{C}}_b^e\delta \omega _{ib}^b)\times {({\tilde{v}}_{ib}^e-J\rho _v^e)}+{\tilde{G}}_{ib}^e-(I_{3\times 3}-\phi ^e\times )G_{ib}^e\\ \approx&\,-G_{ib}^e\times \phi ^e{-({\omega }_{ie}^e\times )J\rho _v^e}+ {\tilde{v}}_{ib}^e\times ({\tilde{C}}_b^e\delta \omega _{ib}^b)+{\tilde{C}}_b^e\delta {f_{ib}^b}+{\tilde{G}}_{ib}^e-G_{ib}^e\\ =&\,-G_{ib}^e\times \phi ^e{-({\omega }_{ie}^e\times )J\rho _v^e}+ {\tilde{v}}_{ib}^e\times ({\tilde{C}}_b^e(\delta b_g^b+w_g^b))+{\tilde{C}}_b^e(\delta b_a^b+w_a^b)+{\tilde{G}}_{ib}^e-G_{ib}^e \end{aligned} \end{aligned}$$

(46)

where the second order small quantity $(J\rho _v^e\times )({\tilde{C}}_b^e\delta \omega _{ib}^b)$ is neglected; and as $G_{ib}^e$ can be approximated as constant, ${\tilde{G}}_{ib}^e-G_{ib}^e$ can also be neglected.

In the same way, the differential equation of the position error state is given as

$$\begin{aligned} \begin{aligned} \frac{d}{dt}(J\rho _{r}^e)=&\,\frac{d}{dt}({\tilde{r}}_{ib}^e-{\tilde{C}}_b^eC_e^br_{ib}^e)=\dot{{\tilde{r}}}_{ib}^e -{\tilde{C}}_b^eC_e^b{\dot{r}}_{ib}^e - \frac{d}{dt}({\tilde{C}}_b^eC_e^b)r_{ib}^e\\ =&\,\left[ (-{\tilde{\omega }}_{ie}^e\times ){\tilde{r}}_{ib}^e+{\tilde{v}}_{ib}^e\right] -{\tilde{C}}_b^eC_e^b\left[ (-\omega _{ie}^e\times )r_{ib}^e+v_{ib}^e\right] \\&-\,\left( {\tilde{C}}_b^e(\delta \omega _{ib}^b\times ){\tilde{C}}_e^b{\tilde{C}}_b^eC_e^b-({\omega }_{ie}^e\times ){\tilde{C}}_b^eC_e^b+{\tilde{C}}_b^eC_e^b(\omega _{ie}^e\times )\right) r_{ib}^e\\ \approx&(-{\tilde{\omega }}_{ie}^e\times )({\tilde{r}}_{ib}^e-{\tilde{C}}_b^eC_e^br_{ib}^e)+({\tilde{v}}_{ib}^e-{\tilde{C}}_b^eC_e^bv_{ib}^e)-(({\tilde{C}}_b^e\delta \omega _{ib}^b)\times ){{\tilde{C}}_b^eC_e^br_{ib}^e}\\ \approx&{(-{\tilde{\omega }}_{ie}^e\times )J\rho _r^e}+J\rho _v^e+(({\tilde{C}}_b^e\delta \omega _{ib}^b)\times ) {({\tilde{r}}_{ib}^e-J\rho _r^e)}\\ \approx&{(-{\tilde{\omega }}_{ie}^e\times )J\rho _r^e}+J\rho _v^e+{\tilde{r}}_{ib}^e\times ({\tilde{C}}_b^e\delta \omega _{ib}^b)\\ =\,&{(-{\omega }_{ie}^e\times )J\rho _r^e}+J\rho _v^e+{\tilde{r}}_{ib}^e\times ({\tilde{C}}_b^e(\delta b_g^b+w_g^b)) \end{aligned} \end{aligned}$$

(47)

where the second order small quantity $(J\rho _r^e\times )(C_b^e\delta \omega _{ib}^b)$ is neglected.

Thus, the error state $\delta x$, the error state transition matrix $F_r$, and the noise driven matrix $G_r$ of the inertial-integrated error state dynamic equation for equivariant filtering with estimated earth frame attitude are represented as

$$\begin{aligned} \begin{aligned} \delta x=&\begin{bmatrix} \phi ^e\\ J\rho _{v}^e \\ J\rho _{r}^e \\ \delta b_g^b \\ \delta b_a^b \end{bmatrix},G_r=\begin{bmatrix} -{\tilde{C}}_b^e&{}0_{3\times 3}\\ {\tilde{v}}_{ib}^e\times {\tilde{C}}_b^e &{}{\tilde{C}}_b^e\\ {\tilde{r}}_{ib}^e\times {\tilde{C}}_b^e&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3} &{}0_{3\times 3} \end{bmatrix}\\ F_r=&\begin{bmatrix} -\omega _{ie}^e\times &{} 0_{3\times 3} &{} 0_{3\times 3}&{} -{\tilde{C}}_b^e &{}0_{3\times 3}\\ -G_{ib}^e\times &{}-\omega _{ie}^e\times &{}0_{3\times 3} &{} {\tilde{v}}_{ib}^e\times {\tilde{C}}_b^e&{} {\tilde{C}}_b^e\\ 0_{3\times 3}&{}I_{3\times 3}&{}-\omega _{ie}^e\times &{}{\tilde{r}}_{ib}^e\times {\tilde{C}}_b^e&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3} \end{bmatrix} \end{aligned} \end{aligned}$$

(48)

Remark 3

As the rotation of the earth is taken into consideration, the error state transition function is slightly different from the error state transition function derived in Hartley et al. (2020). This is reasonable as the earth rotation rate is usually overwhelmed by the noise of the low cost inertial sensor in applications such as visual inertial navigation system. And it is common to choose the ECEF frame as the global frame in the visual-inertial odometry navigation and lidar-inertial odometry navigation. Therefore, the above equivariant filter algorithm can be applied to autonomous navigation based on either high-grade inertial sensors or low-cost Inertial Measurement Unit (IMU).

The position measurement equation of the right invariant error is formulated as follows:

$$\begin{aligned} \begin{aligned} \delta z_r^r &= ({\hat{r}}_{IMU}^e+{\hat{C}}_b^el^b)-{\tilde{r}}_{GNSS}^e\\ &= r_{IMU}^e+\delta r_{IMU}^e+(I_{3\times 3}-\phi ^e\times )C_b^el^b-r_{GNSS}^e-n_{r,GNSS}\\ &= \delta r_{IMU}^e+(C_b^el^b)\times \phi ^e-n_{r,GNSS}\\ &=\delta r_{ib}^e+(C_b^el^b)\times \phi ^e-n_{r,GNSS}\\ &= J\rho _r^e-\phi ^e\times r_{ib}^e+(C_b^el^b)\times \phi ^e-n_{r,GNSS}\\ &= (r_{ib}^e+C_b^el^b)\times \phi ^e+J\rho _r^e-n_{r,GNSS} \end{aligned} \end{aligned}$$

(49)

where $l^b$ is the lever arm; ${\tilde{r}}_{GNSS}^e$ is the measurement provided by GNSS; $n_{r,GNSS}$ is the noise of measurement and is modeled as zero mean Gaussian white noise.

Therefore, the right measurement Jacobian of position measurement is given as

$$\begin{aligned} H_r^l=\begin{bmatrix} ({\tilde{r}}_{ib}^e+C_b^el^b) \times&0_{3\times 3}&I_{3\times 3}&0_{3\times 3}&0_{3\times 3} \end{bmatrix} \end{aligned}$$

(50)

The feedback correction corresponding to the error state definition in Eq. (40) can be termed as the inverse process. The feedback of attitude, velocity, and position can be easily obtained

$$\begin{aligned} \begin{aligned} C_b^e&={\exp _G(\phi ^e\times )^T{\tilde{C}}_b^e}\\ v_{ib}^e&={\tilde{v}}_{ib}^e-J \rho _v^e-{\tilde{v}}_{ib}^e\times \phi ^e\\ r_{ib}^e&={\tilde{r}}_{ib}^e-J \rho _r^e-{\tilde{r}}_{ib}^e\times \phi ^e \end{aligned} \end{aligned}$$

(51)

Now, we consider the left invariant error whose derivation is similar to that of right invariant error. The left invariant error ${\eta ^L}$ is defined as

$$\begin{aligned} \eta ^{L} \triangleq \widetilde{{\mathcal{X}}}^{{ - 1}}{\mathcal{X}} = & \left[ {\begin{array}{*{20}l} {\tilde{C}_{e}^{b} } \hfill & { - \tilde{v}_{{ib}}^{b} } \hfill & { - \tilde{r}_{{ib}}^{b} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right]\left[ {\begin{array}{*{20}l} {C_{b}^{e} } \hfill & {v_{{ib}}^{e} } \hfill & {r_{{ib}}^{e} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] \\ = & \left[ {\begin{array}{*{20}l} {\tilde{C}_{e}^{b} C_{b}^{e} } \hfill & {\tilde{C}_{e}^{b} v_{{ib}}^{e} - \tilde{v}_{{ib}}^{b} } \hfill & {\tilde{C}_{e}^{b} r_{{ib}}^{e} - \tilde{r}_{{ib}}^{b} } \hfill \\ {0_{{1 \times 3}} } \hfill & 1 \hfill & 0 \hfill \\ {0_{{1 \times 3}} } \hfill & 0 \hfill & 1 \hfill \\ \end{array} } \right] \\ \end{aligned}$$

(52)

Then the error defined on the Lie group can be converted to that on the Lie algebra and the Euclidean space. The new error states of attitude, velocity, and position can be defined as

$$\begin{aligned} \begin{aligned} {\exp _G(\phi ^b\times )\triangleq }&{\tilde{C}}_e^b{C}_b^e =C_b^{b'}\approx I_{3\times 3}-\phi ^b\times \\ J\rho _{v}^b{\triangleq }&{\tilde{C}}_e^b{v}_{ib}^e -{\tilde{v}}_{ib}^b={\tilde{C}}_e^b{v}_{ib}^e-{\tilde{C}}_e^b{\tilde{v}}_{ib}^e={\tilde{C}}_e^b({v}_{ib}^e -{\tilde{v}}_{ib}^e)=-{\tilde{C}}_e^b\delta v_{ib}^e \\ J\rho _{r}^b{\triangleq }&{\tilde{C}}_e^b{r}_{ib}^e-{\tilde{r}}_{ib}^b ={\tilde{C}}_e^b{r}_{ib}^e-{\tilde{C}}_e^b{\tilde{r}}_{ib}^e={\tilde{C}}_e^b({r}_{ib}^e -{\tilde{r}}_{ib}^e)=-{\tilde{C}}_e^b\delta r_{ib}^e \end{aligned} \end{aligned}$$

(53)

where $\phi ^b$ is the attitude error state, $J\rho _{v}^b$ is the new definition of velocity error state, $J\rho _{r}^b$ is the new definition of position error state; J is the left Jacobian matrix given in Eq. (42). The errors can be termed as the common frame representation in the body frame (Andrle , Crassidis 2015). The left-invariant error also satisfies that

$$\begin{aligned} \begin{aligned} \eta ^L &= \begin{bmatrix} \exp _G(\phi ^b\times ) &{} J\rho _{v}^b &{} J\rho _{r}^b\\ 0_{1\times 3} &{} 1&{} 0\\ 0_{1\times 3}&{}0&{}1 \end{bmatrix}=\exp _G\left( \begin{bmatrix} (\phi ^b)\times &{} \rho _{v}^b &{} \rho _{r}^b\\ 0_{1\times 3}&{}0&{}0\\ 0_{1\times 3}&{}0&{}0 \end{bmatrix} \right) \\&=\exp _G\left( \Pi \begin{bmatrix} \phi ^b \\ \rho _{v}^b \\ \rho _{r}^b \end{bmatrix} \right) \end{aligned} \end{aligned}$$

(54)

The differential equation of the attitude error state is given as

$$\begin{aligned} \frac{d}{{dt}}(\tilde{C}_{e}^{b} C_{b}^{e} ) = & \dot{\tilde{C}}_{e}^{b} C_{b}^{e} + \tilde{C}_{e}^{b} \dot{C}_{b}^{e} \\ = & \left[ {\tilde{C}_{e}^{b} (\tilde{\omega }_{{ie}}^{e} \times ) - (\tilde{\omega }_{{ib}}^{b} \times )\tilde{C}_{e}^{b} } \right]C_{b}^{e} + \tilde{C}_{e}^{b} \left[ {C_{b}^{e} (\omega _{{ib}}^{b} \times ) - (\omega _{{ie}}^{e} \times )C_{b}^{e} } \right] \\ = & \tilde{C}_{e}^{b} (\omega _{{ie}}^{e} \times )C_{b}^{e} - (\tilde{\omega }_{{ib}}^{b} \times )\tilde{C}_{e}^{b} C_{b}^{e} + \tilde{C}_{e}^{b} C_{b}^{e} (\omega _{{ib}}^{b} \times ) - \tilde{C}_{e}^{b} (\omega _{{ie}}^{e} \times )C_{b}^{e} \\ \approx & - (\tilde{\omega }_{{ib}}^{b} \times )(I_{{3 \times 3}} - \phi ^{b} \times ) + (I_{{3 \times 3}} - \phi ^{b} \times )((\tilde{\omega }_{{ib}}^{b} - \delta \omega _{{ib}}^{b} ) \times ) \\ = & (\tilde{\omega }_{{ib}}^{b} \times )(\phi ^{b} \times ) - (\delta \omega _{{ib}}^{b} ) \times - (\phi ^{b} \times )(\tilde{\omega }_{{ib}}^{b} \times ) + \phi ^{b} \times (\delta \omega _{{ib}}^{b} ) \times \\ \approx & - (\phi ^{b} \times \tilde{\omega }_{{ib}}^{b} ) \times - \delta \omega _{{ib}}^{b} \times = - (\phi ^{b} \times \tilde{\omega }_{{ib}}^{b} ) \times - (\delta b_{g}^{b} + w_{g}^{b} ) \times \\ \end{aligned}$$

(55)

where the angular velocity error of the earth’s rotation can be neglected, i.e., ${\tilde{\omega }}_{ie}^e=\omega _{ie}^e$; and second order small quantity $(\phi ^b\times )(\delta \omega _{ib}^b\times )$ is also neglected. Therefore, the Eq. (55) can be simplified as

$$\begin{aligned} {\dot{\phi }}^b=\,\phi ^b\times {\tilde{\omega }}_{ib}^b+\delta \omega _{ib}^b=-{\tilde{\omega }}_{ib}^b \times \phi ^b+\delta \omega _{ib}^b=-{\tilde{\omega }}_{ib}^b\times \phi ^b+\delta b_g^b+w_g^b \end{aligned}$$

(56)

The differential equation of the velocity error state is given as

$$\begin{aligned} & \frac{d}{{dt}}(J\rho _{v}^{b} ) = - \dot{\tilde{C}}_{e}^{b} \delta v_{{ib}}^{e} + \tilde{C}_{e}^{b} (\dot{v}_{{ib}}^{e} - \dot{\tilde{v}}_{{ib}}^{e} ) \\ & \quad \quad \quad \quad = - \left[ {\tilde{C}_{e}^{b} (\tilde{\omega }_{{ie}}^{e} \times ) - (\tilde{\omega }_{{ib}}^{b} \times )\tilde{C}_{e}^{b} } \right]\delta v_{{ib}}^{e} \\ & \quad \quad \quad \quad \quad + \tilde{C}_{e}^{b} \left( {\left[ {( - \omega _{{ie}}^{e} \times )v_{{ib}}^{e} + C_{b}^{e} f_{{ib}}^{b} + G_{{ib}}^{e} } \right] - \left[ {( - \tilde{\omega }_{{ie}}^{e} \times )\tilde{v}_{{ib}}^{e} + \tilde{C}_{b}^{e} \tilde{f}_{{ib}}^{b} + \tilde{G}_{{ib}}^{e} } \right]} \right) \\ & \quad \quad \quad \quad = - \tilde{C}_{e}^{b} (\omega _{{ie}}^{e} \times )\delta v_{{ib}}^{e} + (\tilde{\omega }_{{ib}}^{b} \times )\tilde{C}_{e}^{b} \delta v_{{ib}}^{e} + \tilde{C}_{e}^{b} C_{b}^{e} f_{{ib}}^{b} - \tilde{f}_{{ib}}^{b} - \tilde{C}_{e}^{b} \omega _{{ie}}^{e} \times (v_{{ib}}^{e} - \tilde{v}_{{ib}}^{e} ) \\ & \quad \quad \quad \quad \quad \quad + \tilde{C}_{e}^{b} (G_{{ib}}^{e} - \tilde{G}_{{ib}}^{e} ) \\ & \quad \quad \quad \quad \approx - (\tilde{\omega }_{{ib}}^{b} \times )J\rho _{v}^{b} + (I_{{3 \times 3}} - \phi ^{b} \times )(\tilde{f}_{{ib}}^{b} - \delta f_{{ib}}^{b} ) - \tilde{f}_{{ib}}^{b} + \tilde{C}_{e}^{b} (G_{{ib}}^{e} - \tilde{G}_{{ib}}^{e} ) \\ & \quad \quad \quad \quad = - (\tilde{\omega }_{{ib}}^{b} \times )J\rho _{v}^{b} - \phi ^{b} \times \tilde{f}_{{ib}}^{b} + \phi ^{b} \times \delta f_{{ib}}^{b} + \tilde{C}_{e}^{b} (G_{{ib}}^{e} - \tilde{G}_{{ib}}^{e} ) - \delta f_{{ib}}^{b} \\ & \quad \quad \quad \quad \approx - (\tilde{\omega }_{{ib}}^{b} \times )J\rho _{v}^{b} + \tilde{f}_{{ib}}^{b} \times \phi ^{b} + \tilde{C}_{e}^{b} (G_{{ib}}^{e} - \tilde{G}_{{ib}}^{e} ) - \delta f_{{ib}}^{b} \\ & \quad \quad \quad \quad = - (\tilde{\omega }_{{ib}}^{b} \times )J\rho _{v}^{b} + \tilde{f}_{{ib}}^{b} \times \phi ^{b} + \tilde{C}_{e}^{b} (G_{{ib}}^{e} - \tilde{G}_{{ib}}^{e} ) - \delta b_{a}^{b} - w_{a}^{b} \\ \end{aligned}$$

(57)

where the second order small quantity $\phi ^b\times \delta {f_{ib}^b}$ is neglected; and as $G_{ib}^e$ can be approximately treated as constant, ${\tilde{C}}_e^b({G}_{ib}^e-{\tilde{G}}_{ib}^e)$ can also be neglected.

In the same way, the differential equation of the position error state is given as

$$\begin{aligned} \begin{aligned} \frac{d}{dt}(J\rho _{r}^b)=&-\dot{{\tilde{C}}}_e^b\delta r_{ib}^e+{\tilde{C}}_e^b(\dot{{r}}_{ib}^e-\dot{{\tilde{r}}}_{ib}^e)\\ =&-\left[ {\tilde{C}}_e^b(\omega _{ie}^e\times )-({\tilde{\omega }}_{ib}^b\times ){\tilde{C}}_e^b\right] \delta r_{ib}^e +{\tilde{C}}_e^b\left( \left[ (-\omega _{ie}^e\times )r_{ib}^e+v_{ib}^e\right] - \left[ (-{\tilde{\omega }}_{ie}^e\times ){\tilde{r}}_{ib}^e+{\tilde{v}}_{ib}^e\right] \right) \\ =&-{\tilde{C}}_e^b(\omega _{ie}^e\times )\delta r_{ib}^e+({\tilde{\omega }}_{ib}^b\times ){{\tilde{C}}_e^b\delta r_{ib}^e}-{\tilde{C}}_e^b\omega _{ie}^e\times ({r}_{ib}^e-{\tilde{r}}_{ib}^e)+{\tilde{C}}_e^b({v}_{ib}^e-{\tilde{v}}_{ib}^e)\\ =&-{\tilde{\omega }}_{ib}^b\times J\rho _{r}^b+J\rho _{v}^b \end{aligned} \end{aligned}$$

(58)

It is worth noting that there is no approximation in the above derivation.

Thus, the inertial-integrated error state dynamic equation for the SE₂(3) based EKF can be obtained

$$\begin{aligned} \delta {\dot{x}}=F_l\delta x+G_lw \end{aligned}$$

(59)

where $F_l$ is the error state transition matrix; $\delta x$ is the error state including the biases; and $G_l$ is the noise driven matrix. Their definitions are given as

$$\begin{aligned} \begin{aligned} \delta x=&\begin{bmatrix} \phi ^b\\ J\rho _{v}^b \\ J\rho _{r}^b \\ \delta b_g^b \\ \delta b_a^b \end{bmatrix}, F_l=\begin{bmatrix} -{\tilde{\omega }}_{ib}^b\times &{} 0_{3\times 3} &{} 0_{3\times 3}&{} I_{3\times 3} &{}0_{3\times 3}\\ {{\tilde{f}}_{ib}^b}\times &{}-{\tilde{\omega }}_{ib}^b\times &{}0_{3\times 3} &{} 0_{3\times 3}&{} -I_{3\times 3}\\ 0_{3\times 3}&{}I_{3\times 3}&{}-{\tilde{\omega }}_{ib}^b\times &{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3} \end{bmatrix},\\ G_l=&\begin{bmatrix} I_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}-I_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}I_{3\times 3}&{}0_{3\times 3}\\ 0_{3\times 3}&{}0_{3\times 3}&{}0_{3\times 3}&{}I_{3\times 3} \end{bmatrix}, w=\begin{bmatrix}w_g^b\\ w_a^b \\ w_{b_g}^b \\ w_{b_a}^b\end{bmatrix} \end{aligned} \end{aligned}$$

(60)

Considering the lever arm, the position measurement equation is written as

$$\begin{aligned} \begin{aligned} \delta z_r^l=\,&({\hat{r}}_{IMU}^e+{\hat{C}}_b^{e}l^b)-{\tilde{r}}_{GNSS}^e\\ =\,&r_{IMU}^e+\delta r_{IMU}^e+C_b^e(I_{3\times 3}+\phi ^b\times )l^b-r_{GNSS}^e-n_{r,GNSS}\\ =\,&\delta r_{IMU}^e-C_b^e(l^b\times ) \phi ^b-n_{r,GNSS}=\delta r_{ib}^e-C_b^e(l^b\times ) \phi ^b-n_{r,GNSS}\\ =\,&-{\tilde{C}}_b^e J\rho _r^b-C_b^e(l^b\times ) \phi ^b-n_{r,GNSS}\\ \approx&-{\tilde{C}}_b^e J\rho _r^b-{\tilde{C}}_b^e(l^b\times ) \phi ^b-n_{r,GNSS} \end{aligned} \end{aligned}$$

(61)

where $l^b$ is the lever arm.

Therefore, the left measurement Jacobian for position measurement is given as

$$\begin{aligned} H_l=\begin{bmatrix} -{\tilde{C}}_b^e(l^b\times )&0_{3\times 3}&-{\tilde{C}}_{b}^e&0_{3\times 3}&0_{3\times 3} \end{bmatrix} \end{aligned}$$

(62)

It is shown that the attitude matrix ${\tilde{C}}_b^e$ can be canceled in the measurement update step for both the error state and its associated covariance (Luo et al. 2021). Therefore, the measurement matrix is $H_l=\begin{bmatrix} -(l^b\times )&0_{3\times 3}&-I_{3\times 3}&0_{3\times 3}&0_{3\times 3} \end{bmatrix}$, which is independent of the system’s trajectory.

Similar to the feedback correction for the case that the error is right invariant, the feedback of attitude, velocity, and position when the error is left invariant can be easily obtained according to Eq. (53):

$$\begin{aligned} \begin{aligned} C_b^e&={{\tilde{C}}_b^e\exp _G(\phi ^b\times )}\\ v_{ib}^e&{={\tilde{C}}_b^e({\tilde{v}}_{ib}^b+J \rho _v^b)={\tilde{v}}_{ib}^e+{\tilde{C}}_b^eJ \rho _v^b}\\ r_{ib}^e&{={\tilde{C}}_b^e({\tilde{r}}_{ib}^b+J \rho _r^b)={\tilde{r}}_{ib}^e+{\tilde{C}}_b^eJ \rho _r^b} \end{aligned} \end{aligned}$$

(63)

Remark 4

If the left invariant error is defined as $\eta ^L={{\mathcal {X}}}^{-1}\tilde{{\mathcal {X}}}$, the attitude error state will be defined as $C_e^b{\tilde{C}}_b^e\approx C_{b'}^b\approx I_{3\times 3}+\phi ^b\times$ and the dynamic equations of error states are similar.

Remark 5

Comparing the transition matrix and measurement matrix of the left invariant error model with that of right invariant error model, they are independent of the system’s trajectory and this property overcomes the consistence issue. The advantage will be demonstrated in the integrated navigation which has fast convergence speed even with large misalignment angles. Moreover, the transition matrix of right invariant error model only depends on the system’s trajectory through the bias and the measurement matrix is also dependent with the position of the system. Therefore, the filter constructed from left invariant error model performs better than the filter constructed from the right invariant error model.

Remark 6

When it comes to the dynamics in the navigation frame, the similar procedures can be performed to obtain the equivariant filtering algorithm with both left invariant error and right invariant error. For detailed derivation readers refer to the literature (Luo et al. 2021).

Experiments

The performances of the left invariant error model and right invariant error model are evaluated in this section using a simulation experiment and two filed experiments with Global Navigation Satellite System (GNSS)/Inertial Navigation System (INS) loosely integrated system. The Kalman filter corresponding to the left invariant error is denoted as LEnEKF, the Kalman filter corresponding to the right invariant error is denoted as REnEKF, and the traditional Kalman filter corresponding to the traditional error in ECEF frame is denoted as EKF. There is no special initial alignment process during the simulation experiment and field experiment. Therefore, the experiments with large misalignment angle are performed to show the superiority of the proposed algorithm. It is also worth noting that the GNSS provides global positioning result which is consistent with the LEnEKF. Therefore, LEnEKF is supposed to perform better than other algorithms.

Simulation experiment

The simulation experiment is under a static condition with the duration is 300 s. The sampling rate for IMU is 200 Hz and for GNSS observation is 10 Hz. The specification of the IMU is given is Table 1. The initial attitude is generated randomly. The horizontal misalignments are assumed to satisfy the zero mean Gaussian distribution ${\mathcal {N}}(0,(5^{\circ })^2)$, and the yaw angle is assumed to satisfy the zero mean Gaussian distribution ${\mathcal {N}}(0,(60^{\circ })^2)$. The initial attitude error variance is set as $\begin{bmatrix} (5^{\circ })^2&(5^{\circ })^2&(60^{\circ })^2 \end{bmatrix}^T$. The initial parameters for the three filters are the same.

Table 1 Specification of the IMU used in the static simulation experiment

The attitude results of 100 Monte Carlo simulations with the three filters are shown in Fig. 1. In the LEnEKF, the roll and pitch converged within 20s and the heading within 130s in all the 200 Monte Carlo experiments. Meanwhile, the roll and pitch converged in most of the 200 Monte Carlo experiments for REnEKF, but the heading failed to converge in most cases. However, in traditional EKF, the roll, pitch, and heading converged in few cases. The mean errors for the three angles with large misalignment angles are given is shown in Fig. 2. As we can see, the roll angle and the pith angle can converge in short time for the LEnEKF and the REnEKF. Furthermore, the LEnEKF performs better than the REnEKF in terms of the yaw angle. We see that the yaw angle can converge to the true value for LEnEKF but the other two filters fail. The REnEKF fails to converge due to the ${\tilde{r}}_{ib}^e\times {\tilde{C}}_b^e$ term in the F matrix. This term is correlated with the bias of the gyroscope, which is difficult to estimate. Consequently, the REnEKF may fail if the bias is not well estimated. Meanwhile, the measurement matrix of REnEKF is correlated with the position but the measurement matrix of LEnEKF can be adjusted to be independent of system states.

Fig. 1

Roll, pitch, and, yaw angle estimated results in static simulation experiment with large misalignment angle

Fig. 2

The mean errors of the three angles in simulation experiment with large misalignment angle

Filed Experiment

The vehicle-mounted loosely coupled integration is carried out for the evaluation of the three filters: LEnEKF, REnEKF, and, EKF. The specification of the IMU is given is Table. 2. The initial attitude error is set as $\begin{bmatrix} 6^{\circ }&6^{\circ }&60^{\circ } \end{bmatrix}^T$, and correspondingly, its initial variance is set as $\begin{bmatrix} (6^{\circ })^2&(6^{\circ })^2&(60^{\circ })^2 \end{bmatrix}^T$. The sampling rate of IMU is 200Hz. The sampling rate of GNSS observations is 1Hz.The duration of the IMU data is 430s. The correction time is 4h for both gyroscope and accelerometer.

Table 2 Specification of the IMU used in the first field experiment

Since most integrated navigation approaches based extended Kalman filtering have similar performance after the filters converge (Shin 2005), we aim at comparing the alignment performance when the attitude is with large heading error. The attitude errors of the three filters are shown in Fig. 3. It is obvious that only the LEnEKF can converge within 5s in all three angles. Meanwhile, the yaw angle of LEnEKF converges faster than that of REnEKF. This is also benefit from its state-independent transition matrix and measurement Jacobian matrix when confronting with large misalignment angle. The fast convergence in the three angles shows the superior performance of the proposed algorithm in the case of large misalignment angles.

Fig. 3

Roll, pitch, and, yaw angle estimated results in field experiment with large misalignment angle

To evaluate the performance of the three filters, 30 second GPS outages were intentionally introduced twice in the field test data and the position errors in horizontal direction of the algorithms are shown in Fig. 4. The position errors of the traditional EKF reached over 70 m in north direction and 150 m in east direction during the first outage. However, the position errors of the traditional EKF during the second outage reached over 250 m in north direction and 300 m in east direction as the navigation parameters have not been well estimated before the GPS signal outage. The LEnEKF and REnEKF approaches have similar performance during the 30 second GPS outages. However, in contract with LEnEKF, the REnEKF didn’t converge in the north direction. Consequently, the LEnEKF performs better than the other two filters during the 30s GPS outages.

Fig. 4

Position errors of two 30s GPS outages

In order to further demonstrate the effectiveness of the proposed algorithm, dynamic data collected from an intermediate-grade fiber optic IMU is used for evaluation. The specification of the IMU is given in Table. 3. The biases of gyroscope and accelerometer are $0.03^{\circ }/\sqrt{{\mathrm{h}}}$ and $0.3 {\mathrm{m}}/{\mathrm{s}}/\sqrt{{\mathrm{h}}}$, respectively. The gyroscope-bias standard deviation is 0.3°/h and the accelerometer-bias standard deviation is 30 mGal. The correction time is 4 h for both gyroscope and accelerometer. The duration of the data is 600 s. The sampling rate of IMU is 200 Hz. The initial attitude error and the associated covariance are set as same as before, and two 60 s outages are introduced.

Table 3 Specification of the IMU used in the second field experiment

Fig. 5

Position errors of two 60 s GPS outages with large misalignment angle

Fig. 6

Velocity errors of two 60 s GPS outages with large misalignment angle

Fig. 7

Attitude errors of two 60 s GPS outages with large misalignment angle

As the REnEKF fails in this experiment with large misalignment angle, only LEnEKF and EKF are compared. The position error, velocity error, and attitude error are shown in Figs. 5, 6, and 7, respectively. The faster convergence of heading for LEnEKF than that of EKF shows the advantage of the proposed LEnEKF which is due to the common frame error representation. As for the outage, the LEnEKF has small velocity error in both two outages and small position error in the second outage. For LEnEKF in the first outage, the east position error reaches 2.5 m and the north position error reaches 1 m, leading to the horizontal position error $\sqrt{2.5^2+1^2}\approx 2.6926m$. However, for the EKF in the first outage, the horizontal position error is $\sqrt{1.5^2+5^2}\approx 5.2202m$. Considering that the down position errors are equal for LEnEKF and EKF, one can infer that the performance of LEnEKF is better than EKF in terms of horizontal position error.

Conclusion

In this paper, the left equivariant property of the inertial-integrated kinematics system is exploited for the design of equivariant filtering framework. It is termed as a specialization of the equivariant filtering for the second order kinematic systems on $\mathrm {T}\mathbf {SE}_2(3)$. The results can be combined with the absolute position measurement for an inertial-integrated navigation system such as the GNSS/INS loosely coupled navigation and combined with the relative pose measurement for inertial-integrated navigation such as visual-inertial navigation odometry. The simulation and filed test experiments of GNSS/INS integrated navigation are performed to show the effectiveness and advantage of the left invariant error based EKF. Future works include observability analysis and more filed experiments with different grades of inertial sensor for practical autonomous navigation applications. Moreover, the right invariant error based EKF is also worth exploring when a relative measurement sensor such as odometer is used.