1. Introduction
The recent development of micro-electro-mechanical system (MEMS) technologies has made MEMS inertial measurement units (IMU) available at a lower price and considerable precision, making MEMS-IMU an ideal inertial sensor in many industrial and military navigation appliances (Jang and Liccardo, Reference Jang and Liccardo2007; Leclerc, Reference Leclerc2007; Ravish et al., Reference Ravish, Jigyasha and Pannaga2013). With the global navigation satellite system (GNSS) signal thus aided, land and aerial vehicles can achieve high-precision integrated navigation. However, due to the low heading damping in strapdown inertial navigation system (SINS)/GNSS integrated navigation, additional heading information is needed to improve the precision, such as magnetometer (Cui et al., Reference Cui, Zhao and Hu2019) and polarised light (Liang et al., Reference Liang, Bai and Liu2020). The magnetometer is more common in navigation applications and its calibration method has been investigated extensively (Wu et al., Reference Wu, Zhou, Chen, Fourati and Li2018; Wu, Reference Wu2019; Xiang et al., Reference Wu, Zou and Liu2019). Therefore, a well-calibrated magnetometer-aided attitude estimation system has become a favoured supplement in low-cost applications (Wu et al., Reference Wu, Liu, Huang, Jin, Wu and Yu2016; Miao et al., Reference Miao, Cao and Ou2014).
Extended Kalman Filter (EKF) is the most popular estimator in SINS; it utilises the Riccati equation to achieve adaptive gain tuning to handle the time-varying nature of the linearised error equation (Sebesta and Boizot, Reference Sebesta and Boizot2014; Brossard et al., Reference Brossard, Barrau and Bonnabel2019). The conventional EKF state error is usually the first-order linearisation of the real nonlinear system, which requires state error to be sufficiently small. However, this strict condition relies greatly on the actual estimated states, which cannot always be met when the navigation system states have a large deviation from real initial values, possibly causing the covariance inconsistency problem and even diverging the Kalman filter (Hartley et al., Reference Hartley, Ghaffari and Eustice2020). For instance, the paper Robert and Perrot (Reference Robert and Perrot2017) illustrated a case where vibration of the vehicle mistakenly causes the estimated covariance of the unobservable error state to undergo continuous reduction. To solve the covariance inconsistency problem, Huang et al. (Reference Huang, Mourikis and Roumeliotis2010) used the observability-constrained EKF (OC-EKF) to optimise the linearisation points and therefore minimise the linearisation error. Schmidt (Reference Schmidt1966) proposed the Schmidt-Kalman filter (S-KF) setting the unobservable state covariance to zero.
Nevertheless, these methodologies do not change the nature of the nonlinearity problem of state errors due to the linear approximation. Wang et al. (Reference Wang, Wu, Zhou and He2018) proposed the state transformation EKF (ST-EKF), which used a new velocity nonlinear error state, and showed robustness and accuracy improvement in high-precision navigation applications significantly. The invariant Kalman filter (Barrau and Bonnabel, Reference Barrau and Bonnabel2017a; Reference Barrau and Bonnabel2020) which utilised the theory of symmetry preserving observers demonstrated that, for the left-invariant system on the Lie group, the nonlinear observer could be designed to make the right-invariant (or left-invariant) error state obey the autonomous equation. This nonlinear observer has a rare property of trajectory independence, which means that even if the navigation state undergoes nonlinear changes, the error state evolution still follows a linear differential equation. The invariant Kalman filter theory has been widely used and provided performance improvement in many navigation engineering cases (Barrau and Bonnabel, Reference Barrau and Bonnabel2017b; Brossard et al., Reference Brossard, Bonnabel and Barrau2018; Hartley et al., Reference Hartley, Ghaffari and Eustice2020).
The contributions and organisation of this paper are as follows. In section 2, the SINS equations with regard to the Earth-centred inertial (ECI) frame resolving in Earth-centred Earth-fixed (ECEF) frame are derived. And then in section 3, the proof that the proposed nonlinear state errors based on the matrix Lie group methodology satisfy trajectory independent error propagation property is given. The common frame definitions of SINS state errors are then presented in section 4. The detailed system equation of LG-EKF is also derived. In section 5, the new measurement equation for SINS/GNSS/magnetometer integration is developed. Section 6 reports a land vehicle field test that was conducted to compare the performance of EKF, ST-EKF and LG-EKF, in which LG-EKF achieved more accurate estimation of all the three attitude angles. The LG-EKF proposed in this paper can be applied in integrated navigation systems no matter whether they are based on high-end or low-end inertial sensors.
2. SINS navigation differential equation in ECEF frame
The new full state SINS differential equations are derived first to obtain the SINS state error differential equations subsequently. The coordinate frames are declared as follows.
i frame: ECI frame.
e frame: ECEF frame.
n frame: Local navigation frame. Its origin is the object described by the navigation solution. Here it is the North-East-Down frame.
b frame: Body frame. This is commonly the IMU frame, which comprises the origin and orientation of the object described by the navigation solution.
${\boldsymbol C}_b^e$ represents the direction cosine matrix transforming b frame to e frame; 
${\boldsymbol v}_{ib}^e$ is the body velocity with regard to the i frame resolving in e frame; 
${\boldsymbol r}_{ib}^e$ is the position vector pointing from i frame's origin to b frame's origin projected in e frame. Other symbols also follow similar naming rules.
The time derivative of 
${\boldsymbol C}_b^e$ can be derived
\begin{equation}\dot{{\boldsymbol C}}_b^e = {\boldsymbol C}_b^e({{\boldsymbol \omega }_{ib}^b \wedge } )- ({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol C}_b^e\end{equation}
and the time derivative of 
${\boldsymbol v}_{ib}^i$ is also expressed as
\begin{equation}\dot{{\boldsymbol v}}_{ib}^i = {\boldsymbol C}_b^i{{\boldsymbol f}^b} + {\boldsymbol g}_{ib}^i\end{equation}
where 
${{\boldsymbol f}^b}$ is the specific force; 
${\boldsymbol g}_{ib}^i$ is the local gravitational acceleration projected in i frame.
From Equation (2) the time derivative of 
${\boldsymbol v}_{ib}^e$ can be derived
\begin{equation}\dot{{\boldsymbol v}}_{ib}^e = \frac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol C}_i^e{\boldsymbol v}_{ib}^i} )={-} ({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol v}_{ib}^e + {\boldsymbol C}_i^e{\boldsymbol C}_b^i{{\boldsymbol f}^b} + {\boldsymbol C}_i^e{\boldsymbol g}_{ib}^i = {\boldsymbol C}_b^e{{\boldsymbol f}^b} - ({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol v}_{ib}^e + {\boldsymbol g}_{ib}^e\end{equation}
where the differential equation 
$\dot{{\boldsymbol C}}_e^i = {\boldsymbol C}_e^i({{\boldsymbol \omega }_{ie}^e \wedge } )$ is used; 
${\boldsymbol g}_{ib}^e$ is the local gravitational acceleration projected in e frame.
The time derivative of the 
${\boldsymbol r}_{ib}^e$ can be derived as
\begin{equation}\dot{{\boldsymbol r}}_{ib}^e = \frac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol C}_i^e{\boldsymbol r}_{ib}^i} )={-} ({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol C}_i^e{\boldsymbol r}_{ib}^i + {\boldsymbol C}_i^e\dot{{\boldsymbol r}}_{ib}^i ={-} ({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol r}_{ib}^e + {\boldsymbol v}_{ib}^e\end{equation}Equations (1), (3) and (4) form the full state SINS navigation differential equations, which build up the basis of the derivation of the nonlinear state error equations in section 4.
3. Proof of the invariance property of new SINS nonlinear states
This section gives the detailed proof that the navigation states defined in Equations (1), (3) and (4) satisfy the autonomous error equation in Barrau and Bonnabel (Reference Barrau and Bonnabel2017a), which means that they have invariance property. For more details of the invariance property, refer to Barrau and Bonnabel (Reference Barrau and Bonnabel2017a).
Define the dynamic system differential equation as
\begin{equation}\frac{\textrm{d}}{{\textrm{dt}}}{\boldsymbol \chi } = {f_{{{\boldsymbol u}_t}}}({\boldsymbol \chi })\end{equation}
where the state 
${\boldsymbol \chi }$ lives in the Lie group and 
${{\boldsymbol u}_t}$ represents a certain input. Define the right-invariant error as 
${{\boldsymbol \eta }^R} = {\boldsymbol \chi }{\tilde{{\boldsymbol \chi }}^{ - 1}}$, where 
$\tilde{{\boldsymbol \chi }}$ is the estimated state. According to Theorem 1 in Barrau and Bonnabel (Reference Barrau and Bonnabel2017a), if 
${f_{{{\bf u}_t}}}({\boldsymbol \chi })$ satisfies Equation (6), then 
${{\boldsymbol \eta }^R}$ is said to have a state trajectory independent property.
\begin{equation}{f_{{{\bf u}_t}}}({{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B}) = {f_{{{\bf u}_t}}}({{\boldsymbol \chi }_A}){{\boldsymbol \chi }_B} + {{\boldsymbol \chi }_A}{f_{{{\bf u}_t}}}({{\boldsymbol \chi }_B}) - {{\boldsymbol \chi }_A}{f_{{{\bf u}_t}}}({{\boldsymbol I}_d}){{\boldsymbol \chi }_B}\end{equation}
where the two states 
${{\boldsymbol \chi }_A}$ and 
${{\boldsymbol \chi }_B}$ live in the Lie group and 
${{\boldsymbol I}_d}$ is the identity matrix.
Moreover, if 
${{\boldsymbol \eta }^R}$ is state trajectory independent, the Equation (7) is satisfied as well.
\begin{equation}\frac{\textrm{d}}{{\textrm{dt}}}{{\boldsymbol \eta }^R} = {g_{{{\boldsymbol u}_t}}}({{\boldsymbol \eta }^R}) = {f_{{{\boldsymbol u}_t}}}({{\boldsymbol \eta }^R}) - {{\boldsymbol \eta }^R}{f_{{{\boldsymbol u}_t}}}({{\boldsymbol I}_d})\end{equation} The property of Equation (6) is proved first. The system state 
${\boldsymbol \chi }$ and its corresponding inverse 
${{\boldsymbol \chi }^{ - 1}}$ are defined as
\begin{equation}{\boldsymbol \chi } = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right],\;\;{{\boldsymbol \chi }^{ - 1}} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_e^b}&{ - {\boldsymbol v}_{ib}^b}&{ - {\boldsymbol r}_{ib}^b}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\end{equation} Then, the differential of 
${\boldsymbol \chi }$ can be derived
\begin{align}
\dfrac{\textrm{d}}{{\textrm{dt}}}{\boldsymbol \chi } &=
{f_{{{\boldsymbol u}_t}}}({\boldsymbol \chi }) =
\dfrac{\textrm{d}}{{\textrm{dt}}}\left[
{\begin{array}{*{20}{c}} {{\boldsymbol
C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}} \right] = \left[
{\begin{array}{*{20}{c}} {\dot{{\boldsymbol
C}}_b^e}&{\dot{{\boldsymbol v}}_{ib}^e}&{\dot{{\boldsymbol
r}}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\
{{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right] =
{\boldsymbol \chi }{{\boldsymbol W}_1} + {{\boldsymbol
W}_2}{\boldsymbol \chi }\notag\\ & = \left[
{\begin{array}{*{20}{c}} {{\boldsymbol
C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}} \right]\left[
{\begin{array}{*{20}{c}} {({{\boldsymbol \omega }_{ib}^b
\wedge } )}&{{{\boldsymbol f}^b}}&{{{\boldsymbol 0}_{3
\times 1}}}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\
{{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right] +
\left[ {\begin{array}{*{20}{c}} { - ({{\boldsymbol \omega
}_{ie}^e \wedge } )}&{{\boldsymbol g}_{ib}^e}&{{\boldsymbol
v}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\
{{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}}
\right]\left[ {\begin{array}{*{20}{c}} {{\boldsymbol
C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}} \right]
\end{align}where
\begin{equation}{{\boldsymbol W}_1} = \left[ {\begin{array}{*{20}{c}} {({{\boldsymbol \omega }_{ib}^b \wedge } )}&{{{\boldsymbol f}^b}}&{{{\boldsymbol 0}_{3 \times 1}}}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right],\;\;{{\boldsymbol W}_2} = \left[ {\begin{array}{*{20}{c}} { - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{{\boldsymbol g}_{ib}^e}&{{\boldsymbol v}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right]\end{equation}Let
\begin{equation}{{\boldsymbol \chi }_A} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_{bA}^e}&{{\boldsymbol v}_{ibA}^e}&{{\boldsymbol r}_{ibA}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right],\;\;{{\boldsymbol \chi }_B} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_{bB}^e}&{{\boldsymbol v}_{ibB}^e}&{{\boldsymbol r}_{ibB}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\end{equation}therefore
\begin{equation}{{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_{bA}^e}&{{\boldsymbol v}_{ibA}^e}&{{\boldsymbol r}_{ibA}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_{bB}^e}&{{\boldsymbol v}_{ibB}^e}&{{\boldsymbol r}_{ibB}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_{bA}^e{\boldsymbol C}_{bB}^e}&{{\boldsymbol v}_{ibA}^e + {\boldsymbol C}_{bA}^e{\boldsymbol v}_{ibB}^e}&{{\boldsymbol r}_{ibA}^e + {\boldsymbol C}_{bA}^e{\boldsymbol r}_{ibB}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\end{equation} According to Equation (9), we have 
${f_{{{\boldsymbol u}_t}}}({{\boldsymbol I}_d}) = {{\boldsymbol W}_1} + {{\boldsymbol W}_2}$ and 
${f_{{u_t}}}({{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B}) = {{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B}{{\boldsymbol W}_1} + {{\boldsymbol W}_2}{{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B}$.
Therefore, the right side of Equation (6) can be written as
\begin{align}
&{f_{{{\boldsymbol u}_t}}}({{\boldsymbol \chi
}_A}){{\boldsymbol \chi }_B} + {{\boldsymbol \chi
}_A}{f_{{{\boldsymbol u}_t}}}({{\boldsymbol \chi }_B}) -
{{\boldsymbol \chi }_A}{f_{{{\boldsymbol
u}_t}}}({{\boldsymbol I}_d}){{\boldsymbol \chi }_B}\notag\\ &\quad =
{{\boldsymbol \chi }_A}{{\boldsymbol W}_1}{{\boldsymbol
\chi }_B} + {{\boldsymbol W}_2}{{\boldsymbol \chi
}_A}{{\boldsymbol \chi }_B} + {{\boldsymbol \chi
}_A}{{\boldsymbol \chi }_B}{{\boldsymbol W}_1} +
{{\boldsymbol \chi }_A}{{\boldsymbol W}_2}{{\boldsymbol
\chi }_B} - {{\boldsymbol \chi }_A}({{{\boldsymbol W}_1} +
{{\boldsymbol W}_2}} ){{\boldsymbol \chi }_B}\notag\\ &\quad =
{{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B}{{\boldsymbol
W}_1} + {{\boldsymbol W}_2}{{\boldsymbol \chi
}_A}{{\boldsymbol \chi }_B} = {f_{{{\boldsymbol
u}_t}}}({{\boldsymbol \chi }_A}{{\boldsymbol \chi }_B})
\end{align} Here we have proved that the navigation states defined in Equations (1), (3) and (4) satisfy the property of Equation (6). The right-invariant error 
${{\boldsymbol \eta }^R}$ satisfies the property of Equation (7) is also proved next.
From Equation (8), we have
\begin{equation}{{\boldsymbol \eta }^R} = {\boldsymbol \chi }{\tilde{{\boldsymbol \chi }}^{ - 1}} = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} {\tilde{{\boldsymbol C}}_e^b}&{ - \tilde{{\boldsymbol v}}_{ib}^b}&{ - \tilde{{\boldsymbol r}}_{ib}^b}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b}&{{\boldsymbol v}_{ib}^e - {\boldsymbol C}_b^e\tilde{{\boldsymbol v}}_{ib}^b}&{{\boldsymbol r}_{ib}^e - {\boldsymbol C}_b^e\tilde{{\boldsymbol r}}_{ib}^b}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}} \right]\end{equation}Substitute Equation (14) into (7),
\begin{equation}\begin{array}{ccccc} \dfrac{\textrm{d}}{{\textrm{dt}}}{{\boldsymbol \eta }^R} & = {g_{{{\boldsymbol u}_t}}}({{\boldsymbol \eta }^R}) = {f_{{{\boldsymbol u}_t}}}({{\boldsymbol \eta }^R}) - {{\boldsymbol \eta }^R}{f_{{{\boldsymbol u}_t}}}({{\boldsymbol I}_d}) = {{\boldsymbol \eta }^R}{{\boldsymbol W}_1} + {{\boldsymbol W}_2}{{\boldsymbol \eta }^R} - {{\boldsymbol \eta }^R}({{{\boldsymbol W}_1} + {{\boldsymbol W}_2}} )\\ & = {{\boldsymbol W}_2}{{\boldsymbol \eta }^R} - {{\boldsymbol \eta }^R}{{\boldsymbol W}_2} = \left[ {\begin{array}{*{20}{c}} { - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{{\boldsymbol g}_{ib}^e}&{{\boldsymbol v}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right]{{\boldsymbol \eta }^R} - {{\boldsymbol \eta }^R}\left[ {\begin{array}{*{20}{c}} { - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{{\boldsymbol g}_{ib}^e}&{{\boldsymbol v}_{ib}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \\ {{{\boldsymbol 0}_{1 \times 3}}} & 0 & 0 \end{array}} \right] \end{array}\end{equation}Thus, the property in Equation (7) has been verified. The proofs above have proved that the navigation states defined in Equations (1), (3) and (4) possess the rare trajectory independent error propagation property, which means that even if the change of the state error is nonlinear, its propagation remains linear. In the next section, the differential equations of the SINS nonlinear state errors are derived in detail.
4. The differential equations of the SINS nonlinear state error
The new SINS nonlinear state errors related to the attitude, velocity and position can be obtained from the right-invariant errors in Equation (14).
Define the new SINS state errors
\begin{equation}\begin{split}
{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b & = \exp
(\boldsymbol{\phi}^{e} \wedge)\\ {\boldsymbol
J\boldsymbol{\rho} }_{\boldsymbol v}^e & = {\boldsymbol v}_{ib}^e -
{\boldsymbol C}_b^e\tilde{{\boldsymbol v}}_{ib}^b =
({{\boldsymbol v}_{ib}^e - \tilde{{\boldsymbol v}}_{ib}^e}
)+ ({{\boldsymbol I} - {\boldsymbol
C}_b^e\tilde{{\boldsymbol C}}_e^b} )\tilde{{\boldsymbol
v}}_{ib}^e ={-} \delta {\boldsymbol v}_{ib}^e +
({\boldsymbol I} - \exp (\boldsymbol{\phi }^{e}
\wedge))\tilde{{\boldsymbol v}}_{ib}^e\\ {\boldsymbol
J\boldsymbol{\rho} }_{\boldsymbol r}^e & = {\boldsymbol r}_{ib}^e -
{\boldsymbol C}_b^e\tilde{{\boldsymbol r}}_{ib}^b =
({{\boldsymbol r}_{ib}^e - \tilde{{\boldsymbol r}}_{ib}^e}
)+ ({{\boldsymbol I} - {\boldsymbol
C}_b^e\tilde{{\boldsymbol C}}_e^b} )\tilde{{\boldsymbol
r}}_{ib}^e ={-} \delta {\boldsymbol r}_{ib}^e +
({\boldsymbol I} - \exp (\boldsymbol{\phi}^{e} \wedge))\tilde{{\boldsymbol r}}_{ib}^e
\end{split}\end{equation}
where 
${\boldsymbol{\phi }^e}$ is the misalignment angle vector, 
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol v}^e$ and 
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e$ denote the new nonlinear velocity error vector and position error vector, respectively; 
$\exp ({\cdot} )$ is the matrix exponential. Compared with the linear state errors, which are the direct difference of the vectors in the computational coordinate frame and the real coordinate frame, the new nonlinear state errors are defined in the common coordinate frame (Andrle and Crassidis, Reference Andrle and Crassidis2015). More specifically, the new nonlinear state errors consider both the magnitude difference and the direction difference of two vectors, which can lead to provable convergence properties of the Kalman filter.
Rewrite the right-invariant error 
${{\boldsymbol \eta }^R}$ in Equation (14)
\begin{equation}{{\boldsymbol
\eta }^R} = \left[ {\begin{array}{*{20}{c}} {\exp (
{\boldsymbol{\phi }^e} \wedge)}&{{\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol v}^e}&{{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol
r}^e}\\ {{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\
{{{\boldsymbol 0}_{1 \times 3}}} & 0 & 1 \end{array}}
\right]\end{equation} In order to establish the system equation of the Kalman filter, the differential equations of 
${\boldsymbol{\phi }^e}$,
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol v}^e$ and 
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e$ must be derived.
Assume that the gyroscope and accelerometer measurements contain noise and can be expressed as
\begin{equation}\begin{split}
\tilde{{\boldsymbol \omega }}_{ib}^b & = {\boldsymbol
\omega }_{ib}^b + \delta {\boldsymbol \omega }_{ib}^b =
{\boldsymbol \omega }_{ib}^b + {{\boldsymbol \varepsilon
}^b} + {{\boldsymbol w}_g}\\ {{\tilde{{\boldsymbol f}}}^b}
& = {{\boldsymbol f}^b} + \delta {{\boldsymbol f}^b} =
{{\boldsymbol f}^b} + {\nabla ^b} + {{\boldsymbol w}_a}
\end{split}\end{equation}
where 
${{\boldsymbol w}_g}$ is the white noise vector of the gyroscopes and 
${{\boldsymbol w}_a}$ is the white noise vector of the accelerometers; 
$\boldsymbol{\varepsilon}^{b}$ is the bias vector of gyroscopes and 
${\nabla ^b}$ is the bias vector of accelerometers, which are constant values with differential equations
\begin{equation}{\dot{{\boldsymbol \varepsilon }}^b} = \boldsymbol{0},\;\;{\dot{\nabla }^b} = \boldsymbol{0}\end{equation} The differential equation of the misalignment angle 
${\boldsymbol{\phi }^e}$ is derived as follows. First, the derivative of 
${\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b$ can be derived as
\begin{equation}\begin{split}
\dfrac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol
C}_b^e\tilde{{\boldsymbol C}}_e^b} )& = \dot{{\boldsymbol
C}}_b^e\tilde{{\boldsymbol C}}_e^b + {\boldsymbol
C}_b^e\dot{{\tilde{\boldsymbol{C}}}}_e^b\\ & = {\boldsymbol
C}_b^e\tilde{{\boldsymbol C}}_e^b({{\boldsymbol \omega
}_{ie}^e \wedge } )- ({{\boldsymbol \omega }_{ie}^e \wedge
} ){\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b -
{\boldsymbol C}_b^e({({{{\boldsymbol \varepsilon }^b} +
{{\boldsymbol w}_g}} )\wedge } ){\boldsymbol C}_e^b
\end{split}\end{equation} The first-order approximation of 
${\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b$ is
\begin{equation}{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b = \exp ({{\boldsymbol{\phi }^e} \wedge } )\approx {\boldsymbol I} + {\boldsymbol{\phi }^e} \wedge\end{equation} Substitute Equation (21) into (20), then the derivative of 
${\boldsymbol{\phi }^e}$ can be expressed as
\begin{equation}\frac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol{\phi }^e}} )={-} {\boldsymbol \omega }_{ie}^e \wedge {\boldsymbol{\phi }^e} - {\boldsymbol C}_b^e{{\boldsymbol \varepsilon }^b} - {\boldsymbol C}_b^e{{\boldsymbol w}_g}\end{equation} The differential equation of the new velocity error 
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol v}^e$ is derived as
\begin{align}
\dfrac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol v}^e} )& =
\dfrac{\textrm{d}}{{\textrm{dt}}}({({{\boldsymbol v}_{ib}^e
- \tilde{{\boldsymbol v}}_{ib}^e} )+ ({{\boldsymbol I} -
{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b}
)\tilde{{\boldsymbol v}}_{ib}^e} )=
\dfrac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol v}_{ib}^e -
{\boldsymbol C}_b^e\tilde{{\boldsymbol
C}}_e^b\tilde{{\boldsymbol v}}_{ib}^e} )\notag\\ & = \left[
{({{\boldsymbol g}_{ib}^e \wedge } )+ \dfrac{\mu
}{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}({{\boldsymbol r}_{ib}^e \wedge }
)} \right]{\boldsymbol{\phi }^e} - ({{\boldsymbol \omega
}_{ie}^e \wedge } ){\boldsymbol J\rho }_{\boldsymbol v}^e -
\dfrac{\mu }{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}({{\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol r}^e} )\notag\\ &\quad - ({\tilde{{\boldsymbol
v}}_{ib}^e \wedge } ){\boldsymbol C}_b^e({{{\boldsymbol
\varepsilon }^b} + {{\boldsymbol w}_g}} )- {\boldsymbol
C}_b^e({{\nabla^b} + {{\boldsymbol w}_a}}
)\end{align}
where 
$\delta {\boldsymbol g}_{ib}^e \approx{-} \frac{\mu }{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}\delta {\boldsymbol r}_{ib}^e \approx \frac{\mu }{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}({{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e - {\boldsymbol r}_{ib}^e \wedge {\boldsymbol{\phi }^e}} )$ is used and 
$\mu$ is defined in Groves (Reference Groves2013).
The differential equation of the new position error 
${\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e$ is derived as
\begin{equation}\begin{split}
\dfrac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol r}^e} )&= \dot{{\boldsymbol r}}_{ib}^e -
{\boldsymbol C}_b^e\tilde{{\boldsymbol
C}}_e^b\dot{{\tilde{\boldsymbol{r}}}}_{ib}^e -
\dfrac{\textrm{d}}{{\textrm{dt}}}({{\boldsymbol
C}_b^e\tilde{{\boldsymbol C}}_e^b} )\tilde{{\boldsymbol
r}}_{ib}^e\\ &= {\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol v}^e -
({{\boldsymbol \omega }_{ie}^e \wedge } ){\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol r}^e - ({\tilde{{\boldsymbol r}}_{ib}^e
\wedge } ){\boldsymbol C}_b^e({{{\boldsymbol \varepsilon
}^b} + {{\boldsymbol w}_g}}
)\end{split}\end{equation}According to Equations (19), (22), (23) and (24), all the state error equations of the LG-EKF can be involved as a unified form as Equation (25)
\begin{equation}{\dot{{\boldsymbol x}}_{LG - EKF}} = {{\boldsymbol F}_{LG - EKF}}{{\boldsymbol x}_{LG - EKF}} + {{\boldsymbol G}_{LG - EKF}}{\boldsymbol w}\end{equation}
where 
${{\boldsymbol F}_{LG - EKF}}$ is the system matrix, 
${{\boldsymbol x}_{LG - EKF}}$ is the state error vector, 
${{\boldsymbol G}_{LG - EKF}}$ is the noise shaping matrix. They are detailed as below
\begin{align}{{\boldsymbol x}_{LG - EKF}} &= {\left[ {\begin{array}{*{20}{c}} {{{({{\boldsymbol{\phi }^e}} )}^T}}&{{{({{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol v}^e} )}^T}}&{{{({{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e} )}^T}}&{{{({{{\boldsymbol \varepsilon }^b}} )}^T}}&{{{({{\nabla^b}} )}^T}} \end{array}} \right]^T}\end{align}
\begin{align}{{\boldsymbol G}_{LG - EKF}} &= \left[ {\begin{array}{*{20}{c}} { - {\boldsymbol C}_b^e}&{{{\boldsymbol 0}_{3 \times 3}}}\\ { - ({{\boldsymbol v}_{ib}^e \wedge } ){\boldsymbol C}_b^e}&{ - {\boldsymbol C}_b^e}\\ { - ({\tilde{{\boldsymbol r}}_{ib}^e \wedge } ){\boldsymbol C}_b^e}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}} \end{array}} \right]\end{align}
\begin{align}{{\boldsymbol F}_{LG - EKF}} &= \left[ {\begin{array}{*{20}{c}} { - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{ - {\boldsymbol C}_b^e}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {\left( \begin{array}{l} ({{\boldsymbol g}_{ib}^e \wedge } )\\ + \dfrac{\mu }{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}({{\boldsymbol r}_{ib}^e \wedge } )\end{array} \right)}&{ - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{ - \dfrac{\mu }{{{{|{\boldsymbol{r}_{ib}^e} |}^3}}}}&{ - ({{\boldsymbol v}_{ib}^e \wedge } ){\boldsymbol C}_b^e}&{ - {\boldsymbol C}_b^e}\\ {{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol I}_{3 \times 3}}}&{ - ({{\boldsymbol \omega }_{ie}^e \wedge } )}&{ - ({\tilde{{\boldsymbol r}}_{ib}^e \wedge } ){\boldsymbol C}_b^e}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}} \end{array}} \right]\end{align}Compare the system matrix of LG-EKF in Equation (28) with the system matrix of EKF in Groves (Reference Groves2013); the specific force term has been replaced by the gravitational acceleration term. In engineering applications, the implementation of the conventional EKF requires high-frequency propagation of the system matrix to adequately cover the bandwidth of specific force (Wang et al., Reference Wang, Wu, Zhou and He2018, Reference Wang, Wu, He and Pan2019a, Reference Wang, Wu, He and Pan2019b, Reference Wang, Wu, He, Li and Pan2019c). However, in the LG-EKF, high-speed processing of the system matrix has been unnecessary since the gravitational acceleration is more robust for local navigation problems. Therefore, the propagation and updating processes of the Kalman filter can be executed simultaneously at a relatively slower rate, which is more computationally efficient. It should be pointed out that the complete Lie formulae expressions have infinity terms and the process and observation models of the Kalman filter are the approximation of linearisation which means that the Jacobian of the Lie exponential is not merely the wedge operation (Wu et al., Reference Wu, Zou and Liu2020).
The system state error models above are capable of use in almost any SINS based integrated navigation system. The next section develops the velocity and position observation equations, and the heading angle measurement equations for the low-cost MIMU/GNSS/magnetometer integrated navigation system.
5. LG-EKF measurement equations for MIMU/GNSS/magnetometer integrated system
The velocity and position measurements are provided by GNSS information. The heading angle measurement is provided by the magnetometer.
The LG-EKF measurement models are a little more complicated than that of the EKF. On the one hand, the navigation parameters 
${\boldsymbol v}_{ib}^e$ and 
$r_{ib}^e$ resolved in SINS are with regard to i frame resolving in e frame, while the GNSS measurements are generally with regard to e frame resolving in e frame. Thus, the following state transformation equations are needed
\begin{equation}\begin{split} \tilde{{\boldsymbol r}}_{ib}^e &= \tilde{{\boldsymbol r}}_{eb}^e,\tilde{{\boldsymbol r}}_{ib}^i = \tilde{{\boldsymbol r}}_{eb}^i,\delta \tilde{{\boldsymbol r}}_{ib}^e = \delta \tilde{{\boldsymbol r}}_{eb}^e,\tilde{{\boldsymbol v}}_{ib}^i = \tilde{{\boldsymbol v}}_{eb}^i + \boldsymbol{\omega }_{ie}^i \times \tilde{{\boldsymbol r}}_{ib}^i,\tilde{{\boldsymbol v}}_{ib}^e = \tilde{{\boldsymbol v}}_{eb}^e + \boldsymbol{\omega }_{ie}^e \times \tilde{{\boldsymbol r}}_{eb}^e\\ \tilde{{\boldsymbol v}}_{eb}^e &= \tilde{{\boldsymbol v}}_{ib}^e - \boldsymbol{\omega }_{ie}^e \times \tilde{{\boldsymbol r}}_{eb}^e,\delta \tilde{{\boldsymbol v}}_{eb}^e = \delta \tilde{{\boldsymbol v}}_{ib}^e - \boldsymbol{\omega }_{ie}^e \times \boldsymbol{\delta }\tilde{{\boldsymbol r}}_{eb}^e \end{split}\end{equation}
where 
$\delta \tilde{{\boldsymbol r}}_{ib}^e$ is the position error with regard to i frame resolving in e frame and 
$\delta \tilde{{\boldsymbol r}}_{eb}^e$ is the position error with regard to e frame resolving in e frame.
The measurement models are described as
\begin{equation}\begin{split}
\delta {{\bf z}_v} &= \delta \boldsymbol{v}_{ib}^e + {{\boldsymbol \upsilon }_{v,3 \times 1}} ={-} {\boldsymbol J\boldsymbol{\rho}
}_{\boldsymbol v}^e + [\tilde{\boldsymbol{v}}_{ib}^e \wedge
]{\boldsymbol{\phi }^e} + {{\boldsymbol \upsilon }_{v,3 \times
1}}\\ \delta {{\bf z}_r} &= \delta \boldsymbol{r}_{ib}^e +
{{\boldsymbol \upsilon }_{r,3 \times 1}} ={-} {\boldsymbol J\boldsymbol{\rho}
}_r^e + [\tilde{\boldsymbol{r}}_{ib}^e \wedge
]{\boldsymbol{\phi }^e} + {{\boldsymbol \upsilon }_{r,3 \times 1}}
\end{split}\end{equation}
where 
$\delta {{\bf z}_v}$ and 
$\delta {{\bf z}_r}$ are the velocity and position errors formed by SINS and GNSS. 
${{\boldsymbol \upsilon }_{v,3 \times 1}}$ and 
${{\boldsymbol \upsilon }_{r,3 \times 1}}$ are the corresponding measurement noises, which are generally considered as white noise with certain covariance.
On the other hand, the heading angle 
${\psi ^n}$ provided by the magnetometer is usually projected in the n frame, whereas the misalignment angle 
${\boldsymbol{\phi }^e}$ is defined in the e frame. Therefore, a relationship of 
${\psi ^n}$ with misalignment angle defined in e frame is needed.
Consider the transformation matrix 
${\boldsymbol C}_n^e$
\begin{equation}{\boldsymbol C}_e^n\textrm{ = }\left[ {\begin{array}{*{20}{c}} { - \sin L\cos \lambda }&{ - \sin L\sin \lambda }&{\cos L}\\ { - \sin \lambda }&{\cos \lambda } & 0 \\ { - \cos L\cos \lambda }&{\cos L\sin \lambda }&{ - \sin L} \end{array}} \right]\end{equation}
where L and 
$\lambda $ are latitude and longitude, respectively.
The skew symmetry matrix related to the misalignment angle vector estimated in e frame is expressed in Equation (32).
\begin{equation}{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b= {\boldsymbol I} + ({{\boldsymbol{\phi }^e} \times } )= \left[ {\begin{array}{*{20}{c}} 1 & { - \phi_z^e}&{\phi_y^e}\\ {\phi_z^e} & 1 &{ - \phi_x^e}\\ { - \phi_y^e}&{\phi_x^e} & 1 \end{array}} \right]\end{equation}
where 
$\phi _x^e,\phi _y^e,\phi _z^e$ are the small misalignment angles represented in e frame.
Similarly, the skew symmetry matrix related to the misalignment angle vector estimated in n frame is expressed in Equation (33) as
\begin{equation}{\boldsymbol C}_b^n\tilde{{\boldsymbol C}}_n^b = \left[ {\begin{array}{*{20}{c}} 1 & { - {\phi_D}}&{{\phi_E}}\\ {{\phi_D}} & 1 &{ - {\phi_N}}\\ { - {\phi_E}}&{{\phi_N}} & 1 \end{array}} \right]\end{equation}
where 
${\phi _N},{\phi _E},{\phi _D}$ are the small misalignment angles represented in n frame.
Then the following equation can be formed from Equations (32) and (33)
\begin{equation}\begin{split}
{\boldsymbol C}_b^e\tilde{{\boldsymbol C}}_e^b & =
{\boldsymbol C}_n^e{\boldsymbol C}_b^n\tilde{{\boldsymbol
C}}_n^b\tilde{{\boldsymbol C}}_e^n\\ & \approx \left[
{\begin{array}{*{20}{c}} { - \sin L\cos \lambda }&{ - \sin
\lambda }&{ - \cos L\cos \lambda }\\ { - \sin L\sin \lambda
}&{\cos \lambda }&{\cos L\sin \lambda }\\ {\cos L} & 0 & { -
\sin L} \end{array}} \right]
\left[ {\begin{array}{*{20}{c}}
1 & { - {\phi_D}}&{{\phi_E}}\\ {{\phi_D}} & 1 &{ - {\phi_N}}\\ {
- {\phi_E}}&{{\phi_N}} & 1 \end{array}} \right]\\
&\quad \times \left[
{\begin{array}{*{20}{c}} { - \sin L\cos \lambda }&{ - \sin
L\sin \lambda }&{\cos L}\\ { - \sin \lambda }&{\cos \lambda
} & 0 \\ { - \cos L\cos \lambda }&{\cos L\sin \lambda }&{ -
\sin L} \end{array}} \right]\\ & = \left[
{\begin{array}{*{20}{c}} 1 & { - \phi_z^e}&{\phi_y^e}\\
{\phi_z^e} & 1 &{ - \phi_x^e}\\ { - \phi_y^e}&{\phi_x^e} & 1
\end{array}} \right]
\end{split}\end{equation}Ignoring the horizontal misalignment angles, Equation (34) can be written as
\begin{equation}\begin{split}
\left[ {\begin{array}{*{20}{c}} 1 & { - {\phi_D}} & 0 \\
{{\phi_D}} & 1 & 0 \\ 0 & 0 & 1 \end{array}} \right] &= \left[
{\begin{array}{*{20}{c}} { - \sin L\cos \lambda }&{ - \sin
L\sin \lambda }&{\cos L}\\ { - \sin \lambda }&{\cos \lambda
} & 0 \\ { - \cos L\cos \lambda }&{\cos L\sin \lambda }&{ -
\sin L} \end{array}} \right]\\
&\quad \times \left[ {\begin{array}{*{20}{c}}
1 & { - \phi_z^e}&{\phi_y^e}\\ {\phi_z^e} & 1 &{ - \phi_x^e}\\ {
- \phi_y^e}&{\phi_x^e} & 1 \end{array}} \right]\left[
{\begin{array}{*{20}{c}} { - \sin L\cos \lambda }&{ - \sin
\lambda }&{ - \cos L\cos \lambda }\\ { - \sin L\sin \lambda
}&{\cos \lambda }&{\cos L\sin \lambda }\\ {\cos L} & 0 & { -
\sin L} \end{array}} \right]
\end{split}\end{equation} From Equation (35), 
${\phi _D}$ can be expressed as
\begin{equation}\begin{array}{ccccc} {\phi _D} & = \left[ {\begin{array}{*{20}{c}} { - \sin \lambda }&{\cos \lambda }&0 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} 1 & { - \phi_z^e}&{\phi_y^e}\\ {\phi_z^e} & 1 &{ - \phi_x^e}\\ { - \phi_y^e}&{\phi_x^e} & 1 \end{array}} \right]\left[ {\begin{array}{*{20}{c}} { - \sin L\cos \lambda }\\ { - \sin L\sin \lambda }\\ {\cos L} \end{array}} \right]\\ & = ({ - \cos \lambda \cos L} )\phi _x^e + ({ - \sin \lambda \cos L} )\phi _y^e + ({ - \sin L} )\phi _z^e \end{array}\end{equation}As a result, the measurement equation related to the heading angle is obtained
\begin{equation}\delta {z_\psi } = {\tilde{\psi }^n} - {\psi ^n} ={-} {\phi _D} = {{\boldsymbol H}_\psi }{\boldsymbol{\phi }^e}\end{equation}where
\begin{equation}{{\boldsymbol H}_\psi } = \left[ {\begin{array}{*{20}{c}} {\cos \lambda \cos L}&{\sin \lambda \cos L}&{\sin L} \end{array}} \right]\end{equation} In a unified form, the measurement error 
$\delta {\boldsymbol z}$ is expressed as
\begin{equation}\delta {\boldsymbol z} = {\boldsymbol H}{{\boldsymbol x}_{LG - EKF}} + {\boldsymbol \upsilon }\end{equation}where
\begin{equation}\delta {\boldsymbol z}= \left[ {\begin{array}{*{20}{c}} {\delta {{\boldsymbol z}_v}}\\ {\delta {{\boldsymbol z}_r}}\\ {\delta {{\boldsymbol z}_\psi }} \end{array}} \right],\textrm{ }{\boldsymbol \upsilon } = \left[ {\begin{array}{*{20}{c}} {{{\boldsymbol \upsilon }_{v,3 \times 1}}}\\ \begin{array}{l} {{\boldsymbol \upsilon }_{r,3 \times 1}}\\ {{\boldsymbol \upsilon }_{\psi ,1 \times 1}} \end{array} \end{array}} \right],\textrm{ }{\boldsymbol H} = \left[ {\begin{array}{*{20}{l}} {({\tilde{{\boldsymbol v}}_{ib}^e \times } )}&{ - {{\boldsymbol I}_3}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {({\tilde{{\boldsymbol r}}_{ib}^e \times } )}&{{{\boldsymbol 0}_{3 \times 3}}}&{ - {{\boldsymbol I}_3}}&{{{\boldsymbol 0}_{3 \times 3}}}&{{{\boldsymbol 0}_{3 \times 3}}}\\ {{{\boldsymbol H}_\psi }}&{}&{{{\boldsymbol 0}_{12 \times 1}}}&{}&{} \end{array}} \right]\end{equation}Finally, the navigation parameters should be corrected according to Equation (41)
\begin{equation}\left[
{\begin{array}{*{20}{c}} {{\boldsymbol
C}_b^e}&{{\boldsymbol v}_{ib}^e}&{{\boldsymbol r}_{ib}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}} \right] = \left[
{\begin{array}{*{20}{c}} {\exp ({{\boldsymbol{\phi }^e}
\wedge } )}&{{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol
v}^e}&{{\boldsymbol J\boldsymbol{\rho} }_{\boldsymbol r}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}} \right]\left[
{\begin{array}{*{20}{c}} {\tilde{{\boldsymbol
C}}_b^e}&{\tilde{{\boldsymbol
v}}_{ib}^e}&{\tilde{{\boldsymbol r}}_{ib}^e}\\
{{{\boldsymbol 0}_{1 \times 3}}} & 1 & 0 \\ {{{\boldsymbol 0}_{1
\times 3}}} & 0 & 1 \end{array}}
\right]\end{equation}And the attitude parameters of roll, pitch and yaw in n frame can be obtained from
\begin{equation}{\boldsymbol C}_b^n = {\boldsymbol C}_e^n{\boldsymbol C}_b^e\end{equation}6. Land vehicle field test
The land vehicle field test was conducted to compare the performance of EKF, ST-EKF and LG-EKF (proposed). The experiment platform consists of a GPS signal receiver, a MEMS-IMU (MIMU, Stim-300), a magnetometer chip (HMC5983) and a fibre optic gyroscope IMU (FOG-IMU) as the main reference system. The update interval of the GPS is 1 s, with single point positioning accuracy of 0⋅1 m/s and 5 m for the velocity and the position respectively. The detailed sensor configurations are illustrated in Figure 1. The specifications of the Stim-300 MIMU and HMC5983 are shown in Table 1 and Table 2 respectively. The trajectory of this experiment is drawn on the Ovita map (red line in Figure 2).
Figure 1. Sensor configurations of the land vehicle field test
Figure 2. Trajectory of the land vehicle field test
Table 1. Specifications of the STIM300-IMU
Table 2. Specifications of the HMC5983-magnetometer
The total time length of the test was 1,094 s. All the sensors were well calibrated before the experiment, thanks to the work of He et al. (Reference He, Hu, Zhang, He and Han2020). The magnetometer heading angle error is presented in Figure 3, with mean 0⋅074° and standard deviation 2⋅017°. The initial attitude of the vehicle was given by the magnetometer heading angle and accelerometer-based levelling, it being unnecessary to undergo a static stage to execute the coarse alignment process. The reference heading result from the FOG-IMU integrated navigation system has an accuracy of 0⋅05° (1 σ). The propagation and updating processes of the three Kalman filters – EKF, ST-EKF and LG-EKF – were performed simultaneously at a frequency of 1 Hz. For land vehicle applications, the heading angle is the angle most concerned in attitude estimation since roads are generally relatively flat and do not tilt too much.
Figure 3. Heading error result of magnetometer in the land vehicle field test
The heading angle estimation performance is compared in Figure 4 (blue line, EKF without magnetometer heading angle measurement; red line, EKF with magnetometer heading angle measurement; green line, ST-EKF with magnetometer heading angle measurement; black line, LG-EKF with magnetometer heading angle measurement).
Figure 4. Heading error results in the land vehicle field test
As can be seen in Figure 4 and Table 3, although the heading angle error provided by the magnetometer contains fluctuations and noise, the EKF's heading angle accuracy significantly improves after adding magnetometer heading angle measurement, with its mean from 0⋅7191 ° to 0⋅5617° and its root mean square (RMS) from 0⋅8064° to 0⋅6036°, which verifies the usefulness of fusing heading measurement with GNSS/IMU integrated navigation. The ST-EKF has a relatively remarkable reduction of error mean by about 0⋅1 ° compared with the EKF. The LG-EKF heading error becomes and remains the lowest among the four lines after 700 s, whose mean is 0⋅3606°, and RMS is 0⋅4104°, also the smallest. Therefore, the LG-EKF with nonlinear state errors defined in the Lie group has presented distinctly superior performance in terms of the heading estimation.
Table 3. Mean and RMS of the heading error results
Table 4 and Table 5 list the mean and RMS of the roll and pitch error results with the four methods mentioned above respectively, where it also can be seen that LG-EKF is the one with the best performance. The position and velocity errors of the four methods above are almost the same with high precision and well observable GPS measurement, so only the result of LG-EKF is shown Figure 5 and Figure 6.
Table 4. Mean and RMS of the roll error results
Table 5. Mean and RMS of the pitch error results
Figure 5. Position errors of LG-EKF in the land vehicle field test
Figure 6. Velocity errors of LG-EKF in the land vehicle field test
7. Conclusions
In this paper, an LG-EKF is proposed for the integrated navigation system to tackle the nonlinear problem. The SINS navigation differential equations in the ECEF frame are first derived, and then the novel nonlinear right-invariant state errors of SINS under the Lie group are defined, which are proved to be trajectory independent. The system equations of SINS are developed for the proposed nonlinear state errors. The specific force item is replaced by a gravitational related item in the new velocity differential equation, which improves robustness and accuracy of the navigation result. The corresponding measurement equations of velocity, position and heading angle for the proposed LG-EKF are derived. A land vehicle equipped with a MIMU/GPS/magnetometer system is used to conduct a field test, which validates that the GNSS/IMU integrated navigation system achieves a better attitude estimation with the assistance of a well-calibrated magnetometer. The proposed LG-EKF also has a better heading angle estimation accuracy compared with the traditional EKF and ST-EKF. The proposed LG-EKF with nonlinear state errors is not limited within this specific scenario but is capable of other navigation applications with various sensors, both high-end and low-end. To fully exploit the potentialities of the LG-EKF, there remain many meaningful works could be done. The performance of the LG-EKF should be tested in more applications, for example, evaluating LG-EKF's accuracy and robustness in the integrated navigation assisted with other sensors, like odometer or atmospheric-pressure altimeter, and exploring the possibility of usage in visual-inertial odometry applications and so on.
Funding statement
This work was supported by the Research Project of the National University of Defense Technology (grant number ZK19-26) and supported by the National Natural Science Foundation of China (No. 61573371, No. 61773394).
