An Improved Initial Alignment Method Based on SE₂(3)/EKF for SINS/GNSS Integrated Navigation System with Large Misalignment Angles

Abstract

This paper proposes an improved initial alignment method for a strap-down inertial navigation system/global navigation satellite system (SINS/GNSS) integrated navigation system with large misalignment angles. Its methodology is based on the three-dimensional special Euclidean group and extended Kalman filter (SE₂(3)/EKF) and aims to overcome the challenges of achieving fast alignment under large misalignment angles using traditional methods. To accurately characterize the state errors of attitude, velocity, and position, these elements are constructed as elements of a Lie group. The nonlinear error on the Lie group can then be well quantified. Additionally, a group vector mixed error model is developed, taking into account the zero bias errors of gyroscopes and accelerometers. Using this new error definition, a GNSS-assisted SINS dynamic initial alignment algorithm is derived, which is based on the invariance of velocity and position measurements. Simulation experiments demonstrate that the alignment method based on SE₂(3)/EKF can achieve a higher accuracy in various scenarios with large misalignment angles, while the attitude error can be rapidly reduced to a lower level.

1. Introduction

The initial alignment is a crucial component of the strap-down inertial navigation system (SINS) and is assessed based on its speed and accuracy [1]. Currently, there are two main categories of traditional alignment methods based on misalignment angles—small misalignment angles linear alignment and large misalignment angles nonlinear alignment [2]. Linear alignment models and linear filtering algorithms for small misalignment angles are well established, while research on the alignment problems for large misalignment angles has mainly focused on nonlinear models and nonlinear filtering algorithms [3]. However, these approaches can lead to errors in model linearization, increased computational complexity, and reduced filtering accuracy for large misalignment angles. In the 21st century, modern warfare has demonstrated the significance of high-tech conditions, where precision is a key consideration, alongside speed. Consequently, it is essential to find ways to achieve fast and highly accurate initial alignment under large misalignment angles, for applications like weapon launchers and guided weapons that require emergency mobile transfer. Addressing this urgent problem is crucial from both a system performance perspective in modern warfare and from practical alignment environments, data utilization, and engineering software development [4,5,6]. Hence, studying the linearized uniform initial alignment algorithm without coarse alignment at any misalignment angles is of paramount importance and offers significant theoretical and engineering benefits.

The current most popular mode of navigation is the SINS/Global navigation satellite system (GNSS) integrated navigation model [7,8]. Satellite navigation offers both high accuracy and low cost. In the SINS/GNSS integrated navigation system, GNSS not only improves navigation accuracy by correcting inertial navigation information, but also provides initial position and velocity information for SINS [9]. However, the GNSS cannot directly provide attitude information. In recent years, nonlinear system state estimation has gained attention in the field of inertial navigation for combined navigation and initial alignment. This includes methods such as the extended Kalman filter (EKF) [10], unscented Kalman filter (UKF) [11,12], particle filter (PF) [13], cubature Kalman filter (CKF) [14], unscented particle filter (UPF) [15], distributed Kalman filter (DKF) [16], or a combination of multiple nonlinear filters [17,18]. Among these, the traditional EKF has two drawbacks. Firstly, it requires high accuracy in the initial value of the system state. If the initial value is based on the actual situation, the filter may struggle to converge. Secondly, the EKF can lead to inconsistency. When a new observation is received, the EKF calculates the covariance matrix of the current state based on the linearization of the previous state. However, the actual value of the covariance matrix may not align with this calculated value. Compared to the EKF, the UKF is more practical. However, both the traditional UKF and EKF require prior statistics on known system noise and measurement noise. In practical applications, the accuracy of the filter is inevitably affected by environmental limitations and algorithm problems, leading to a decrease in accuracy or even divergence.

In the traditional EKF framework used for the GNSS/SINS integrated navigation system, state errors such as position and velocity are defined only by considering differences in size, completely ignoring differences in direction. This oversight can lead to inconsistent definitions of state error coordinate systems. In recent years, a new filter has been developed based on Lie groups and manifold theory. Its core idea is to define the states and errors such as attitude, velocity, and position in the group space, rather than in the traditional Euclidean space. According to the multiplicative closure property of Lie groups and the affine property between Lie algebras, a more compact dynamic equation of position and coordinate state errors, considering attitude errors, has been redesigned to meet the requirement that the error transfer matrix or measurement matrix remain unchanged or slowly change and to achieve the independence of F or H matrix and state estimation. Therefore, it can be unchanged, also known as invariant EKF. The error definition corresponding to the invariant EKF has good self-consistency, which effectively overcomes the defect that the traditional EKF is too dependent on the initial value, and has better convergence and consistency of filtering [19]. It is more rigorous, in theory, and has been well applied in inertial navigation fields such as robot attitude estimation, simultaneous localization and mapping (SLAM), and visual odometers (VOs) [20].To address this issue, this paper adopts the invariant EKF concept. It defines states and errors, such as attitude, velocity, and position, in group space. Based on this approach, new inertial navigation error equations and measurement update methods are deduced. The main contributions of this paper can be summarized as follows: (1) Constructing the attitude, velocity, and position states as three-dimensional special Euclidean group (SE₂(3))/EKF elements, which takes into account the zero bias errors of gyroscopes and accelerometers. This facilitates the formation of a mixed group vector error. The alignment method based on SE₂(3)/EKF filtering has demonstrated faster convergence speed, higher accuracy, and greater computational efficiency than the traditional EKF. (2) Attaining the accurate position of the vehicle during the alignment process is crucial for improving the alignment accuracy of the system. Moreover, the position alignment aids in directly transitioning the vehicle to the autonomous navigation stage after the GNSS-aided alignment. Consequently, it enhances the vehicle’s applicability, by eliminating the need to obtain the vehicle’s position again at the end of the alignment. Accurate and real-time position alignment can improve the attitude alignment accuracy of the system.

The paper follows this outline—Section 2 establishes the inertial navigation error equation under the Lie group framework. Section 3 presents the filtering model for the SINS/GNSS integrated navigation system based on SE₂(3)/EKF. The simulation experiment was conducted in Section 4. Finally, Section 5 concludes.

2. Inertial Navigation Error Equation under the Lie Group Framework

SE₂(3)/EKF does not simply use the difference between the estimated state and the real state as the error, but more carefully considers the consistency of the coordinate frame of the state definition and provides a more rigorous and compact mathematical form by redefining the error in Lie group space. In order to describe the state error, a lie group state error model including attitude, velocity, and position is designed and the gyro bias error and accelerometer bias error are still defined in the traditional vector space.

The related Cartesian reference coordinate frames used in this study are defined as follows [21]:

(1)

b-frame: Body coordinate system, with its three axes pointing to the right–front–up (R-F-U) of the carrier, respectively, denoted as $x_b{y}_b{z}_b$;

(2)

n-frame: It indicates the navigation frame and it is the frame used by the SINS to calculate the navigation parameters, denoted as Eastward–Northward–Upward (E-N-U);

(3)

e-frame: Earth coordinate system, with its origin at the geocenter. The x-axis is the intersection of the geocenter pointing to the prime meridian and the equator, the z-axis is the geocenter pointing to the north pole, and the y-axis forms a right-handed coordinate system with the x-axis and z-axis, denoted as $x_e{y}_e{z}_e$;

(4)

i-frame: Inertial coordinate system. It is a non-rotating coordinate system in inertial space, denoted as $x_i{y}_i{z}_i$.

The schematic diagram of the coordinate system mentioned above is shown in Figure 1. ${\omega }_{ie}$ is the angular velocity of the Earth’s rotation and $L$ is the latitude of the SINS.

2.1. SINS Navigation Differential and Error Equations

Selecting the E-N-U geographic coordinate system as the navigation reference coordinate system for the SINS, the attitude differential equation using the n-frame as the reference frame is as follows [22]:

$${\dot{C}}_b^n=C_b^n({\omega }_{ib}^b\times )-[({\omega }_{ie}^n+{\omega }_{en}^n)\times ]C_b^n$$

(1)

where $C_b^n$ denotes the attitude matrix of the b-frame relative to the n-frame, ${\omega }_{ib}^b$ is the angular velocity of the b-frame relative to the i-frame, ${\omega }_{ie}^n$ is the rotational angular velocity of the Earth, ${\omega }_{ie}^n={\left[\begin{array}{ccc}0& {\omega }_{ie}\cos L& {\omega }_{ie}sinL\end{array}\right]}^T$, ${\omega }_{en}^n$ is the angular velocity generated by the relative motion of the n-frame to the e-frame, ${\omega }_{en}^n={\left[\begin{array}{ccc}-\frac{v_{\mathrm{N}}}{R_M+h}& \frac{v_{\mathrm{E}}}{R_N+h}& \frac{v_{\mathrm{E}}}{R_N+h}\tan L\end{array}\right]}^T$, and $h$ is the geographical altitude. $R_M$ is the principal radius of curvature along the meridional section, $R_N$ is the principal radius of curvature along the prime-vertical normal section, $v^n$ is the velocity in the n-frame, and $v^n={\left[\begin{array}{ccc}{v}_E& v_N& v_U\end{array}\right]}^T$. $v_{\mathrm{E}}$, $v_{\mathrm{N}}$, and $v_U$ are the eastern, northern, and upward velocity, respectively. $\times$ denotes the conversion of vectors into oblique symmetric matrices.

The differential equation for attitude error can be derived as follows [22]:

$${\dot{\mathit{\varphi }}}^n={\mathit{\varphi }}^n\times {\omega }_{in}^n+\mathsf{\delta }{\omega }_{in}^n-C_b^n\mathsf{\delta }{\omega }_{ib}^b$$

(2)

where $\mathit{\varphi }$ is attitude error, ${\omega }_{in}^n={\omega }_{ie}^n+{\omega }_{en}^n$, $\mathsf{\delta }{\omega }_{in}^n$ is the calculation error, $\mathsf{\delta }{\omega }_{in}^n=\delta {\omega }_{ie}^n+\delta {\omega }_{en}^n$, $\mathsf{\delta }{\omega }_{ie}^n={\left[\begin{array}{ccc}0& -{\omega }_{ie}\sin L\cdot \mathsf{\delta }L& {\omega }_{ie}\cos L\cdot \mathsf{\delta }L\end{array}\right]}^T=M_1\mathsf{\delta }{p}^n$, $M_1=\left[\begin{array}{ccc}0& 0& 0\\ -{\omega }_{ie}\sin L& 0& 0\\ {\omega }_{ie}\cos L& 0& 0\end{array}\right]$, and $\mathsf{\delta }{\omega }_{ib}^b$ is the measurement error of the gyroscope.

$$\mathsf{\delta }{\omega }_{en}^n=\left[\begin{array}{c}\frac{-\mathsf{\delta }{v}_N}{R_M+h}+\frac{v_{\mathrm{N}}\mathsf{\delta }h}{{(R_M+h)}^2}\\ \frac{\mathsf{\delta }{v}_{\mathrm{E}}}{R_N+h}-\frac{v_{\mathrm{E}}\mathsf{\delta }h}{{(R_N+h)}^2}\\ \frac{\tan L\cdot \mathsf{\delta }{v}_{\mathrm{E}}}{R_N+h}+\frac{v_{\mathrm{E}}{\sec }^{2}L\cdot \mathsf{\delta }L}{R_N+h}-\frac{v_{\mathrm{E}}\tan L\cdot \mathsf{\delta }h}{{(R_N+h)}^2}\end{array}\right]=M_{av}\mathsf{\delta }{v}^n+M_2\mathsf{\delta }{p}^n$$

(3)

where $M_{av}=\left[\begin{array}{ccc}0& -\frac{1}{R_M+h}& 0\\ \frac{1}{R_N+h}& 0& 0\\ \frac{\tan L}{R_N+h}& 0& 0\end{array}\right]$, $M_2=\left[\begin{array}{ccc}0& 0& \frac{v_{\mathrm{N}}}{{(R_M+h)}^2}\\ 0& 0& -\frac{v_{\mathrm{E}}}{{(R_N+h)}^2}\\ \frac{v_{\mathrm{E}}{\sec }^{2}L}{R_N+h}& 0& -\frac{v_{\mathrm{E}}\tan L}{{(R_N+h)}^2}\end{array}\right]$, $p^n={\left[\begin{array}{ccc}L& \lambda & h\end{array}\right]}^T$, $\lambda$ is the longitude, $\mathsf{\delta }{p}^n={\left[\begin{array}{ccc}\mathsf{\delta }L& \mathsf{\delta }\lambda & \mathsf{\delta }h\end{array}\right]}^{\mathrm{T}}$ is the position error, and $\mathsf{\delta }L$, $\mathsf{\delta }\lambda$, and $\mathsf{\delta }h$ are the latitude error, longitude error, and height errors, respectively. $\mathsf{\delta }{v}^n$ is the velocity error in the n-frame, $\delta v^n={\left[\begin{array}{ccc}\delta v_E& \delta v_N& \delta v_U\end{array}\right]}^T$, and $\delta v_{\mathrm{E}}$, $\delta v_{\mathrm{N}}$, and $\delta v_U$ are the eastern, northern, and upward velocity errors, respectively.

The velocity differential equation defined under the local navigation system is as follows [18]:

$${\dot{v}}^n=C_b^n{f}^b-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n+g^n$$

(4)

where $f^b$ is the specific force measured using the accelerometer, $2{\omega }_{ie}^n\times v^n$ is the Coriolis acceleration caused by the motion of the carrier and the rotation of the Earth, ${\omega }_{en}^n\times v^n$ is the centripetal acceleration caused by the movement of the carrier towards the ground, $g^n$ is gravitational acceleration, and $-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times v^n+g^n$ is collectively referred to as harmful acceleration.

The corresponding differential equation for velocity error is as follows [23]:

$$\mathsf{\delta }{\dot{v}}^n=f^n\times {\mathsf{\varphi }}^n+v^n\times (2\mathsf{\delta }{\omega }_{ie}^n+\mathsf{\delta }{\omega }_{en}^n)-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times \mathsf{\delta }{v}^n+C_b^n\mathsf{\delta }{f}^b+\mathsf{\delta }{g}^n$$

(5)

where $f^n$ is the projection of the specific force vector in the n-frame, $\mathsf{\delta }{f}^b$ is the measurement error of the accelerometer, and $\mathsf{\delta }{g}^n$ is the gravitational acceleration error.

The position differential equation defined under the local navigation system is as follows [23]:

$${\dot{p}}^n={\left[\begin{array}{ccc}\dot{L}& \dot{\lambda }& \dot{h}\end{array}\right]}^T=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]v^n=M_{pv}{v}^n$$

(6)

where $M_{pv}=\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]$.

The corresponding differential equation for positional error is as follows [23]:

$$\begin{array}{ll}\delta {\dot{p}}^n& ={\left[\begin{array}{ccc}\delta \dot{L}& \delta \dot{\lambda }& \delta \dot{h}\end{array}\right]}^T\\ & =\left[\begin{array}{ccc}0& \frac{1}{R_M+h}& 0\\ \frac{\sec L}{R_N+h}& 0& 0\\ 0& 0& 1\end{array}\right]\mathsf{\delta }{v}^n+\left[\begin{array}{ccc}0& 0& \frac{-v_{\mathrm{N}}}{{(R_M+h)}^2}\\ \frac{v_{\mathrm{E}}\sec L\tan L}{R_N+h}& 0& \frac{-v_{\mathrm{E}}\sec L}{{(R_N+h)}^2}\\ 0& 0& 0\end{array}\right]\mathsf{\delta }{p}^n\\ & =M_{pv}\mathsf{\delta }{v}^n+M_{pp}\mathsf{\delta }{p}^n\end{array}$$

(7)

The error model for gyroscopes and accelerometers is as follows:

$$\{\begin{array}{c}{\widehat{\omega }}_{ib}^b={\omega }_{ib}^b+{\epsilon }^b+w_g\\ {\widehat{f}}^b=f^b+{\nabla }^b+w_a\end{array}$$

(8)

where ${\widehat{\omega }}_{ib}^b$ and ${\widehat{f}}^b$ are actual measured values of gyroscopes and accelerometers, respectively. ${\epsilon }^b$ is the random constant drift of the gyroscopes, ${\nabla }^b$ is the random constant bias of the accelerometers, and ${\dot{\epsilon }}^b=0$, ${\dot{\nabla }}^b=0$. $w_g$, and $w_a$ are the Gaussian white noises of the accelerometers and gyroscopes, respectively.

2.2. Left Invariant Error Equation of Inertial Navigation in the Lie Group Framework

In the Lie group framework, the n-frame is chosen as the projection coordinate system for the SINS/GNSS integrated system. The attitude, velocity, and position of the n-frame are defined as a group, $\chi$, [24] as follows:

$$\chi =\left[\begin{array}{ccc}{C}_b^n& v^n& p^n\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(9)

where $C_b^n\in SO(3)$, $SO(3)$ is a three-dimensional special orthogonal group, $0_{1\times 3}$ is a zero matrix with one row and three columns, and $\chi \in SE(3)$ is a more concise group that includes elements of attitude, velocity, and position.

By using the properties of Lie groups [25], we can obtain the following:

$${\chi }^{-1}=\left[\begin{array}{ccc}{C}_n^b& -v^b& -p^b\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(10)

where ${\chi }^{-1}$ denotes the inverse matrix of $\chi$ and, similarly, ${\chi }^{-1}\in SE(3)$.

If $\chi$ represents the true state and $\widehat{\chi }$ represents the nominal state, the error of the attitude, velocity, and position states is defined as $\eta \in SE(3)$. The error in Lie group space can be classified into two types—left invariance, $\eta ={\widehat{\chi }}^{-1}\chi$, and right invariance, $\eta =\chi {\widehat{\chi }}^{-1}$. Research has shown that left invariance can achieve the invariance or gradual change of the measurement matrix, while the observations of GNSS theoretically belong to the category of left invariant observations [25]; therefore, this paper focuses on the left invariant error for processing.

Based on Equations (1) and (2), the specific expression for the left invariant error is given by:

$$\eta ={\widehat{\chi }}^{-1}\chi =\left[\begin{array}{ccc}{\widehat{C}}_n^b& -{\widehat{v}}^b& -{\widehat{p}}^b\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]\left[\begin{array}{ccc}{C}_b^n& v^n& p^n\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]=\left[\begin{array}{ccc}{\widehat{C}}_n^b{C}_b^n& {\widehat{C}}_n^b{v}^n-{\widehat{v}}^b& {\widehat{C}}_n^b{p}^n-{\widehat{p}}^b\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(11)

The errors corresponding to the attitude, velocity, and position under the defined Lie group are as follows:

$${\widehat{C}}_n^b{C}_b^n=Exp({\varphi }^b)=\exp ({\varphi }^b\times )$$

(12)

$$J{\rho }_v^b={\widehat{C}}_n^b{v}^n-{\widehat{v}}^b={\widehat{C}}_n^b{v}^n-{\widehat{C}}_n^b{\widehat{v}}^n=-{\widehat{C}}_n^b\delta v^n$$

(13)

$$J{\rho }_p^b={\widehat{C}}_n^b{p}^n-{\widehat{p}}^b={\widehat{C}}_n^b{p}^n-{\widehat{C}}_n^b{\widehat{p}}^n=-{\widehat{C}}_n^b\delta p^n$$

(14)

where ${\varphi }^b$ is the attitude error angles, $Exp(\cdot )$ represents the mapping between Lie algebras and Lie groups, and $\exp (\cdot )$ represents exponentiation. $\exp ({\varphi }^b\times )=I_3+\frac{\sin \theta }{\theta }({\varphi }^b\times )+\frac{1-\cos \theta }{{\theta }^2}{({\varphi }^b\times )}^2$ and $\theta =\left|\varphi \right|$. $J$ is the Jacobian matrix of the Rodriguez formula and $J={\displaystyle \sum _{n=0}^{\infty }\frac{1}{(n+1)!}}({\varphi }^b\times )=I_3+\frac{1-\cos \theta }{{\theta }^2}({\varphi }^b\times )+\frac{\theta -\sin \theta }{{\theta }^3}{({\varphi }^b\times )}^2$. $J{\rho }_v^b$ is the new definition of velocity error and $J{\rho }_p^b$ is the new definition of positional error.

From the error form defined in Equation (11), we can observe that the velocity and position errors defined in the Lie group framework include attitude terms. This takes into account the differences in numerical magnitude and direction between true values and estimates. Therefore, the error definition is more reasonable and concise compared to the traditional vector space method of differencing, which solves the problem of inconsistent benchmarks for defining velocity error states.

According to the characteristics of Lie groups and Lie algebras, the left invariant error satisfies the following [26,27]:

$$\eta =\left[\begin{array}{ccc}\exp ({\varphi }^n\times )& J{\rho }_v^b& J{\rho }_p^b\\ 0_{1\times 3}& 1& 0\\ 0_{1\times 3}& 0& 1\end{array}\right]$$

(15)

The new attitude error equation is derived as follows:

$$\begin{array}{ll}\frac{d}{dt}\left({\widehat{C}}_n^b{C}_b^n\right)& ={\dot{\widehat{C}}}_n^b{C}_b^n+{\widehat{C}}_n^b{\dot{C}}_b^n\\ & =\left[{\widehat{C}}_n^b({\widehat{\omega }}_{in}^n\times )-({\widehat{\omega }}_{ib}^b\times ){\widehat{C}}_n^b\right]C_b^n+{\widehat{C}}_n^b\left[C_b^n({\omega }_{ib}^b\times )-({\omega }_{in}^n\times )C_b^n\right]\\ & ={\widehat{C}}_n^b({\widehat{\omega }}_{in}^n\times )C_b^n-({\widehat{\omega }}_{ib}^b\times ){\widehat{C}}_n^b{C}_b^n+{\widehat{C}}_n^b{C}_b^n({\omega }_{ib}^b\times )-{\widehat{C}}_n^b({\omega }_{in}^n\times )C_b^n\\ & \approx {\widehat{C}}_n^b\left[({\widehat{\omega }}_{in}^n-{\omega }_{in}^n)\times \right]C_b^n-\left[({\omega }_{ib}^b+\delta {\omega }_{ib}^b)\times \right]\left[I_3+({\varphi }^b\times )\right]+\left[I_3+({\varphi }^b\times )\right]({\omega }_{ib}^b\times )\\ & \approx \delta {\omega }_{in}^n\times \left[I_3+({\varphi }^b\times )\right]+({\varphi }^b\times {\omega }_{ib}^b)\times -\delta {\omega }_{ib}^b\times \\ & \approx \delta {\omega }_{in}^n\times +({\varphi }^b\times {\omega }_{ib}^b)\times -\delta {\omega }_{ib}^b\times \end{array}$$

(16)

where ${\widehat{C}}_n^b{C}_b^n\approx I_3+({\varphi }^b\times )$, $\delta {\omega }_{in}^n={\widehat{\omega }}_{in}^n-{\omega }_{in}^n$, $\delta {\omega }_{ib}^b={\widehat{\omega }}_{ib}^b-{\omega }_{ib}^b$, and the second-order small quantities $(\delta {\omega }_{in}^n\times )({\varphi }^b\times )$ and $(\delta {\omega }_{ib}^b\times )({\varphi }^b\times )$ are neglected in the process of formula derivation.

The new velocity error equation is derived as follows:

$$\begin{array}{ll}\frac{d}{dt}\left(J{\rho }_v^b\right)& =-{\dot{\widehat{C}}}_n^b\delta v^n-{\widehat{C}}_n^b\delta {\dot{v}}^n\\ & =\left[{\widehat{C}}_n^b({\widehat{\omega }}_{in}^n\times )-({\widehat{\omega }}_{ib}^b\times ){\widehat{C}}_n^b\right]\delta v^n\\ & -{\widehat{C}}_n^b\left[f^n\times {\varphi }^n+v^n\times (2\mathsf{\delta }{\omega }_{ie}^n+\mathsf{\delta }{\omega }_{en}^n)-(2{\omega }_{ie}^n+{\omega }_{en}^n)\times \mathsf{\delta }{v}^n+C_b^n\mathsf{\delta }{f}^b+\mathsf{\delta }{g}^n\right]\\ & \approx -({\widehat{\omega }}_{ib}^b\times )J{\rho }_v^b-\delta f^b+{\varphi }^b\times f^b-C_n^b\mathsf{\delta }{g}^n+(C_n^b{\omega }_{ie}^n)\times J{\rho }_v^b-C_n^b(v^n\times )(\mathsf{\delta }{\omega }_{ie}^n+\mathsf{\delta }{\omega }_{in}^n)\end{array}$$

(17)

where $\delta f^b={\widehat{f}}^b-f^b$, $\mathsf{\delta }{\omega }_{ie}^n={\widehat{\omega }}_{ie}^n-{\omega }_{ie}^n$, and $\mathsf{\delta }{g}^n={\widehat{g}}^n-g^n$.

The new position error equation is derived as follows:

$$\begin{array}{ll}\frac{d}{dt}\left(J{\rho }_p^b\right)& =-{\dot{\widehat{C}}}_n^b\delta p^n-{\widehat{C}}_n^b\delta {\dot{p}}^n\\ & =-\left[{\widehat{C}}_n^b({\widehat{\omega }}_{in}^n\times )-({\widehat{\omega }}_{ib}^b\times ){\widehat{C}}_n^b\right]\delta p^n-{\widehat{C}}_n^b(M_{pv}\mathsf{\delta }{v}^n+M_{pp}\mathsf{\delta }{p}^n)\\ & =({\widehat{\omega }}_{ib}^b\times ){\widehat{C}}_n^b\delta p^n-{\widehat{C}}_n^b({\widehat{\omega }}_{in}^n\times )\delta p^n-{\widehat{C}}_n^b{M}_{pv}\mathsf{\delta }{v}^n-{\widehat{C}}_n^b{M}_{pp}\mathsf{\delta }{p}^n\\ & \approx -({\widehat{\omega }}_{ib}^b\times )J{\rho }_p^b+J{\rho }_v^b-C_n^b(p^n\times )\delta {\omega }_{en}^n+(C_n^b{\omega }_{ie}^n)\times J{\rho }_p^b\end{array}$$

(18)

By substituting Equations (13) and (14) into $\delta {\omega }_{ie}^n$ and $\delta {\omega }_{in}^n$, we can obtain the following equations:

$$\delta {\omega }_{ie}^n=M_1\mathsf{\delta }{p}^n=-M_1{\widehat{C}}_b^{n}J{\rho }_p^b$$

(19)

$$\mathsf{\delta }{\omega }_{in}^n=(M_1+M_2)\delta p^n+M_{av}\mathsf{\delta }{v}^n=-(M_1+M_2){\widehat{C}}_b^{n}J{\rho }_p^b-M_{av}{\widehat{C}}_b^{n}J{\rho }_v^b$$

(20)

Finally, the error equations of SINS can be written as follows:

$$\frac{d}{dt}\left({\widehat{C}}_n^b{C}_b^n\right)=-C_n^b(M_1+M_2){\widehat{C}}_b^{n}J{\rho }_p^b-C_n^b{M}_{av}{\widehat{C}}_b^{n}J{\rho }_v^b-{\omega }_{ib}^b\times {\varphi }^b-({\epsilon }^b+w_g)$$

(21)

$$\begin{array}{ll}\frac{d}{dt}\left(J{\rho }_v^b\right)& =C_n^b(v^n\times )(2M_1+M_2)C_b^{n}J{\rho }_p^b+\left[C_n^b(v^n\times )M_{av}{C}_b^n-({\omega }_{ib}^b\times )-(C_n^b{\omega }_{ie}^n)\times \right]J{\rho }_v^b\\ & -f^b\times {\varphi }^b-C_n^b\delta g^n-({\nabla }^b+w_a)\end{array}$$

(22)

$$\frac{d}{dt}\left(J{\rho }_p^b\right)=\left[C_n^b(p^n\times )M_2{C}_b^n-({\omega }_{ib}^b\times )+(C_n^b{\omega }_{ie}^n)\times \right]J{\rho }_p^b+\left[I_3+C_n^b(p^n\times )M_{av}{C}_b^n\right]J{\rho }_v^b$$

(23)

3. Filtering Model Based on SE₂(3)/EKF

3.1. State Equation Based on SE₂(3)

If the state vector is X and the system noise vector is W, then the state equation based on SE₂(3) is as follows [28]:

$$\dot{X}=FX+GW$$

(24)

where $F$ is the state transition matrix, $G$ is the system noise allocation matrix, and $E[W{W}^T]=Q$.

$$X={\left[\begin{array}{lllll}{({\varphi }^b)}^T\hfill & {(J{\rho }_v^b)}^T\hfill & {(J{\rho }_p^b)}^T\hfill & {({\epsilon }^b)}^T\hfill & {({\nabla }^b)}^T\hfill \end{array}\right]}^T$$

(25)

$$F=\left[\begin{array}{lllll}-{\widehat{\omega }}_{ib}^b\times \hfill & -{\widehat{C}}_n^b{M}_{av}{\widehat{C}}_b^n\hfill & {\widehat{C}}_n^b(M_1+M_2)\hfill & -I_3\hfill & 0_3\hfill \\ -{\widehat{f}}^b\times \hfill & {\widehat{C}}_n^b({\widehat{v}}^n\times )M_{av}{\widehat{C}}_b^n-{\widehat{\omega }}_{ib}^b\times -{\widehat{C}}_n^b({\widehat{\omega }}_{ie}^n\times ){\widehat{C}}_b^n\hfill & -{\widehat{C}}_n^b({\widehat{v}}^n\times )(2M_1+M_2)\hfill & 0_3\hfill & -I_3\hfill \\ 0_3\hfill & -M_{pv}{\widehat{C}}_b^n\hfill & M_{pv}\hfill & 0_3\hfill & 0_3\hfill \\ 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill \\ 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill \end{array}\right]$$

(26)

$$G=\left[\begin{array}{ll}-I_3\hfill & 0_3\hfill \\ 0_3\hfill & -I_3\hfill \\ 0_3\hfill & 0_3\hfill \\ 0_3\hfill & 0_3\hfill \\ 0_3\hfill & 0_3\hfill \end{array}\right]$$

(27)

$$W=\left[\begin{array}{c}{w}_g\\ w_a\end{array}\right]$$

(28)

where $I_3$ is a $3\times 3$ unit vector and $0_3$ is a $3\times 3$ zero matrix.

3.2. Measurement Equation Based on SE₂(3)

The GNSS can provide position and velocity information in integrated navigation as measurements for Kalman filtering, to suppress the divergence of inertial navigation calculation results. Therefore, the measurement equation based on the SE₂(3) can be written as follows [29]:

$$Z=\left[\begin{array}{c}{v}_{SINS}^n-v_{GNSS}^n\\ p_{SINS}^n-p_{GNSS}^n\end{array}\right]=HX+V$$

(29)

where $v_{SINS}^n$ and $v_{GNSS}^n$ are the velocity of the SINS and the GNSS in the n-frame, respectively. $p_{SINS}^n$ and $p_{GNSS}^n$ are the position of the SINS and the GNSS in the n-frame, respectively. $H$ is the measurement matrix and $V$ is the white noise for velocity measurement and position measurement of satellite receivers.

$$H=\left[\begin{array}{llllll}{\widehat{v}}^n\hfill & -I_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill \\ 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill & 0_3\hfill \end{array}\right]$$

(30)

$$V=\left[\begin{array}{c}{V}_v\\ V_p\end{array}\right]$$

(31)

where $V_v$ and $V_p$ are the white noise for satellite receiver velocity measurement and position measurement, respectively.

3.3. SE₂(3)/EKF Algorithm

Assuming the sampling interval of IMU is $\Delta t$ and the discretized state transition matrix is $\Phi$, then the SE₂(3)/EKF algorithm is as follows [30]:

(1)

One-step state prediction

$${\widehat{X}}_{k,k-1}={\Phi }_{k,k-1}{\widehat{X}}_{k-1}$$

(32)

(2)

State estimation

$${\widehat{X}}_k={\widehat{X}}_{k,k-1}+\delta {\widehat{X}}_k$$

(33)

(3)

State estimation error

$$\delta {\widehat{X}}_k=K_k(Z_k-H_k{\widehat{X}}_{k,k-1})$$

(34)

(4)

Filter gain

$$K_k=P_{k,k-1}{H}_k^T{[H_k{P}_{k,k-1}{H}_k^T+R_k]}^{-1}$$

(35)

(5)

One-step prediction mean square error

$$P_{k,k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^T+Q_{k-1}$$

(36)

(6)

Estimating mean square error

$$P_k=(I-K_k{H}_k)P_{k,k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$$

(37)

where ${\widehat{X}}_{k,k-1}$ is the state one-step prediction matrix, ${\Phi }_{k,k-1}$ is the one-step transition matrix from time $t_{k-1}$ to time $t_k$, and ${\Phi }_{k,k-1}=I+F\Delta t$. ${\widehat{X}}_{k-1}$ is the state estimation matrix at time $t_{k-1}$, $\delta {\widehat{X}}_k$ is the state estimation error matrix at time $t_k$, $K_k$ is the filtering gain matrix at time $t_k$, $Z_k$ is the measurement matrix at time $t_k$, $P_{k,k-1}$ is the one step prediction mean square error matrix, $R_k$ is the measurement noise variance matrix, $Q_{k-1}$ is the system noise variance matrix at time $t_{k-1}$, $Q_{k-1}=G_{k-1}(q_{k-1}\Delta t)G_{k-1}^T$, $G_{k-1}$ is the discretized system noise allocation matrix at time $t_{k-1}$, and $q$ is the variance intensity matrix corresponding to the system noise matrix $W$. $P_k$ is the estimated mean square error matrix.

4. Simulation Results

This section presents the simulation experiments conducted to verify the validity and feasibility of the proposed method. The specific parameters of the inertial sensor used in the simulation experiments are as follows: the constant drift and random walk of the gyroscopes are represented by values ${0.03}^{\circ }/\mathrm{h}$ and ${0.001}^{\circ }/\sqrt{\mathrm{h}}$, respectively. The constant bias and random bias of the accelerometers are represented by values $100 \mu \mathrm{g}$ and $10 \mu \mathrm{g}$, respectively. The data output frequency is $200 \mathrm{H}\mathrm{z}$ and the calculation period of SINS is $5 \mathrm{m}\mathrm{s}$. The performance parameters of the GNSS are as follows: the velocity measurement accuracy is represented by a value of $0.1 \mathrm{m}/\mathrm{s}$, the position measurement accuracy is represented by a value of $1 \mathrm{m}$, and the data output frequency is represented by a value of $1 \mathrm{H}\mathrm{z}$. The initial position is set as follows: ${118.786365}^{\circ } \mathrm{E}$, ${32.057313}^{\circ } \mathrm{N}$, and the height is $0 \mathrm{m}$. The initial velocity is represented by a value of ${[\begin{array}{ccc}0& 0& 0\end{array}]}^T \mathrm{m}/\mathrm{s}$. The simulation time is 900 s. The state and trajectory of the vehicle are illustrated in Figure 2 and Figure 3, respectively.

To fully demonstrate the performance of the proposed algorithm, contrast experiments of SE₂(3)/EKF and the EKF algorithm, as well as position alignment experiments, will be designed. These experiments will compare the traditional EKF with the SE₂(3)/EKF algorithm proposed in this paper, using three different initial misalignment angle scenarios. The performance of various algorithms will be evaluated to verify the advantages of the SE₂(3)/EKF algorithm.

4.1. Experiment 1

In this experiment, the misalignment angles in the three directions are sequentially set ${[\begin{array}{ccc}{1}^{\circ }& -1^{\circ }& {40}^{\circ }\end{array}]}^T$. The mean and variance of the attitude error and position error are recorded in

Table 1. Mean and variance of attitude error and position error (${[\begin{array}{ccc}{1}^{\circ }& -1^{\circ }& {40}^{\circ }\end{array}]}^T$).

Algorithm	Attitude and Position Error	Mean	Variance
SE2(3)/EKF	Pitch Error	−0.7871″	0.1214
	Roll Error	1.2537″	0.0926
	Heading Error	0.0595′	0.0087
	Latitude Error	0.4381 m	0.0412
	Longitude Error	−0.2522 m	0.0201
	Height Error	−0.3418 m	0.0603
EKF	Pitch Error	0.9947″	0.1216
	Roll Error	1.9856″	0.0945
	Heading Error	−0.3037′	0.0107
	Latitude Error	0.4882 m	0.0415
	Longitude Error	−0.3065 m	0.0203
	Height Error	−0.5243 m	0.0616

for the 50 s leading up to the end of alignment. The attitude angle error are as shown in Figure 4, Figure 5 and Figure 6, and the position alignment error are as shown in Figure 7, Figure 8 and Figure 9.

Based on the comprehensive information from the chart, both methods can converge the attitude angles to a relatively stable range in this scenario. The figure shows that the time taken to converge the horizontal attitude angles estimated by the two methods is not significantly different; both methods converge quickly. However, the convergence time for the heading angles is longer. The data provided in Table 1 indicate that the attitude angle accuracy of the two methods after alignment is relatively close. Overall, under the misalignment angles set in Experiment 1, both methods perform equally. This is because the initial misalignment angles are small at this time and the nonlinearity of the traditional EKF model is not severe. Additionally, defining the velocity error as the direct difference between the true value and the estimated value is considered reasonable, allowing both alignment methods to converge quickly.

4.2. Experiment 2

In this experiment, the misalignment angles in the three directions are sequentially set ${[\begin{array}{ccc}{1}^{\circ }& -1^{\circ }& {95}^{\circ }\end{array}]}^T$. The mean and variance of the attitude error and position error are recorded in

Table 2. Mean and variance of attitude error and position error (${[\begin{array}{ccc}{1}^{\circ }& -1^{\circ }& {95}^{\circ }\end{array}]}^T$).

Algorithm	Attitude and Position Error	Mean	Variance
SE2(3)/EKF	Pitch Error	11.0881″	0.2950
	Roll Error	−7.7115″	0.1303
	Heading Error	−1.0848′	0.0259
	Latitude Error	0.2507 m	0.1201
	Longitude Error	0.8113 m	0.0552
	Height Error	1.6058 m	0.1191
EKF	Pitch Error	57.2824″	0.3147
	Roll Error	−17.7575″	0.1366
	Heading Error	−2.5052′	0.0445
	Latitude Error	0.5569 m	0.1238
	Longitude Error	2.3491 m	0.0612
	Height Error	3.1618 m	0.1307

for the 50 s leading up to the end of alignment. The attitude angle error are as shown in Figure 10, Figure 11 and Figure 12, and the position alignment error are as shown in Figure 13, Figure 14 and Figure 15.

From the figures, it can be observed that, due to further increasing the initial heading angle error, the nonlinearity of the traditional model is enhanced. At this point, the EKF method still uses a linear error differential equation error model, disregarding the influence of high-order system terms. As a result, significant model errors occur, leading to a slower convergence speed in attitude compared to the SE₂(3)/EKF method. According to Table 2, the convergence accuracy of the EKF method is lower than that of SE₂(3)/EKF. The SE₂(3)/EKF method redefines errors more rigorously within the Lie group framework, resulting in a better performance for larger misalignment angles.

4.3. Experiment 3

In this experiment, the misalignment angles in the three directions are sequentially set ${[\begin{array}{ccc}{20}^{\circ }& {20}^{\circ }& {165}^{\circ }\end{array}]}^T$. The mean and variance of the attitude error and position error are recorded in

Table 3. Mean and variance of attitude error and position error (${[\begin{array}{ccc}{20}^{\circ }& {20}^{\circ }& {165}^{\circ }\end{array}]}^T$).

Algorithm	Attitude and Position Error	Mean	Variance
SE2(3)/EKF	Pitch Error	$13.641$″	0.4387
	Roll Error	$17.0$″	1.2976
	Heading Error	$4.5116$′	0.2122
	Latitude Error	−1.8307 m	0.0204
	Longitude Error	2.2376 m	0.1855
	Height Error	4.1076 m	0.1372
EKF	Pitch Error	$5.4181\times {10}^2$″	0.4905
	Roll Error	$-2.5211\times {10}^2$″	2.2026
	Heading Error	$-38.9656\times {10}^2$′	0.1851
	Latitude Error	5.3826 m	0.1123
	Longitude Error	22.3550 m	0.6989
	Height Error	16.1874 m	0.1525

for the 50 s leading up to the end of alignment. The attitude angle error are as shown in Figure 16, Figure 17 and Figure 18, and the position alignment error are as shown in Figure 19, Figure 20 and Figure 21.

The figure shows that, when the misalignment angles in three directions reach a severe level, the nonlinearity of the model becomes extremely severe. Additionally, defining the velocity error as a direct subtraction becomes even more unreasonable. Consequently, the traditional EKF algorithm is no longer able to converge, while the SE₂(3)/EKF method still converges relatively quickly. According to Table 3, it is evident that, in this scenario, the attitude angle error calculated by SE₂(3)/EKF can still maintain a level close to Experiment 1 and Experiment 2, further demonstrating the stable characteristics of the SE₂(3)/EKF alignment method.

5. Conclusions

This paper proposed a novel initial alignment method for the SINS/GNSS integrated navigation system with large misalignment angles based on SE₂(3)/EKF. The left invariant SE₂(3)/EKF adopts a group-vector mixed error model, which allows the linear state error based on Lie algebra to effectively capture the nonlinear error on the Lie group. This compatibility with the linear assumption of EKF filtering results in a higher precision dynamic model, improved measurement update accuracy, and better performance in dynamic initial alignment, especially when considering attitude error. Simulation experiments demonstrated that the EKF method can achieve attitude convergence in scenarios with small initial misalignment angles. On the other hand, the alignment method based on SE₂(3)/EKF can converge to higher accuracy in the different large misalignment angle scenarios simulated in this paper, quickly reducing attitude errors to a lower level.

In future research, we focus on studying a more reasonable difference resistance adaptive filtering method within the framework of SE₂(3). This will enable us to adapt to complex GNSS observation environments or high dynamic scenes of carriers, expanding the application scenario of this algorithm. Additionally, we aim to conduct real-time vehicle experiments to validate the proposed methodology for practical use.