Rapid SINS Two-Position Ground Alignment Scheme Based on Piecewise Combined Kalman Filter and Azimuth Constraint Information

The accuracy and rate of convergence are two important performance factors for initial ground alignment of a strapdown inertial navigation system (SINS). For navigation-grade SINS, gyro biases and accelerometer offsets can be modeled as constant values during the alignment period, and they can be calibrated through two-position ground alignment schemes. In many situations for SINS ground alignment, the azimuth of the vehicle remains nearly constant. This quasi-stationary alignment information can be used as an augmented measurement. In this paper, a piecewise combined Kalman filter utilizing relative azimuth constraint (RATP) is proposed to improve the alignment precision and to reduce the time consumption for error convergence. It is presented that a piecewise time-invariant linear system can be combined into a whole extended time-invariant linear system so that a piecewise combined Kalman filter can be designed for state estimation. A two-position ground alignment algorithm for SINS is designed based on the proposed piecewise combined Kalman filter. Numerical simulations and experimental results show its superiority to the conventional algorithms in terms of accuracy and the rate of convergence.


Introduction
Ground alignment is divided into integrated alignment and self-alignment. The initial-self alignment process generally consists of two stages called coarse alignment and fine alignment [1][2][3][4][5][6]. This paper focuses on fine alignment, which plays a pivotal role in the alignment accuracy and the rate of convergence. The rotation modulation technique (RMT) is one of the effective techniques available for self-alignment without any external information [7,8], and improves the alignment performance compared with traditional alignment schemes [9,10]. The RMT includes single-axis modulation, dual-axis modulation, and tri-axis modulation [11]. Single-axis modulation is much more widely used in practice because it is simpler and easier to operate [1,9,12]. In recent years, scholars have conducted much research on single-axis modulation. Acharya et al. [13] used augmented measurement to improve the rate of convergence of azimuth error. Refs. [14][15][16] utilized adding angular rates as measurements to provide good alignment performance, while Ref. [17] proposed an improved initial alignment based on horizontal alignment information in inertial frame, and the authors of [18] employed linear equality to improve the observability of strapdown inertial navigation system (SINS). A self-alignment method was put forward based on three vectors of gravitational apparent motion in an inertial frame [19]. Ref. [20] proposed a dual mathematical calculation system (DMCS) to improve the alignment accuracy when there is a large initial misalignment angle. Furthermore, nonlinear filters [12,21] were also ( ) ( ) ( ) ( ) ( ) where matrix ( ) Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model: 11 Meanwhile, there are observations associated with two state vectors ( ) 11 Consequently, a piecewise combined Kalman filter based on the system dynamic model in Equation (4) and observation model in Equation (5) can be constructed, which is named as PC-KF in the following sections. The PC-KF can be widely used in piecewise constant linear systems for state estimation, while we only focus its application on two-position alignment schemes in this paper.

The Conventional SINS Two-Position Alignment Model
The conventional error state model for SINS rapid self-alignment under quasi-stationary conditions, which is similar to Equation (1), can be written as follows [16,18,28]: where ( ) represents the attitude error vector; represents the velocity error vector expressed in local navigation frame n .
represent the gyro triad bias vector and the accelerometer triad offset vector, respectively, which can be seen as constant values for rapid SINS self-alignment.
where matrix ( ) Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model: 11 where matrix satisfy the following conditions while T is the system running time: Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model: Meanwhile, there are observations associated with two state vectors ( ) Therefore, the observation model shown in Equation (2) can be further expressed as Consequently, a piecewise combined Kalman filter based on the system dynamic model in Equation (4) and observation model in Equation (5) can be constructed, which is named as PC-KF in the following sections. The PC-KF can be widely used in piecewise constant linear systems for state estimation, while we only focus its application on two-position alignment schemes in this paper.

The Conventional SINS Two-Position Alignment Model
The conventional error state model for SINS rapid self-alignment under quasi-stationary conditions, which is similar to Equation (1), can be written as follows [16,18,28]: where ( ) represents the attitude error vector; represents the velocity error vector expressed in local navigation frame n .
represent the gyro triad bias vector and the accelerometer triad offset vector, respectively, which can be seen as constant values for rapid SINS self-alignment.  where t 1 ∈ 0, T 2 , t 2 ∈ T 2 , T . Meanwhile, there are observations associated with two state vectors x 11 (t 1 ) and x 12 (t 2 ). Therefore, the observation model shown in Equation (2) can be further expressed as where matrix satisfy the following conditions while T is the system running time: Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model: Meanwhile, there are observations associated with two state vectors ( ) Therefore, the observation model shown in Equation (2) can be further expressed as Consequently, a piecewise combined Kalman filter based on the system dynamic model in Equation (4) and observation model in Equation (5) can be constructed, which is named as PC-KF in the following sections. The PC-KF can be widely used in piecewise constant linear systems for state estimation, while we only focus its application on two-position alignment schemes in this paper.

The Conventional SINS Two-Position Alignment Model
The conventional error state model for SINS rapid self-alignment under quasi-stationary conditions, which is similar to Equation (1), can be written as follows [16,18,28]: where ( ) represents the attitude error vector; represents the velocity error vector expressed in local navigation frame n .
represent the gyro triad bias vector and the accelerometer triad offset vector, respectively, which can be seen as constant values for rapid SINS self-alignment.  Consequently, a piecewise combined Kalman filter based on the system dynamic model in Equation (4) and observation model in Equation (5) can be constructed, which is named as PC-KF in the following sections. The PC-KF can be widely used in piecewise constant linear systems for state estimation, while we only focus its application on two-position alignment schemes in this paper.

The Conventional SINS Two-Position Alignment Model
The conventional error state model for SINS rapid self-alignment under quasi-stationary conditions, which is similar to Equation (1), can be written as follows [16,18,28]: where matrix satisfy the following conditions while T is the system running time: Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model: Meanwhile, there are observations associated with two state vectors ( ) Therefore, the observation model shown in Equation (2) can be further expressed as Consequently, a piecewise combined Kalman filter based on the system dynamic model in Equation (4) and observation model in Equation (5) can be constructed, which is named as PC-KF in the following sections. The PC-KF can be widely used in piecewise constant linear systems for state estimation, while we only focus its application on two-position alignment schemes in this paper.

The Conventional SINS Two-Position Alignment Model
The conventional error state model for SINS rapid self-alignment under quasi-stationary conditions, which is similar to Equation (1), can be written as follows [16,18,28]: where ( ) represents the attitude error vector; represents the velocity error vector expressed in local navigation frame n .
represent the gyro triad bias vector and the accelerometer triad offset vector, respectively, which can be seen as constant values for rapid SINS self-alignment.
represents the attitude error vector; δv n = δv N δv E δv D T represents the velocity error vector expressed in local navigation frame n.
represent the gyro triad bias vector and the accelerometer triad offset vector, respectively, which can be seen as constant values for rapid SINS self-alignment. δv D and ∇ b z are generally not considered for SINS self-alignment in land vehicle applications, in addition, ε b z is difficult to estimate accurately in a short time, so it is not considered. Components of w(t) are process noises of φ n and δv n which can be denoted in a combined vector as w(t) = w φN w φE w φD w vN w vE T .
The process covariance matrix of w(t) is Q which can be written as: The matrix F takes the following form: . The process covariance matrix of ( ) t w is Q which can be written as: The matrix F takes the following form: Similarly, we have ( ) 11 C which denotes the attitude transformation matrix from b frame to n frame.
is the earth rotation rate with respect to the inertial frame expressed in n frame, while n en ω is the rotation rate of the n frame with respect to the e frame expressed in n frame. f represents the specific force acceleration. For quasi-stationary base initial alignment, the following simplification can be made: The conventional north and east velocity error observation model is given by Refs. [10][11][12][13] where ( ) 1 t υ represents the observation noise vector; 1 R represents the observation noise covariance matrix.
The conventional Kalman filter KF) can be designed based on the observation model shown in Equation (10) and the dynamic error model shown in Equation (6). The corresponding conventional two-position initial alignment algorithm is noted as TP in the following sections.

The Conventional Augmented Observation Model Based on Angular Rate Measurements
As shown in Refs. [13,15,16], angular rate measurements and the vehicle rotation rate constraint can be used in order to improve the convergence rate of alignment. Theoretically, n ie ω can be written as (8) Similarly, we have is Q which can be written as: The matrix F takes the following form: Similarly, we have ( ) 11 C which denotes the attitude transformation matrix from b frame to n frame.
is the earth rotation rate with respect to the inertial frame expressed in n frame, while n en ω is the rotation rate of the n frame with respect to the e frame expressed in n frame. f represents the specific force acceleration. For quasi-stationary base initial alignment, the following simplification can be made: The conventional north and east velocity error observation model is given by Refs. [10][11][12][13] where ( ) 1 t υ represents the observation noise vector; 1 R represents the observation noise covariance matrix.
The conventional Kalman filter KF) can be designed based on the observation model shown in Equation (10) and the dynamic error model shown in Equation (6). The corresponding conventional two-position initial alignment algorithm is noted as TP in the following sections.

The Conventional Augmented Observation Model Based on Angular Rate Measurements
As shown in Refs. [13,15,16], angular rate measurements and the vehicle rotation rate constraint can be used in order to improve the convergence rate of alignment. Theoretically, n ie ω can be written as (9) C ij (i, j = 1, 2, 3) represents the element of C n b which denotes the attitude transformation matrix from b frame to n frame. ω n ie = ω n ieN ω n ieE ω n ieD T is the earth rotation rate with respect to the inertial frame expressed in n frame, while ω n en is the rotation rate of the n frame with respect to the e frame expressed in n frame. f represents the specific force acceleration. For quasi-stationary base initial alignment, the following simplification can be made: f n = −g n , ω n en = 0. The conventional north and east velocity error observation model is given by Refs. [10][11][12][13] where υ 1 (t) represents the observation noise vector; R 1 represents the observation noise covariance matrix.
The conventional Kalman filter KF) can be designed based on the observation model shown in Equation (10) and the dynamic error model shown in Equation (6). The corresponding conventional two-position initial alignment algorithm is noted as TP in the following sections.

The Conventional Augmented Observation Model Based on Angular Rate Measurements
As shown in Refs. [13,15,16], angular rate measurements and the vehicle rotation rate constraint can be used in order to improve the convergence rate of alignment. Theoretically, ω n ie can be written as However, during the ground initial alignment period, ω n nb and ω n en can be seen as nearly zero if the vehicle or the SINS is in quasi-stationary situations. The estimated transformation matrix C n b is used for attitude update and C If we ignore small quantities and take the equivalent east gyro error as measurement, then the following equation can be obtained: As can be seen in Equation (14), the augmented observation model can be obtained by adding angular rate measurements. Then, the observation model can be expressed as If we ignore small quantities and take the equivalent east gyro error as measurement, then the following equation can be obtained: As can be seen in Equation (14), the augmented observation model can be obtained by adding angular rate measurements. Then, the observation model can be expressed as where 2 R takes the following form: represents the angular rate measurements noise about the east axis, and Here, the corresponding angular rate measurement-augmented two-position initial alignment algorithm is denoted as ARTP. Although sometimes the angular vibration amplitude of the vehicle is very small, the vibration angular rate can be large, especially along horizontal directions. For example, if the vehicle engine is still working in the quasi-stationary situations, n nb ω and n en ω are no longer zero, which lead to the changing of measurement noise . Thus, the ARTP algorithm has limitations in these cases.

The Piecewise Combined Kalman Filter for Improved Two-Position Initial Alignment
In this section, a piecewise combined Kalman filter (noted as PC-KF) is designed for the two-position SINS initial alignment to improve the azimuth accuracy and the rate of convergence.
For the two-position initial alignment, we suppose that the SINS is in state A when the IMU is in the first position, and in state B after the IMU has rotated 180° while the two positions have the same time duration [23]. (15) where R 2 takes the following form: However, during the ground initial alignment period, n nb ω and n en ω can be seen as nearly zero if the vehicle or the SINS is in quasi-stationary situations. The estimated transformation matrix n b C  is used for attitude update and n If we ignore small quantities and take the equivalent east gyro error as measurement, then the following equation can be obtained: As can be seen in Equation (14), the augmented observation model can be obtained by adding angular rate measurements. Then, the observation model can be expressed as where 2 R takes the following form: represents the angular rate measurements noise about the east axis, and Here, the corresponding angular rate measurement-augmented two-position initial alignment algorithm is denoted as ARTP. Although sometimes the angular vibration amplitude of the vehicle is very small, the vibration angular rate can be large, especially along horizontal directions. . Thus, the ARTP algorithm has limitations in these cases.

The Piecewise Combined Kalman Filter for Improved Two-Position Initial Alignment
In this section, a piecewise combined Kalman filter (noted as PC-KF) is designed for the two-position SINS initial alignment to improve the azimuth accuracy and the rate of convergence.
For the two-position initial alignment, we suppose that the SINS is in state A when the IMU is in the first position, and in state B after the IMU has rotated 180° while the two positions have the same time duration [23]. (16) υ ωE (t) represents the angular rate measurements noise about the east axis, and υ ωE (t) ≈ 0 1 0 (ω n nb + ω n en ) Here, the corresponding angular rate measurement-augmented two-position initial alignment algorithm is denoted as ARTP. Although sometimes the angular vibration amplitude of the vehicle is very small, the vibration angular rate can be large, especially along horizontal directions. For example, if the vehicle engine is still working in the quasi-stationary situations, ω n nb and ω n en are no longer zero, which lead to the changing of measurement noise υ ωE (t). Thus, the ARTP algorithm has limitations in these cases.

The Piecewise Combined Kalman Filter for Improved Two-Position Initial Alignment
In this section, a piecewise combined Kalman filter (noted as PC-KF) is designed for the two-position SINS initial alignment to improve the azimuth accuracy and the rate of convergence.
For the two-position initial alignment, we suppose that the SINS is in state A when the IMU is in the first position, and in state B after the IMU has rotated 180 • while the two positions have the same time duration [23].
Differently from the conventional SINS alignment system model shown in Equation (6) 0, where ( ) is Q , which is written as The matrixes 1 G and 2 G take the following form:  The observation model is constructed by taking the north velocity error, the east velocity error, and the relative azimuth error angle as the measurements. The relative azimuth error angle can be obtained based on the IMU azimuth relationship between state A and state B. where The covariance matrix of w(t) is Q, which is written as The matrixes G 1 and G 2 take the following form: 0, where ( ) . The covariance matrix of ( ) t w is Q , which is written as The matrixes 1 G and 2 G take the following form:   The observation model is constructed by taking the north velocity error, the east velocity error, and the relative azimuth error angle as the measurements. The relative azimuth error angle can be obtained based on the IMU azimuth relationship between state A and state B.
C n bA and C n bB represent the attitude transformation matrix in state A and state B. C ijA (i, j = 1, 2, 3) and C ijB (i, j = 1, 2, 3) represent the element of attitude matrix C n bA and C n bB , respectively. The observation model is constructed by taking the north velocity error, the east velocity error, and the relative azimuth error angle as the measurements. The relative azimuth error angle can be obtained based on the IMU azimuth relationship between state A and state B. C n b A and C n b B are the computational values of C n b in two states: The relationship between C n b A and C n b B can be written as Suppose the horizontal Euler angles between state A and state B are ∆γ AB and ∆θ AB , while the azimuth change is π + ∆ϕ AB . ∆γ AB , ∆θ AB and ∆ϕ AB are seen as unknown small angles. Thus, Taking c 21AB as the measurement, we then have Hence, the modified observation model is given by Taking 21AB c  as the measurement, we then have The observation noise covariance matrix R takes the following form: Consequently, the piecewise combined Kalman filter (PC-KF) based on the error dynamic model shown in Equation (18) and the modified observation model shown in Equation (26) can be designed. The corresponding two-position initial alignment algorithm with relative azimuth constraints is denoted as RATP in the following sections. (26) where H 1 1×3 is expressed as

Comparison of Simulation Results among Different Alignment Schemes
The observation noise covariance matrix R takes the following form: C  can be rewritten as Taking 21AB c  as the measurement, we then have Hence, the modified observation model is given by , 0 , The observation noise covariance matrix R takes the following form: Consequently, the piecewise combined Kalman filter (PC-KF) based on the error dynamic model shown in Equation (18) and the modified observation model shown in Equation (26) can be designed. The corresponding two-position initial alignment algorithm with relative azimuth constraints is denoted as RATP in the following sections.

Comparison of Simulation Results among Different Alignment Schemes
. (28) Consequently, the piecewise combined Kalman filter (PC-KF) based on the error dynamic model shown in Equation (18) and the modified observation model shown in Equation (26) can be designed. The corresponding two-position initial alignment algorithm with relative azimuth constraints is denoted as RATP in the following sections.

Comparison of Simulation Results among Different Alignment Schemes
According to the system dynamic error model constructed in Section 2, the third diagonal element of the covariance matrix P(t) in the conventional TP and ARTP algorithms, which is denoted as P (3,3) (t), or the eighth element diagonal of the covariance matrix P(t) in the new RATP algorithm, which is denoted as P (8,8) (t), may be regarded as a quantitative measurement of observability of the azimuth error and the alignment performance [23,26,29]. This section uses covariance simulation to compare P (3,3) (t) or P (8,8) (t) among the three alignment schemes. The simulation parameters are set in Table 1.  Con.1 represents the stationary condition, Con.2 represents angular vibrations with an amplitude of approximately 5 arcsec and a random frequency of 5-10 Hz, and Con.3 represents angular vibrations with an amplitude of approximately 10 arcsec and a random frequency of 5-10 Hz. TP conventional two-position initial alignment algorithm; ARTP, angular rate measurement-augmented two-position initial alignment algorithm; RATP, two-position initial alignment algorithm with relative azimuth constraints.
Simulation analysis is divided into two parts. The first part analyzes the variation of 1 − σ value of the final estimated azimuth error with the total alignment time changing from 30 to 300 s under different angular vibration conditions. The simulation results can be seen in Table 2 and from Figures 1-3. It is shown that the 1 − σ final azimuth error of RATP is smaller than that of TP and ARTP. The alignment accuracy of RATP is less affected by angular vibration conditions compared with that of ARTP under different angular vibration conditions. The estimated azimuth error of ARTP tends to be equal to TP if the alignment time is long enough. The performance of ARTP is better than that of TP under stationary conditions, but its performance becomes worse when there are angular vibrations. In the case of angular vibration, ARTP can achieve faster convergence rate than TP for rapid alignment, but its accuracy is inferior to TP when the alignment time is longer. The second part analyzes the azimuth error convergence process through time history curves of the three schemes with the same total alignment time (180 s). The simulation result can be seen in Figures 1-3. The convergence speed of RATP is faster than that of TP and ARTP, and ARTP is even slower than TP under large angular vibration conditions. Therefore, RATP is more suitable for rapid initial alignment.
Con.1 represents the stationary condition, Con.2 represents angular vibrations with an amplitude of about 5 arcsec and a random frequency of 5-10 Hz, and Con.3 represents angular vibrations with an amplitude of about 10 arcsec and a random frequency of 5-10 Hz. vibrations with an amplitude of approximately 10 arcsec and a random frequency of 5-10 Hz. TP conventional two-position initial alignment algorithm; ARTP, angular rate measurement-augmented two-position initial alignment algorithm; RATP, two-position initial alignment algorithm with relative azimuth constraints. Con.1 represents the stationary condition, Con.2 represents angular vibrations with an amplitude of about 5 arcsec and a random frequency of 5-10 Hz, and Con.3 represents angular vibrations with an amplitude of about 10 arcsec and a random frequency of 5-10 Hz.   vibrations with an amplitude of approximately 10 arcsec and a random frequency of 5-10 Hz. TP conventional two-position initial alignment algorithm; ARTP, angular rate measurement-augmented two-position initial alignment algorithm; RATP, two-position initial alignment algorithm with relative azimuth constraints. Con.1 represents the stationary condition, Con.2 represents angular vibrations with an amplitude of about 5 arcsec and a random frequency of 5-10 Hz, and Con.3 represents angular vibrations with an amplitude of about 10 arcsec and a random frequency of 5-10 Hz.

Two-Position Initial Alignment Experiment
Practical experimental tests were carried out to compare the two-position alignment performance by using the three schemes described earlier. The total alignment time is 180 s. For the

Two-Position Initial Alignment Experiment
Practical experimental tests were carried out to compare the two-position alignment performance by using the three schemes described earlier. The total alignment time is 180 s. For the first 90 s, the IMU is in state A, then the IMU rotated 180 • to state B. The experimental environment is shown in Figure 4. The main parameters of the inertial devices are shown in Table 3.

Two-Position Initial Alignment Experiment
Practical experimental tests were carried out to compare the two-position alignment performance by using the three schemes described earlier. The total alignment time is 180 s. For the first 90 s, the IMU is in state A, then the IMU rotated 180 to state B. The experimental environment is shown in Figure 4. The main parameters of the inertial devices are shown in Table 3.    The velocity error curves under different conditions are shown in Figure 5. We can see from Figure 5 that the velocity errors of the three schemes are all less than 0.0005 m/s under different angular vibrations. Therefore, the velocity errors of the three schemes all meet the performance requirements. Figure 6 shows the horizontal curves of the three schemes, hence, we can make the conclusion that TP, ARTP, and RATP have a rate of convergence of horizontal attitude error which is similar with the velocity error. Furthermore, the convergence time of horizontal attitude errors is less than 10 s when SINS is in state B. Figure 5 that the velocity errors of the three schemes are all less than 0.0005 m/s under different angular vibrations. Therefore, the velocity errors of the three schemes all meet the performance requirements. Figure 6 shows the horizontal curves of the three schemes, hence, we can make the conclusion that TP, ARTP, and RATP have a rate of convergence of horizontal attitude error which is similar with the velocity error. Furthermore, the convergence time of horizontal attitude errors is less than 10 s when SINS is in state B.  The azimuth errors of the three schemes for 180 s rapid alignment are shown in Table 4, and  The azimuth errors of the three schemes for 180 s rapid alignment are shown in Table 4, and   azimuth error of RATP at 180 s are 43.75, 50.75, and 87.83 arcsec, respectively. RATP shows superior performance under different conditions. Above all, the azimuth error convergence to a steady value is much faster for RATP than for the other two schemes under different conditions. The azimuth error accuracy of RATP in the same total alignment time is significantly higher than that of the traditional algorithms. Therefore, the experimental results have shown the effectiveness and superiority of the proposed algorithm. Con.1 represents the stationary condition, Con.2 represents angular vibrations with an amplitude of about 5 arcsec and a random frequency of 5-10 Hz, and Con.3 represents angular vibrations with an amplitude of about 10 arcsec and a random frequency of 5-10 Hz.

Conclusions
To improve alignment precision and reduce the time needed for error convergence, this paper proposes a novel piecewise combined Kalman filter for state estimation. A two-position ground alignment algorithm for SINS is designed based on the proposed piecewise combined Kalman filter. Simulation results show that the proposed RATP algorithm presents better performance under different experimental conditions compared with the conventional alignment algorithms. ARTP performs better than TP under stationary condition, and it can achieve a faster convergence rate than TP for rapid alignment, but the accuracy of ARTP accuracy was inferior to TP when the alignment time under angular vibrations was longer. The real experimental results indicate that for the SINS in this paper, the azimuth error of RATP is about 40% less than that of the conventional algorithms under stationary or angular vibration conditions for 180 s rapid alignment. The proposed two-position ground alignment scheme, which is based on a piecewise combined Kalman filter and azimuth constraint information, has important engineering value for rapid SINS self-alignment.

Conclusions
To improve alignment precision and reduce the time needed for error convergence, this paper proposes a novel piecewise combined Kalman filter for state estimation. A two-position ground alignment algorithm for SINS is designed based on the proposed piecewise combined Kalman filter. Simulation results show that the proposed RATP algorithm presents better performance under different experimental conditions compared with the conventional alignment algorithms. ARTP performs better than TP under stationary condition, and it can achieve a faster convergence rate than TP for rapid alignment, but the accuracy of ARTP accuracy was inferior to TP when the alignment time under angular vibrations was longer. The real experimental results indicate that for the SINS in this paper, the azimuth error of RATP is about 40% less than that of the conventional algorithms under stationary or angular vibration conditions for 180 s rapid alignment. The proposed two-position ground alignment scheme, which is based on a piecewise combined Kalman filter and azimuth constraint information, has important engineering value for rapid SINS self-alignment.