Abstract
A new two-stage alignment on a moving base is proposed for a low-cost inertial navigation system with the aid of a Global Positioning System (GPS) receiver in the Earth-centered, Earth-fixed (ECEF) frame under large initial attitude errors. Both the state and measurement equations are derived using the additive quaternion error model in the ECEF frame for alignment without the need for level adjustment. The nonlinear velocity error equations in both the coarse and the fine stages are linearized further. An improved strong tracking filter is proposed for the coarse stage to converge quickly with large initial attitude errors. After the attitude errors converge to smaller values in the coarse stage, a standard Kalman filter is implemented for the fine stage. Experiments demonstrate that attitude error can converge quickly with the proposed adaptive alignment method.
1 INTRODUCTION
Although low-cost inertial measurement unit (IMU) sensors, such as micro-electromechanical systems (MEMS), accelerometers, and gyroscopes, have improved significantly in accuracy, an inertial navigation system (INS) composed of low-cost IMU sensors is still difficult to align accurately with stationary self-alignment (Li & Wang, 2012; Syed et al., 2008). However, if it is aided by global navigation satellite system (GNSS) measurements on a moving base, it may be aligned accurately and quickly (Kaygisiz & Şen, 2014; Tsukerman & Klein, 2018; Wu & Pan, 2013; Zhong et al., 2018).
An in-motion alignment algorithm based on optimization called the Optimization-Based Alignment (OBA) that converges very quickly in the coarse stage has been put forward to estimate the three attitude angles, IMU sensor biases, and even the GNSS antenna’s level arm recursively (Chang et al., 2016; Chen et al., 2021; Wu et al., 2014; Wu & Pan, 2013). Unfortunately, neither the states nor their covariances of the Kalman filter (KF) in the fine stage can be initialized by the OBA directly. Instead, if the alignment in the coarse stage is implemented by a KF, the KF in the fine stage can be conveniently initialized by the converged results in the coarse stage. In the coarse stage, the main challenge faced by the KF is the nonlinear state equations, which have been studied intensively (Kaygisiz & Şen, 2014; Li & Wang, 2012; Tsukerman & Klein, 2018; Zhong et al., 2018).
If a stationary level adjustment is accomplished before the initial alignment in the local level frame, both the pitch and the rolling errors would be small, but the yaw error would still be very large for the low-cost INS. That is, in the local level frame, the in-motion alignment in the coarse stage would still suffer the severe nonlinearity in the filtering model (Syed et al., 2008). Unscented Kalman filters (UKFs) and other nonlinear filters have been proposed for coarse alignment (Dmitriyev et al., 1997; Kong et al., 1999; Shin & El-Sheimy, 2004), but they exhaust more computational loads than a linear KF.
The state equations in the wander yaw frame will be linear if the cosine and sine values of the yaw error, rather than the yaw error itself, are included in the states (Han & Wang, 2010; Pham, 1991; Qi & Pengfei, 2015; Rogers, 1997). Unfortunately, both the states and the state equations in the fine stage are different from those with the sine and cosine terms in the coarse stage, so the filtering results in the coarse stage cannot be employed to initialize the filter in the fine stage directly. Alternatively, if the state equations in the two stages are derived by the quaternion method, the states in the fine stage will be the same as those in the coarse stage. In fact, the state equations without the assumption of small attitude errors have been derived using the linear Additive Quaternion Error (AQE) model (Yu et al., 1997, 1999). However, the velocity error equations derived using the AQE model are nonlinear for the large yaw error even though the level adjustment has been accomplished before the coarse stage. Hence, they will have to be linearized for the linear KF, but the linearization error in the coarse stage will be very large, which is not beneficial in improving alignment performance.
The linearized velocity error equations using the AQE model have been elaborated to further improve on alignment performance (Wang et al., 2020). Moreover, if the fading KF is used to improve the contribution of the latest measurements in the state updating (Geng & Wang, 2008; Simon, 2006), the impact of the linearized error on alignment can be mitigated significantly. Considering that the fading factor is usually preset as a constant, a strong tracking filter (STF) has been employed to adjust the fading factor adaptively by the updated residual (Zhong et al., 2018). Consequently, all the states are attenuated by the fading factor equally in both the fading KF and the STF. However, by using the AQE model, both the attitude error and the position error equations are linear, while only the velocity error equations are nonlinear. Hence, it is more reasonable to allocate the states with different fading factors to address the difference among the state equations, which has yet to be investigated until now.
If the INS navigation is implemented in the Earth-centered, Earth-fixed (ECEF) frame, rather than in the local level frame, more than 30% of the computational load can be reduced, which is attractive to low-cost applications because of the limited online computation (Wei & Schwarz, 1990). Correspondingly, the INS should be initially aligned in the ECEF frame, too. Actually, linearized alignment in the ECEF frame has been proposed, but it is derived by assuming small attitude errors (Xie et al., 2014). Furthermore, the level adjustment in the local level frame will lose its effectiveness for alignment in the ECEF frame. That is, alignment in the ECEF frame should be implemented with the condition of large attitude errors rather than only one large yaw error, which has not been studied either.
In this paper, a new two-stage alignment on a moving base is proposed for a low-cost MEMS INS with the aid of a Global Positioning System (GPS) receiver in the ECEF frame under large initial attitude errors. Both the state and measurement equations are derived in the ECEF frame for alignment without the need for a level adjustment. The nonlinear velocity error equations are linearized in both the coarse and fine stages further. An improved STF algorithm is proposed for the coarse stage to converge quickly with large attitude errors. After the attitude errors converge to the small values in the coarse stage, a standard KF is implemented in the fine stage.
The rest of this paper is organized as follows. The AQE models in the ECEF frame are provided for the alignment on a moving base in Section 2. The velocity error equations are linearized for the following filtering. An improved STF is proposed for the coarse stage in Section 3. The proposed alignment is compared with those using a fading KF and an existing STF with a vehicle test to verify its performance in Section 4. Finally, conclusive remarks are provided for the proposed alignment’s potential applications.
2 STATE AND MEASUREMENT EQUATIONS USING THE AQE MODEL
The state equations, including the attitude error, velocity error, position error, and measurement equations, are derived using the AQE model in the ECEF frame. Nonlinear velocity error equations are linearized further in the following filtering.
2.1 Attitude Error Equations
Herein, e and b represent the ECEF and body frames, respectively. The ECEF frame is assumed to be the navigation frame. Attitude errors are defined as the misalignment between the mathematical platform and the navigation frame. 
and 
are the true and indicated quaternion vectors, respectively. The AQE vector of 
and 
is defined as:
1
Then, the attitude error equations based on the AQE model are given as (Yu et al., 1999):
2
where 
is the angular rate vector in the body frame, ωie equals 
, and 
is the Earth rotation rate in the ECEF frame. Herein, the tildes indicate values with error. The other items are defined as:
3
4
5
Obviously, the attitude error equations in Equation (2) based on the AQE model are linear without any assumptions.
2.2 Velocity Error Equations
The velocity equations in the ECEF frame are as follows:
6
where ve is the velocity vector in the ECEF frame; 
is the rotation matrix from the body frame to the ECEF frame; 
, and fb are the specific forces in the ECEF and the body frames, respectively; a × is the skew-symmetric matrix of a; and ge is the gravity in the ECEF frame. The quaternions of the two 3D vectors, fe and fb, are denoted as fe′ and fb′, respectively. Their scalar and complex components are zero and 3D vectors, respectively. The specific force 
can be written as:
7
where ∘ denotes the quaternion multiplication and 
is the complex conjugate of 
. The error of the specific force can be obtained as:
8
where 
is the quaternion related to 
. If δfe is the vector related to the quaternion 
, the velocity error equations can be derived as:
9
where δve and δge are the errors of ve and ge, respectively. 
, assumed as a constant, is the Earth rotation rate in the ECEF frame. For the low-cost INS, δge can be treated as noise. Hence, only δfe is the nonlinear item in Equation (9). Substitute Equation (1) and 
into Equation (8) and yield:
10
If the attitude errors are small enough that the last four items on the right side of Equation (10) can be ignored, Equation (10) can be approximated as:
11
The item, δfe, can be linearized as:
12
where:
13
14
15
Substitute Equation (12) into Equation (9) and yield:
16
Equation (16) is the linearized velocity error equation using the AQE model in the ECEF frame, but it is derived under small attitude errors. In Equation (10), δfb′ is related to the accelerometer errors so the values of δfb′ are usually far less than those of 
. The values of 
are also far less than those of 
under small attitude errors. However, with large attitude errors, the values of 
are no less than those of 
, so the linearized errors would be very large if the four items related to 
are ignored on the right side of Equation (10). Hence, Equation (16) is only applicable for small attitude errors (i.e., the fine stage of the alignment).
However, in the coarse stage of alignment, although the values of δfb′ are still small, the values of 
are large in Equation (10) because the attitude errors are very large. Hence, the fourth item on the right side of Equation (10) should be taken into account in the linearized equation while the four items related to δfb′ can be ignored. That is, δfe can be approximated as:
17
where:
18
19
20
where 
is the estimated 
. Substitute Equation (17) into Equation (9) and yield:
21
Equation (21) is the linearized velocity error equation using the AQE model in the ECEF frame for the coarse stage. It should be noted that the estimated 
, is used in Equation (21). Hence, the impact of large errors due to linearized and ignored items in the filtering should be considered in the coarse stage, which will be further addressed in Section 3.
2.3 Position Error Equations
The position equation in the ECEF frame is as follows:
22
where re is the position vector in the ECEF frame. If the position error is denoted as 
, there is:
23
Equation (23) is the position error equation in the ECEF frame, which is linear.
2.4 State Equations
If the states are denoted as:
24
herein, the errors of accelerometers and gyroscopes are modeled as:
25
26
27
28
where nra, nrg, na, and ng are zero-mean white noise.
The state equations include Equations (2), (9), (23), (27), and (28). Because all the equations except Equation (9) are linear, the state equations are linear, too, if the linearized velocity error equations in Equation (16) or (21) are employed. The state equations can be written as:
29
In the coarse stage:
30
31
where In×n is an n × n unit matrix. In the fine stage:
32
33
In this paper, the estimated ba and bg values are only used to compensate for the accelerometer and gyroscope biases in the fine stage since they are not accurate enough for the coarse stage.
2.5 Measurement Equations
The measurements are the difference between the velocity and position provided by GPS and those output by the INS. The measurement equations are as follows:
34
where 
and measurement noise, 
. The measurement equations in Equation (34) are also linear.
3 FILTERING
According to the derivation above, the linearized velocity error equations in Equations (16) and (21) are applicable to the fine and coarse stages of alignment, respectively, so that the standard KF can be employed. However, the linearized errors in Equation (21) will be very large in the coarse stage so the linear KF runs the risk of becoming significantly degraded or even diverged.
The most effective method to mitigate the impact of large linearized errors is the use of a fading KF which relies more on the latest measurements than the earlier ones in filtering. The STF algorithm may be more effective than the fading KF in mitigating the linearized error since it adaptively adjusts the fading factor to the residuals in real time.
However, both the fading KF and STF methods may not achieve the desirable results in the coarse alignment because only velocity error equations are linearized (whereas the others are linear). Hence, an improved STF algorithm is proposed to address the difference among the state equations herein.
3.1 Discrete State and Measurement Equations
If the superscript e is omitted in Equations (29) and (34) for brevity, the state and measurement equations in Equations (29) and (34) can be rewritten as:
35
36
The corresponding discrete equations can be derived as:
37
38
where xk is the state vector at time tk; Φk–1 is the state transition matrix from tk–1 to tk; wk–1 is the state noise vector; zk is the measurement vector; Hk is the measurement matrix; nk is the measurement error; E(wk) = 016×1; E(nk) = 06×1; 
; 
; E(x) is the expectation of x; δkl is the Kronecker delta function; and Qk and Rk are the covariance matrices of wk and nk, respectively. Since the updating period, T, is very short (such as 10 ms), Φk–1 is approximated as (I + FT).
If there are no errors in both Equations (37) and (38), the standard KF method can achieve the optimal state estimation in the merit of minimum variance. On the contrary, the standard KF will be significantly degraded in performance or even divergent if there are large errors in Equation (37) and/or Equation (38). Hence, a standard KF is usually employed in the fine stage while a more robust algorithm should be used in the coarse stage.
3.2 Improved STF Algorithm
The improved STF algorithm is composed of two steps, updating and prediction, which is similar to the standard KF algorithm. Equations (39) through (43) explain further.
Updating Step:
39
40
41
Prediction Step:
42
43
where 
is the estimate of xk; (−) and (+) denote the values before and after the measurement update, respectively; Pk is the state error covariance matrix; 
and 
are the two matrices determined by the position and velocity measurements in real time, respectively; and Kk is the gain matrix.
Obviously, for the standard KF algorithm, Pk(−) is determined as:
44
For the fading KF algorithm, Pk(−) is modified as:
45
where s is the fading factor and s > 1. The value of s is usually preset as a constant so the fading KF cannot achieve a better performance by adaptively adjusting the value of s in real time. Hence, in the STF algorithm, s is updated to be:
46
where max(a, b) equals a if a ≥ b or b otherwise; trace (A) is the trace of A; and:
47
48
49
50
where ρ is a constant less than 1 and 
.
Hence, both the fading KF and STF algorithms enlarge Pk(−) with the same scale for all states as shown in Equation (45). However, only the velocity error in the state equations have large linearization errors in the coarse stage, so only parts of Pk(−) will be affected by linearization errors. Hence, it is more reasonable to fade the parts of Pk(−) that is more significantly affected by linearization errors than the others. As shown in Equation (43), the velocity-related parts of Pk(−), 
, are faded further in the improved STF algorithm. Both 
and 
are determined as follows.
In Equation (9), only δfe has large linearized errors, so the state matrix error can be specified as:
51
52
where 
and F are the state matrix with error and the linearized state matrix, respectively, and ΔF is the error matrix. Since Φk–1 is approximated as (I + FT), the error of Φk–1 can be derived as:
53
The matrix Pk(−), due to the impact of ΔΦ, can be written as:
54
If we let 
, there is:
55
Substituting Equation (53) into Equation (55) yields:
56
According to Equation (56), only parts of Pk(−) are affected by ΔF. In Equation (56), ΔP12, ΔP32, ΔP42, and ΔP52 are determined by the first item on the right side of Equation (55). Similarly, ΔP21, ΔP23, ΔP24, and ΔP25 are determined by the second item on the right side of Equation (55). However, ΔP22 is contributed by all three items on the right side of Equation (55).
Substituting Equation (55) into Equation (54) yields:
57
In the fading KF or the STF algorithm, only the first item on the right side of Equation (57) is faded. Herein, the second item on the right side of Equation (57) is also taken into account. Hence, Pk(−) is modified as Equation (43), where:
58
59
60
61
62
63
64
where λ is a constant and λ = 3 in the following vehicle test. As expressed in Equation (43), the first two items on the right side are faded by the STF method. The fading factor sPos is related to the position measurements while sVel is mainly related to the velocity measurements. Since ΔP22 is contributed by all three items on the right side of Equation (55), 
in Equation (64) is multiplied by (3sVel − 2).
4 EXPERIMENTS
The performance of the proposed two-stage alignment in the ECEF frame was verified by two simulations as well as a vehicle test. In both the simulations and the vehicle test, the biases and the random walks of the gyroscopes and accelerometers were 
, 16 mg, and 1.3×10−4 m/s3/2, respectively. The GPS receiver had a position error of 5 m and a velocity error of 0.2 m/s, respectively. The GPS receiver’s data were sampled at 1 Hz while the IMU data were acquired at 100 Hz.
In the vehicle test, a Unicorecomm UR370 real-time kinematic (RTK) receiver used as a reference had position, velocity, and attitude errors of 0.01 m, 0.03 m/s, and 
(0.8-m baseline), respectively. The RTK receiver’s data were sampled at 1 Hz. The RTK receiver had two antennas so it could measure two attitude angles of the vehicle. In the test, the two antennas were put along the vehicle’s body direction so that the yaw and pitch angles of the vehicle could be measured.
The level adjustment was not performed herein. The performance of the alignments are evaluated in the local level frame since the output of the RTK receiver is in the local level frame. The local level frame is herein referred to as the east-north-up (ENU) frame. The initial attitude errors were set as large as 
in the local level frame to evaluate the performance of the alignments under large initial attitude errors.
The coarse alignments with the fading KF, the STF, and the improved STF algorithms are switched to fine alignments with the standard KF algorithm if the criterion for the transition is satisfied (Wang et al., 2020). In experimentation, it requires the module of the estimated attitude error vector to be less than 5° for 10 s. The coarse alignment with the OBA algorithm switches to a fine alignment with the standard KF algorithm after a preset epoch. Herein, the switch time was set at 50 s.
4.1 Simulations
A car moving in a plane is simulated herein. In the following two simulations, the trajectories composed of U-turns and lines, which are convenient to implement in applications, are beneficial to improving observability in the alignment since the car’s acceleration is changing all the time (Li et al., 2015; Rhee et al., 2004). Figures 1 and 2 depict the trajectory and car velocities in the ENU frame in the first simulation, respectively. The car ran along a figure-8-shaped trajectory for six laps over 288 s.
The figure-8 trajectory for the first simulation
The car velocities in the ENU frame in the first simulation
Figure 3 depicts the yaw errors for the fading KF, original STF, and improved STF algorithms for the coarse stage and the standard KF algorithm for the fine stage in the ECEF frame, respectively. The yaw errors with the OBA algorithm for the coarse stage and the standard KF algorithm for the fine stage in the ECEF frame are also provided in Figure 3 for comparison. The coarse alignments with the fading KF, original STF, and improved STF algorithms were switched to fine alignment with the standard KF algorithm at 124 s, 58 s, and 36 s, respectively. However, the coarse alignment with the OBA algorithm was switched to the fine ones with the standard KF algorithm at 50 s. The converging process of the pitch errors was similar to that of the yaw errors in Figure 3, so it is not provided.
Yaw errors for the fading KF, original STF, and improved STF algorithms in the ECEF frame
In Figure 3, the yaw errors were very large at the beginning of the coarse stages for the four algorithms, but they converged to small values with the coarse alignments going on. In particular, it took the improved STF algorithm only 36 s to reduce the yaw error from 
to 
or less as shown in Figure 3. However, it took the STF algorithm 58 s and the fading KF algorithm 124 s to reduce the yaw error to 
or less, respectively. Hence, the STF algorithm was faster in converging than the fading KF algorithm, since the fading factor was adjusted adaptively in the former but set as a constant in the latter. Moreover, the improved STF algorithm was even faster in converging than the original STF algorithm due to the specific treatment of the linearized velocity error equation in the improved STF algorithm.
It should be noted that the OBA algorithm converged the fastest of all four algorithms for coarse alignment. It took the OBA algorithm just 20 s to reduce the yaw error to 
or less. Unfortunately, as mentioned in the introduction, the OBA algorithm in the coarse stage can initialize neither the states nor their covariances directly in the following fine stage, which is not good for KF convergence. As illustrated in Figure 3, it took the standard KF algorithm in the fine stage more than 50 s to converge if the OBA algorithm was used in the coarse stage, while it took the standard KF algorithm in the fine stage no more than 5 s to converge if the improved STF algorithm was employed in the coarse stage. Hence, the improved STF algorithm is desirable for the coarse alignment compared with the other three algorithms.
The trajectory and car velocities in the ENU frame in the second simulation are shown in Figures 4 and 5, respectively. The car ran along the long square trajectory for eight laps over 304 s. Obviously, in practice, the long square trajectory in Figure 4 would be easier to implement than the figure-8 trajectory in Figure 1. Figure 6 depicts the yaw errors. As shown in Figure 6, coarse alignments with the fading KF, original STF, and improved STF algorithms were switched to fine ones with the standard KF algorithm at 119 s, 62 s, and 29 s, respectively. The OBA algorithm also converged the fastest of the four algorithms for coarse alignment, but it took the following fine alignment more than 50 s to converge, which was similar to that in Figure 3. That is, the alignments in the second simulation achieved almost the same performance as those in the first simulation, but the long square trajectory would be easier to implement in practice than the figure-8 trajectory. Hence, only the long square trajectory was further employed in the following vehicle test.
The long square trajectory for the second simulation
Car velocities in the ENU frame in the second simulation
Yaw errors for the fading KF, original STF, and improved STF algorithms in the ECEF frame
4.2 Vehicle test
The performance of the proposed alignment was further verified by a vehicle test. In the test system shown in Figure 7, there was an ARM® STM32F103 controller with a frequency of 166 MHz, an AD® ADIS16375 MEMS IMU, a u-blox® NEO-6M GPS receiver, and a Unicorecomm UR370 RTK receiver. More of the main specifications of the test system are listed at the beginning of the section. The test system was mounted on the top of the vehicle in test. The IMU, GPS, and RTK data in the test, which are the same as those in Wang et al. (2020), were sampled and saved for offline processing. As shown in Figure 8, the vehicle ran along a long square trajectory similar to that in Figure 4. Its velocities are depicted in Figure 9. Herein, the outputs of the MEMS IMU and the NEO-6M GPS receiver were used to verify the performance of the alignment algorithms and those of the RTK receiver were used for reference.
Setup of the test system
The car trajectory in the test
The car velocities in the ENU frame in the test
Figure 10 depicts yaw errors. As shown in Figure 10, the coarse alignments for the fading KF, original STF, and improved STF algorithms were switched to the fine ones with the standard KF algorithm at 149 s, 54 s, and 39 s, respectively. The OBA algorithm converged fastest of all four algorithms for coarse alignment once more, but it took the standard KF algorithm in the following fine alignment more than 25 s to converge, which is similar to the results in the previous two simulations. That is, the alignments in the vehicle test had similar performance to those in the two prior simulations.
Yaw errors for the fading KF, original STF, and improved STF algorithms in the ECEF frame
5 CONCLUSION
Aiming to provide a fast initial alignment algorithm for a low-cost INS in the ECEF frame under the condition of large initial attitude errors (since the alignment and the following navigation in the ECEF frame is efficient in computation), a two-stage in-motion alignment with GPS measurements was proposed using the AQE model. An improved STF algorithm was proposed for the coarse stage while the standard KF was used for the fine stage. Although the initial attitude errors were very large, level adjustment was not needed for the improved STF algorithm so the time previously reserved for adjustment is no longer necessary.
Both of the simulations as well as the following vehicle test proved that the improved STF algorithm was fastest among the three algorithms for the coarse stage under the same conditions. The main reason for this lies in the specific processing of the linearized velocity error equations in the improved STF algorithm. Although the OBA algorithm converges faster than the improved STF algorithm, the standard KF algorithm in the subsequent fine alignment would take an extra period of time to converge. Hence, the proposed two-stage alignment method is especially appropriate for low-cost INS applications, such as self-driving cars, due to its low computational load and fast convergence.
HOW TO CITE THIS ARTICLE
Wang, K., Gao, W., Xu, X., & Wang, J. (2023). Adaptive alignment for low-cost INS in ECEF frame under large initial attitude errors. NAVIGATION, 70(1). https://doi.org/10.33012/navi.554
ACKNOWLEDGMENTS
This paper is sponsored by National Natural Science Foundation under the grant number 62173011.
This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited.
Jump to section
Related Articles
Cited By...
- No citing articles found.