ByYarong Luo, Chi Guo and Jingnan Liu

Dec 27, 2021 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 ^etimes ) }&{triangleq }{ {tilde{C}}_b^eC_e^b= C_{e}^{e’}approx I_{3times 3}-phi ^etimes }\ Jrho _{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_{3times 3}-exp _G(phi ^etimes )) v_{ib}^e =delta v_{ib}^e+phi ^etimes v_{ib}^e\ Jrho _{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_{3times 3}-exp _G(phi ^etimes )) r_{ib}^e =delta r_{ib}^e+phi ^etimes 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 ^etimes ) &{} Jrho _{v}^e &{} Jrho _{r}^e\ 0_{1times 3} &{} 1&{} 0\ 0_{1times 3}&{}0&{}1 end{bmatrix}=exp _Gleft( begin{bmatrix} (phi ^etimes ) &{} rho _{v}^e &{} rho _{r}^e\ 0_{1times 3} &{} 0&{} 0\ 0_{1times 3}&{}0&{}0 end{bmatrix}right) \&=exp _Gleft( 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; (Jrho _v^e) is the new definition of velocity error state; (Jrho _r^e) is the new definition of position error state; (Pi) is a linear isomorphism between the Lie algebra and the associated vector; (phi ^etimes) denotes the skew-symmetric matrix generated from a 3D vector (phi ^ein {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_{3times 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 (Japprox I_{3times 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_{3times 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’}^eapprox I_{3times 3}+phi ^etimes) but (C_{e}^{e’}) in Eq. (40) can be approximated as (C_{e}^{e’}approx I_{3times 3}-phi ^etimes) 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^bapprox C_{e’}^eapprox I_{3times 3}+phi ^etimes).

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}^btimes )-({tilde{omega }}_{ie}^etimes ){tilde{C}}_b^eright] C_e^b\ &quadquad+{tilde{C}}_b^eleft[ C_e^b(omega _{ie}^etimes )-(omega _{ib}^btimes )C_e^bright] \&quad =,{tilde{C}}_b^e(tilde{{omega }}_{ib}^btimes )C_e^b-({omega }_{ie}^etimes ){tilde{C}}_b^eC_e^b\ &quadquad+{tilde{C}}_b^eC_e^b(omega _{ie}^etimes )-{tilde{C}}_b^e(omega _{ib}^btimes )C_e^b\&quad approx, {tilde{C}}_b^e(delta {omega }_{ib}^btimes )C_e^b-({omega }_{ie}^etimes )(I_{3times 3}-phi ^etimes )\ &quadquad+(I_{3times 3}-phi ^etimes )(omega _{ie}^etimes )\&quad =,{tilde{C}}_b^e(delta omega _{ib}^btimes ){{tilde{C}}_e^b{tilde{C}}_b^eC_e^b}\ &quadquad+({omega }_{ie}^etimes )(phi ^etimes )-(phi ^etimes )(omega _{ie}^etimes )\&quad approx, (({tilde{C}}_b^edelta omega _{ib}^b)times )(I_{3times 3}-phi ^etimes )-((phi ^etimes omega _{ie}^e)times )\&quad approx ,({tilde{C}}_b^edelta omega _{ib}^b)times -(phi ^etimes omega _{ie}^e)times\ &quad =({tilde{C}}_b^e(delta b_g^b+w_g^b))times -(phi ^etimes omega _{ie}^e)times end{aligned}

(44)

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

begin{aligned} {dot{phi }}^e=phi ^etimes omega _{ie}^e-{tilde{C}}_b^edelta b_g^b-{tilde{C}}_b^ew_g^b={-omega _{ie}^etimes phi ^e}-{tilde{C}}_b^edelta 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}(Jrho _{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}^etimes ){tilde{v}}_{ib}^e+{tilde{C}}_b^e{{tilde{f}}_{ib}^b}+{tilde{G}}_{ib}^eright] -{tilde{C}}_b^eC_e^bleft[ (-omega _{ie}^etimes )v_{ib}^e+C_b^e{f_{ib}^b}+G_{ib}^eright] \&-left( {tilde{C}}_b^e(delta omega _{ib}^btimes ){tilde{C}}_e^b{tilde{C}}_b^eC_e^b-({omega }_{ie}^etimes ){tilde{C}}_b^eC_e^b+{tilde{C}}_b^eC_e^b(omega _{ie}^etimes )right) v_{ib}^e\ =&,{{tilde{C}}_b^edelta {f_{ib}^b}}-({omega }_{ie}^etimes )({tilde{v}}_{ib}^e-{tilde{C}}_b^eC_e^bv_{ib}^e) -left( ({tilde{C}}_b^edelta 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^edelta {f_{ib}^b}-({omega }_{ie}^etimes )Jrho _v^e -({tilde{C}}_b^edelta omega _{ib}^b)times {({tilde{v}}_{ib}^e-Jrho _v^e)}+{tilde{G}}_{ib}^e-(I_{3times 3}-phi ^etimes )G_{ib}^e\ approx&,-G_{ib}^etimes phi ^e{-({omega }_{ie}^etimes )Jrho _v^e}+ {tilde{v}}_{ib}^etimes ({tilde{C}}_b^edelta omega _{ib}^b)+{tilde{C}}_b^edelta {f_{ib}^b}+{tilde{G}}_{ib}^e-G_{ib}^e\ =&,-G_{ib}^etimes phi ^e{-({omega }_{ie}^etimes )Jrho _v^e}+ {tilde{v}}_{ib}^etimes ({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 ((Jrho _v^etimes )({tilde{C}}_b^edelta 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}(Jrho _{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}^etimes ){tilde{r}}_{ib}^e+{tilde{v}}_{ib}^eright] -{tilde{C}}_b^eC_e^bleft[ (-omega _{ie}^etimes )r_{ib}^e+v_{ib}^eright] \&-,left( {tilde{C}}_b^e(delta omega _{ib}^btimes ){tilde{C}}_e^b{tilde{C}}_b^eC_e^b-({omega }_{ie}^etimes ){tilde{C}}_b^eC_e^b+{tilde{C}}_b^eC_e^b(omega _{ie}^etimes )right) r_{ib}^e\ approx&(-{tilde{omega }}_{ie}^etimes )({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^edelta omega _{ib}^b)times ){{tilde{C}}_b^eC_e^br_{ib}^e}\ approx&{(-{tilde{omega }}_{ie}^etimes )Jrho _r^e}+Jrho _v^e+(({tilde{C}}_b^edelta omega _{ib}^b)times ) {({tilde{r}}_{ib}^e-Jrho _r^e)}\ approx&{(-{tilde{omega }}_{ie}^etimes )Jrho _r^e}+Jrho _v^e+{tilde{r}}_{ib}^etimes ({tilde{C}}_b^edelta omega _{ib}^b)\ =,&{(-{omega }_{ie}^etimes )Jrho _r^e}+Jrho _v^e+{tilde{r}}_{ib}^etimes ({tilde{C}}_b^e(delta b_g^b+w_g^b)) end{aligned} end{aligned}

(47)

where the second order small quantity ((Jrho _r^etimes )(C_b^edelta 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\ Jrho _{v}^e \ Jrho _{r}^e \ delta b_g^b \ delta b_a^b end{bmatrix},G_r=begin{bmatrix} -{tilde{C}}_b^e&{}0_{3times 3}\ {tilde{v}}_{ib}^etimes {tilde{C}}_b^e &{}{tilde{C}}_b^e\ {tilde{r}}_{ib}^etimes {tilde{C}}_b^e&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}\ 0_{3times 3} &{}0_{3times 3} end{bmatrix}\ F_r=&begin{bmatrix} -omega _{ie}^etimes &{} 0_{3times 3} &{} 0_{3times 3}&{} -{tilde{C}}_b^e &{}0_{3times 3}\ -G_{ib}^etimes &{}-omega _{ie}^etimes &{}0_{3times 3} &{} {tilde{v}}_{ib}^etimes {tilde{C}}_b^e&{} {tilde{C}}_b^e\ 0_{3times 3}&{}I_{3times 3}&{}-omega _{ie}^etimes &{}{tilde{r}}_{ib}^etimes {tilde{C}}_b^e&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 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_{3times 3}-phi ^etimes )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}\ &= Jrho _r^e-phi ^etimes r_{ib}^e+(C_b^el^b)times phi ^e-n_{r,GNSS}\ &= (r_{ib}^e+C_b^el^b)times phi ^e+Jrho _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_{3times 3}&I_{3times 3}&0_{3times 3}&0_{3times 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 ^etimes )^T{tilde{C}}_b^e}\ v_{ib}^e&={tilde{v}}_{ib}^e-J rho _v^e-{tilde{v}}_{ib}^etimes phi ^e\ r_{ib}^e&={tilde{r}}_{ib}^e-J rho _r^e-{tilde{r}}_{ib}^etimes 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 ^btimes )triangleq }&{tilde{C}}_e^b{C}_b^e =C_b^{b’}approx I_{3times 3}-phi ^btimes \ Jrho _{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^bdelta v_{ib}^e \ Jrho _{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^bdelta r_{ib}^e end{aligned} end{aligned}

(53)

where (phi ^b) is the attitude error state, (Jrho _{v}^b) is the new definition of velocity error state, (Jrho _{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 ^btimes ) &{} Jrho _{v}^b &{} Jrho _{r}^b\ 0_{1times 3} &{} 1&{} 0\ 0_{1times 3}&{}0&{}1 end{bmatrix}=exp _Gleft( begin{bmatrix} (phi ^b)times &{} rho _{v}^b &{} rho _{r}^b\ 0_{1times 3}&{}0&{}0\ 0_{1times 3}&{}0&{}0 end{bmatrix} right) \&=exp _Gleft( 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 ^btimes )(delta omega _{ib}^btimes )) is also neglected. Therefore, the Eq. (55) can be simplified as

begin{aligned} {dot{phi }}^b=,phi ^btimes {tilde{omega }}_{ib}^b+delta omega _{ib}^b=-{tilde{omega }}_{ib}^b times phi ^b+delta omega _{ib}^b=-{tilde{omega }}_{ib}^btimes 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}}(Jrho _{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 )Jrho _{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 )Jrho _{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 )Jrho _{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 )Jrho _{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 ^btimes 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}(Jrho _{r}^b)=&-dot{{tilde{C}}}_e^bdelta r_{ib}^e+{tilde{C}}_e^b(dot{{r}}_{ib}^e-dot{{tilde{r}}}_{ib}^e)\ =&-left[ {tilde{C}}_e^b(omega _{ie}^etimes )-({tilde{omega }}_{ib}^btimes ){tilde{C}}_e^bright] delta r_{ib}^e +{tilde{C}}_e^bleft( left[ (-omega _{ie}^etimes )r_{ib}^e+v_{ib}^eright] – left[ (-{tilde{omega }}_{ie}^etimes ){tilde{r}}_{ib}^e+{tilde{v}}_{ib}^eright] right) \ =&-{tilde{C}}_e^b(omega _{ie}^etimes )delta r_{ib}^e+({tilde{omega }}_{ib}^btimes ){{tilde{C}}_e^bdelta r_{ib}^e}-{tilde{C}}_e^bomega _{ie}^etimes ({r}_{ib}^e-{tilde{r}}_{ib}^e)+{tilde{C}}_e^b({v}_{ib}^e-{tilde{v}}_{ib}^e)\ =&-{tilde{omega }}_{ib}^btimes Jrho _{r}^b+Jrho _{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_ldelta 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\ Jrho _{v}^b \ Jrho _{r}^b \ delta b_g^b \ delta b_a^b end{bmatrix}, F_l=begin{bmatrix} -{tilde{omega }}_{ib}^btimes &{} 0_{3times 3} &{} 0_{3times 3}&{} I_{3times 3} &{}0_{3times 3}\ {{tilde{f}}_{ib}^b}times &{}-{tilde{omega }}_{ib}^btimes &{}0_{3times 3} &{} 0_{3times 3}&{} -I_{3times 3}\ 0_{3times 3}&{}I_{3times 3}&{}-{tilde{omega }}_{ib}^btimes &{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3} end{bmatrix},\ G_l=&begin{bmatrix} I_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}-I_{3times 3}&{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}I_{3times 3}&{}0_{3times 3}\ 0_{3times 3}&{}0_{3times 3}&{}0_{3times 3}&{}I_{3times 3} end{bmatrix}, w=begin{bmatrix}w_g^b\ w_a^b \ w_{b_g}^b \ w_{b_a}^bend{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_{3times 3}+phi ^btimes )l^b-r_{GNSS}^e-n_{r,GNSS}\ =,&delta r_{IMU}^e-C_b^e(l^btimes ) phi ^b-n_{r,GNSS}=delta r_{ib}^e-C_b^e(l^btimes ) phi ^b-n_{r,GNSS}\ =,&-{tilde{C}}_b^e Jrho _r^b-C_b^e(l^btimes ) phi ^b-n_{r,GNSS}\ approx&-{tilde{C}}_b^e Jrho _r^b-{tilde{C}}_b^e(l^btimes ) 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^btimes )&0_{3times 3}&-{tilde{C}}_{b}^e&0_{3times 3}&0_{3times 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^btimes )&0_{3times 3}&-I_{3times 3}&0_{3times 3}&0_{3times 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^eexp _G(phi ^btimes )}\ 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^eapprox C_{b’}^bapprox I_{3times 3}+phi ^btimes) 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).