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

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


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 Sensors 2024, 24, 2945 2 of 20 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 threedimensional special Euclidean group (SE 2 (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 2 (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 2 (3)/EKF.The simulation experiment was conducted in Section 4. Finally, Section 5 concludes.

Inertial Navigation Error Equation under the Lie Group Framework
SE 2 (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.ω ie is the angular velocity of the Earth's rotation and L is the latitude of the SINS.
Sensors 2024, 24, x FOR PEER REVIEW 3 of 21 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 SE2(3)/EKF.The simulation experiment was conducted in Section 4. Finally, Section 5 concludes.

Inertial Navigation Error Equation under the Lie Group Framework
SE2(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 b b b x y z ; (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 zaxis 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 e e e x y z ; (4) i-frame: Inertial coordinate system.It is a non-rotating coordinate system in inertial space, denoted as i i i x y z .
The schematic diagram of the coordinate system mentioned above is shown in Figure 1.ie ω is the angular velocity of the Earth's rotation and L is the latitude of the SINS.

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]: Sensors 2024, 24, 2945 4 of 20 where C n b denotes the attitude matrix of the b-frame relative to the n-frame, ω b ib is the angular velocity of the b-frame relative to the i-frame, ω n ie is the rotational angular velocity of the Earth, ω n ie = 0 ω ie cos L ω ie sinL T , ω n en is the angular velocity generated by the relative motion of the n-frame to the e-frame, , 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 and v U are the eastern, northern, and upward velocity, respectively.× denotes the conversion of vectors into oblique symmetric matrices.
The differential equation for attitude error can be derived as follows [22]: .
where ϕ is attitude error, ib is the measurement error of the gyroscope. where T is the position error, and δL, δλ, and δh are the latitude error, longitude error, and height errors, respectively.δv n is the velocity error in the n-frame, δv n = δv E δv N δv U T , and δv E , δv N , and δ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]: where f b is the specific force measured using the accelerometer, 2ω n ie × v n is the Coriolis acceleration caused by the motion of the carrier and the rotation of the Earth, ω n en × v n is the centripetal acceleration caused by the movement of the carrier towards the ground, g n is gravitational acceleration, and −(2ω n ie + ω n en ) × v n + g n is collectively referred to as harmful acceleration.
The corresponding differential equation for velocity error is as follows [23]: where f n is the projection of the specific force vector in the n-frame, δf b is the measurement error of the accelerometer, and δg n is the gravitational acceleration error.The position differential equation defined under the local navigation system is as follows [23]: Sensors 2024, 24, 2945 5 of 20 where The corresponding differential equation for positional error is as follows [23]: The error model for gyroscopes and accelerometers is as follows: where = 0. w g , and w a are the Gaussian white noises of the accelerometers and gyroscopes, respectively.

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, χ, [24] as follows: where C n b ∈ SO(3), SO(3) is a three-dimensional special orthogonal group, 0 1×3 is a zero matrix with one row and three columns, and χ ∈ 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: where χ −1 denotes the inverse matrix of χ and, similarly, χ −1 ∈ SE(3).
If χ represents the true state and χ represents the nominal state, the error of the attitude, velocity, and position states is defined as η ∈ SE(3).The error in Lie group space can be classified into two types-left invariance, η = 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: The errors corresponding to the attitude, velocity, and position under the defined Lie group are as follows: where ϕ b is the attitude error angles, Exp(•) represents the mapping between Lie algebras and Lie groups, and exp(•) represents exponentiation.exp(ϕ b ×) 2 and θ = |ϕ|.J is the Jacobian matrix of the Rodriguez formula and is the new definition of velocity error and Jρ b p 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]: The new attitude error equation is derived as follows: where , and the second-order small quantities (δω n in ×)(ϕ b ×) and (δω b ib ×)(ϕ b ×) are neglected in the process of formula derivation.
The new velocity error equation is derived as follows: where By substituting Equations ( 13) and ( 14) into δω n ie and δω n in , we can obtain the following equations: Finally, the error equations of SINS can be written as follows:

State Equation Based on SE 2 (3)
If the state vector is X and the system noise vector is W, then the state equation based on SE 2 (3) is as follows [28]: where F is the state transition matrix, G is the system noise allocation matrix, and Sensors 2024, 24, 2945 8 of 20 where I 3 is a 3 × 3 unit vector and 0 3 is a 3 × 3 zero matrix.

Measurement Equation Based on SE 2 (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 2 (3) can be written as follows [29]: where v n SI NS and v n GNSS are the velocity of the SINS and the GNSS in the n-frame, respectively.p n SI NS and p n GNSS 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.
where V v and V p are the white noise for satellite receiver velocity measurement and position measurement, respectively.

SE 2 (3)/EKF Algorithm
Assuming the sampling interval of IMU is ∆t and the discretized state transition matrix is Φ, then the SE 2 (3)/EKF algorithm is as follows [30]: (1) One-step state prediction (3) State estimation error Sensors 2024, 24, 2945 9 of 20 (4) Filter gain (5) One-step prediction mean square error (6) Estimating mean square error where Xk,k−1 is the state one-step prediction matrix, Φ k,k−1 is the one-step transition matrix from time t k−1 to time t k , and Φ k,k−1 = I + F∆t.Xk−1 is the state estimation matrix at time t k−1 , δ Xk 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 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.

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 • /h and 0.001 • / √ h, respectively.The constant bias and random bias of the accelerometers are represented by values 100 µg and 10 µg, respectively.The data output frequency is 200 Hz and the calculation period of SINS is 5 ms.The performance parameters of the GNSS are as follows: the velocity measurement accuracy is represented by a value of 0.1 m/s, the position measurement accuracy is represented by a value of 1 m, and the data output frequency is represented by a value of 1 Hz.The initial position is set as follows: 118.786365 • E, 32.057313 • N, and the height is 0 m.The initial velocity is represented by a value of [0 0 0] T m/s.The simulation time is 900 s.
The state and trajectory of the vehicle are illustrated in Figures 2 and 3, respectively.

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 / h To fully demonstrate the performance of the proposed algorithm, contrast experiments of SE2(3)/EKF and the EKF algorithm, as well as position alignment experiments, will be designed.These experiments will compare the traditional EKF with the SE2(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 SE2(3)/EKF algorithm.To fully demonstrate the performance of the proposed algorithm, contrast experiments of SE 2 (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 2 (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 2 (3)/EKF algorithm.

Experiment 1
In this experiment, the misalignment angles in the three directions are sequentially set T .The mean and variance of the attitude error and position error are recorded in Table 1 for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures 4-6, and the position alignment error are as shown in Figures 7-9.In this experiment, the misalignment angles in the three directions are sequentially set [1 . The mean and variance of the attitude error and position error are recorded in Table 1 for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures 4-6, and the position alignment error are as shown in Figures 7-9. ). ).In this experiment, the misalignment angles in the three directions are sequentially set [1 . The mean and variance of the attitude error and position error are recorded in Table 1 for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures 4-6, and the position alignment error are as shown in Figures 7-9. ). ).   ). ). ).  ). ). ).  ).

Table 1. Mean and variance of attitude error and position error (
).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.

Experiment 2
In this experiment, the misalignment angles in the three directions are sequentially set [1   ).  ). ).  ). ).   ). ). ).  ). ). ).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 2 (3)/EKF method.According to Table 2, the convergence accuracy of the EKF method is lower than that of SE 2 (3)/EKF.The SE 2 (3)/EKF method redefines errors more rigorously within the Lie group framework, resulting in a better performance for larger misalignment angles.

Experiment 3
In this experiment, the misalignment angles in the three directions are sequentially set [20    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 SE2(3)/EKF method.According to Table 2, the convergence accuracy of the EKF method is lower than that of SE2(3)/EKF.The SE2(3)/EKF method redefines errors more rigorously within the Lie group framework, resulting in a better performance for larger misalignment angles.

Experiment 3
In this experiment, the misalignment angles in the three directions are sequentially set  ).  ). ). ).  ). ). ).   ). ).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 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 2 (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 2 (3)/EKF can still maintain a level close to Experiment 1 and Experiment 2, further demonstrating the stable characteristics of the SE 2 (3)/EKF alignment method.

Conclusions
This paper proposed a novel initial alignment method for the SINS/GNSS integrated navigation system with large misalignment angles based on SE 2 (3)/EKF.The left invariant SE 2 (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 2 (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 2 (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.

Figure 1 .
Figure 1.Schematic diagram of the coordinate system.Figure 1.Schematic diagram of the coordinate system.

Figure 1 .
Figure 1.Schematic diagram of the coordinate system.Figure 1.Schematic diagram of the coordinate system.
ωb ib and fb are actual measured values of gyroscopes and accelerometers, respectively.ε b is the random constant drift of the gyroscopes, ∇ b is the random constant bias of the accelerometers, and.
and δg n = ĝn − g n .The new position error equation is derived as follows:


and 0.001 / h  , respectively.The constant bias and random bias of the accelerometers are represented by values 100 g  and 10 g  , respectively.The data output frequency is 200 Hz and the calculation period of SINS is 5 ms .The performance parameters of the GNSS are as follows: the velocity measurement accuracy is represented by a value of 0.1 / m s , the position measurement accuracy is represented by a value of 1 m , and the data output frequency is represented by a value of 1 Hz .The initial position is set as follows: 118.786365E  , 32.057313 N  , and the height is 0 m .The initial velocity is represented by a value of [0 0 0] / T m s .The simulation time is 900 s.The state and trajectory of the vehicle are illustrated in Figure 2 and Figure 3, respectively.

Figure 2 .
Figure 2. The dynamic characteristics of the vehicle during the experiment.

Figure 2 .
Figure 2. The dynamic characteristics of the vehicle during the experiment.

Figure 2 .
Figure 2. The dynamic characteristics of the vehicle during the experiment.

Figure 3 .Figure 3 .
Figure 3.The trajectory of the vehicle during the experiment.

.
The mean and variance of the attitude error and position error are recorded in Table3for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures16-18, and the position alignment error are as shown in Figures 19-21.
• −1 • 95 • ] T .The mean and variance of the attitude error and position error are recorded in Table 2 for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures 10-12, and the position alignment error are as shown in Figures 13-15.
20 • 165 • ] T .The mean and variance of the attitude error and position error are recorded in Table 3 for the 50 s leading up to the end of alignment.The attitude angle error are as shown in Figures 16-18, and the position alignment error are as shown in Figures 19-21.

Table 3 .
Mean and variance of attitude error and position error ( [20 20 165 ] T