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 SE2(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 SE2(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 SE2(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).