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

Abstract

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.

1. 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 used to solve the large misalignment angle, but system model and computation became complex. Meanwhile, the error of coarse alignment is less than one degree in general situations [2,6], therefore the Kalman filter is mostly employed, although an alternative method was proposed to perform the initial alignment [22]. Hence, we consider the initial misalignment angle as a small angle after coarse alignment in this paper.

Different schemes can be implemented for single-axis rotation, such as two-position alignment schemes [13,15,16,23,24,25,26,27], reciprocating rotation schemes, continuously rotation schemes [7,9,18], and others. Two-position alignment schemes can improve the observability of the system and allow nearly all error states to be estimated, including gyro biases and accelerometer offsets. The heading change of the IMU (inertial measurement unit) is generally arranged as approximately 180° for two-position alignment with the same time duration to achieve optimal estimation results [23,25,26].

The north and east velocities are generally used as measurements in the Kalman filter for SINS self-initial alignment [23,28]. Recently, adding angular rates as measurements to get faster convergence was proposed [13,15,16,17,21]. The observation models are established in the body frame [13,17] and navigation frame [15,16,21], respectively.

In many inertial navigation applications, both high initial alignment precision and fast error convergence are required [15,16,18]. Ref. [16] considered the influence of cross-correlation between system noises and measurement noises. The output of the equivalent east gyro in geographic frame [29] and a nonlinear state constraint [18] are used to improve the speed of convergence. It is shown from the above literature that adding effective measurement information can accelerate the convergence speed of azimuth error. The current two-position alignment research is mainly concentrated on adding gyro information, and the vehicle rotation rate constraint as measurements [13,14,15,16,17,18,21,30]. The performance of this method is limited in practical applications because the observation noise related with gyro information and the vehicle rotation rate constraint can be generally much larger, even under small angular vibrations.

On the other hand, for ground self-alignment applications, the SINS is approximately stationary with respect to Earth, and the azimuth remained nearly constant during the quasi-stationary alignment period. The azimuth constraint information could be used as observation, based on which the vehicle velocity, attitude, and IMU errors could be estimated and calibrated. The observation noise related with the IMU azimuth constraint information is generally much smaller under quasi-stationary situations.

Using the above considerations, a piecewise combined Kalman filter is proposed in this paper to improve the accuracy and rate of convergence of the two-position alignment based on IMU azimuth constraint information.

The rest of the paper is organized as follows. The piecewise combine Kalman filter, the new two-position alignment models and alignment performance by simulation are shown in Section 2. Results and discussions of experiments are presented in Section 3. Conclusions are given in Section 4.

2. Materials and Methods

The concept of the piecewise combined time-invariant linear system model and the piecewise combined Kalman filter is proposed in this section. The conventional two-position alignment model as well as the augmented observation model based on gyro information and vehicle rotation rate constraint are briefly introduced. Finally, an improved two-position alignment scheme is presented based on the piecewise combined Kalman filter, which utilizes IMU azimuth constraint information for self-alignment.

2.1. The Piecewise Combined Time-Invariant Linear System Model

The system equation and observation equation of a piecewise time-invariant linear system [31,32] can be expressed as follows:

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_1\left(t\right)\\ {\dot{\mathit{x}}}_2\end{array}\right]=\left[\begin{array}{cc}\mathit{F}\left(t\right)& \mathit{G}\left(t\right)\\ 0& 0\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_1\left(t\right)\\ {\mathit{x}}_2\end{array}\right]+\left[\begin{array}{c}\mathit{w}\left(t\right)\\ 0\end{array}\right],$$

(1)

$$\mathit{z}=\left[\begin{array}{cc}\mathit{H}& 0\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_1\left(t\right)\\ {\mathit{x}}_2\end{array}\right]+\mathit{\upsilon }\left(t\right),$$

(2)

where matrix $\mathit{F}\left(t\right)$ and $\mathit{G}\left(t\right)$ satisfy the following conditions while $T$ is the system running time:

$$\mathit{F}\left(t\right)=\{\begin{array}{c}{\mathit{F}}_1\hspace{1em}t\in \left(0,\frac{T}{2}\right]\\ {\mathit{F}}_2\hspace{1em}t\in \left(\frac{T}{2},T\right]\end{array}\hspace{1em}\mathit{G}\left(t\right)=\{\begin{array}{c}{\mathit{G}}_1\hspace{1em}t\in \left(0,\frac{T}{2}\right]\\ {\mathit{G}}_2\hspace{1em}t\in \left(\frac{T}{2},T\right]\end{array}.$$

(3)

Therefore, the original piecewise time-invariant linear system model shown in Equation (1) can be rewritten as the following piecewise combined system model:

$$\left[\begin{array}{c}{\dot{\mathit{x}}}_{11}\left(t_1\right)\\ {\dot{\mathit{x}}}_{12}\left(t_2\right)\\ {\dot{\mathit{x}}}_2\end{array}\right]=\left[\begin{array}{ccc}{\mathit{F}}_1& 0& {\mathit{G}}_1\\ 0& {\mathit{F}}_2& {\mathit{G}}_2\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{11}\left(t_1\right)\\ {\mathit{x}}_{12}\left(t_2\right)\\ {\mathit{x}}_2\end{array}\right]+\left[\begin{array}{c}{\mathit{w}}_1\left(t_1\right)\\ {\mathit{w}}_2\left(t_2\right)\\ 0\end{array}\right],$$

(4)

where $t_1\in \left(0,\frac{T}{2}\right]$, $t_2\in \left(\frac{T}{2},T\right]$.

Meanwhile, there are observations associated with two state vectors ${\mathit{x}}_{11}\left(t_1\right)$ and ${\mathit{x}}_{12}\left(t_2\right)$. Therefore, the observation model shown in Equation (2) can be further expressed as

$$\overline{\mathit{z}}=\left[\begin{array}{ccc}\mathit{H}& 0& 0\\ 0& \mathit{H}& 0\\ {\mathit{H}}_1& {\mathit{H}}_2& 0\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_{11}\left(t_1\right)\\ {\mathit{x}}_{12}\left(t_2\right)\\ {\mathit{x}}_2\end{array}\right]+\left[\begin{array}{c}{\mathit{\upsilon }}_1\left(t_1\right)\\ {\mathit{\upsilon }}_2\left(t_2\right)\\ {\mathit{\upsilon }}_r\left(t_{12}\right)\end{array}\right].$$

(5)

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.

2.2. 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]:

$$\begin{array}{c}\left[\begin{array}{c}{\dot{\mathit{x}}}_1\left(t\right)\\ {\dot{\mathit{x}}}_2\end{array}\right]=\left[\begin{array}{cc}\mathit{F}& \mathit{G}\left(t\right)\\ 0_{4\times 5}& 0_{4\times 4}\end{array}\right]\left[\begin{array}{c}{\mathit{x}}_1\left(t\right)\\ {\mathit{x}}_2\end{array}\right]+\left[\begin{array}{c}\mathit{w}\left(t\right)\\ 0_{1\times 4}\end{array}\right]\hspace{1em}\\ =\mathit{A}\mathit{x}\left(t\right)+\mathit{w}\left(t\right),\hspace{1em}\mathit{w}\left(t\right)\sim N\left(0,\mathit{Q}\right),\end{array}$$

(6)

where ${\mathit{x}}_1\left(t\right)={\left[\begin{array}{ccccc}{\varphi }_N^{}& {\varphi }_E^{}& {\varphi }_D^{}& \delta v_N& \delta v_E\end{array}\right]}^T$ and ${\mathit{x}}_2={\left[\begin{array}{cccc}{\epsilon }_x^b& {\epsilon }_y^b& {\nabla }_x^b& {\nabla }_y^b\end{array}\right]}^T$; ${\mathit{\varphi }}^n={\left[\begin{array}{ccc}{\varphi }_N^{}& {\varphi }_E^{}& {\varphi }_D^{}\end{array}\right]}^T$ represents the attitude error vector; $\delta {\mathit{v}}^n={\left[\begin{array}{ccc}\delta v_N& \delta v_E& \delta v_D\end{array}\right]}^T$ represents the velocity error vector expressed in local navigation frame $n$. ${\mathit{\epsilon }}^b={\left[\begin{array}{ccc}{\epsilon }_x^b& {\epsilon }_y^b& {\epsilon }_z^b\end{array}\right]}^T$ and ${\mathbf{\nabla }}^b={\left[\begin{array}{ccc}{\nabla }_x^b& {\nabla }_y^b& {\nabla }_z^b\end{array}\right]}^T$ 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. $\delta v_D$ and ${\nabla }_z^b$ are generally not considered for SINS self-alignment in land vehicle applications, in addition, ${\epsilon }_z^b$ is difficult to estimate accurately in a short time, so it is not considered. Components of $\mathit{w}\left(t\right)$ are process noises of ${\mathit{\varphi }}^n$ and $\delta {\mathit{v}}^n$ which can be denoted in a combined vector as $\mathit{w}\left(t\right)={\left[\begin{array}{ccccc}{w}_{\varphi N}& w_{\varphi E}& w_{\varphi D}& w_{vN}& w_{vE}\end{array}\right]}^T$. The process covariance matrix of $\mathit{w}\left(t\right)$ is $\mathit{Q}$ which can be written as:

$$\mathit{Q}=\left[\begin{array}{cc}{n}_g^2{\mathit{I}}_{3\times 3}& 0_{3\times 2}\\ 0_{2\times 3}& n_a^2{\mathit{I}}_{2\times 2}\end{array}\right]$$

(7)

The matrix $\mathit{F}$ takes the following form:

$$\mathit{F}=\left[\begin{array}{ccccc}0& {\omega }_{ieD}^n& 0& 0& 0\\ -{\omega }_{ieD}^n& 0& {\omega }_{ieN}^n& 0& 0\\ 0& -{\omega }_{ieN}^n& 0& 0& 0\\ 0& g& 0& 0& 2{\omega }_{ieD}^n\\ -g& 0& 0& -2{\omega }_{ieD}^n& 0\end{array}\right].$$

(8)

Similarly, we have

$$\mathit{G}\left(t\right)=\left[\begin{array}{cccc}-C_{11}& -C_{12}& 0& 0\\ -C_{21}& -C_{22}& 0& 0\\ -C_{31}& -C_{32}& 0& 0\\ 0& 0& C_{11}& C_{12}\\ 0& 0& C_{21}& C_{22}\end{array}\right].$$

(9)

$C_{ij}\left(i,j=1,2,3\right)$ represents the element of ${\mathit{C}}_b^n$ which denotes the attitude transformation matrix from $b$ frame to $n$ frame. ${\mathit{\omega }}_{ie}^n={\left[\begin{array}{ccc}{\omega }_{ieN}^n& {\omega }_{ieE}^n& {\omega }_{ieD}^n\end{array}\right]}^T$ is the earth rotation rate with respect to the inertial frame expressed in $n$ frame, while ${\mathit{\omega }}_{en}^n$ is the rotation rate of the $n$ frame with respect to the $e$ frame expressed in $n$ frame. $\mathit{f}$ represents the specific force acceleration. For quasi-stationary base initial alignment, the following simplification can be made: ${\mathit{f}}^n=-{\mathit{g}}^n$, ${\mathit{\omega }}_{en}^n=0$.

The conventional north and east velocity error observation model is given by Refs. [10,11,12,13]

$$\begin{array}{c}{\mathit{z}}_1\left(t\right)={\left[\begin{array}{cc}\delta v_N& \delta v_E\end{array}\right]}^T=\left[\begin{array}{ccc}{0}_{2\times 3}& {\mathit{I}}_{2\times 2}& 0_{2\times 4}\end{array}\right]\mathit{x}\left(t\right)+{\mathit{\upsilon }}_1\left(t\right)\\ ={\mathit{H}}_1\left(t\right)\mathit{x}\left(t\right)+{\mathit{\upsilon }}_1\left(t\right),\hspace{1em}{\mathit{\upsilon }}_1\left(t\right)\sim N\left(0,{\mathit{R}}_1\right)\end{array}$$

(10)

where ${\mathit{\upsilon }}_1\left(t\right)$ represents the observation noise vector; ${\mathit{R}}_1$ represents the observation noise covariance matrix.

$${\mathit{R}}_1=\left[\begin{array}{cc}{r}_v^2& 0\\ 0& r_v^2\end{array}\right]$$

(11)

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.

2.3. 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, ${\mathit{\omega }}_{ie}^n$ can be written as

$${\mathit{\omega }}_{ie}^n={\mathit{C}}_b^n{\mathit{\omega }}_{ib}^b-{\mathit{\omega }}_{nb}^n-{\mathit{\omega }}_{en}^n.$$

(12)

However, during the ground initial alignment period, ${\mathit{\omega }}_{nb}^n$ and ${\mathit{\omega }}_{en}^n$ can be seen as nearly zero if the vehicle or the SINS is in quasi-stationary situations. The estimated transformation matrix ${\tilde{\mathit{C}}}_b^n$ is used for attitude update and ${\overline{\mathit{C}}}_b^n=\left[\mathit{I}-{\mathit{\varphi }}^n\times \right]{\mathit{C}}_b^n$. Therefore,

$$\begin{array}{l}{\mathit{C}}_n^b\left({\mathit{C}}_b^n{\mathit{\omega }}_{ib}^b-{\mathit{\omega }}_{nb}^n-{\mathit{\omega }}_{en}^n\right)\approx \left({\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\epsilon }}^b\right)\\ \Rightarrow {\tilde{\mathit{C}}}_n^b\left(\mathit{I}-\left[{\mathit{\varphi }}^n\times \right]\right){\mathit{\omega }}_{ie}^n=\left({\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\epsilon }}^b\right)\\ \Rightarrow \left(\mathit{I}-\left[{\mathit{\varphi }}^n\times \right]\right){\mathit{\omega }}_{ie}^n={\tilde{\mathit{C}}}_b^n\left({\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\epsilon }}^b\right)\\ \Rightarrow {\tilde{\mathit{C}}}_b^n{\tilde{\mathit{\omega }}}_{ib}^b=\left(\mathit{I}-\left[{\mathit{\varphi }}^n\times \right]\right){\mathit{\omega }}_{ie}^n+{\tilde{\mathit{C}}}_b^n{\mathit{\epsilon }}^b\\ \Rightarrow {\tilde{\mathit{C}}}_b^n{\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\omega }}_{ie}^n=\left[{\mathit{\omega }}_{ie}^n\times \right]{\mathit{\varphi }}^n+{\tilde{\mathit{C}}}_b^n{\mathit{\epsilon }}^b\end{array}$$

(13)

If we ignore small quantities and take the equivalent east gyro error as measurement, then the following equation can be obtained:

$$\left[\begin{array}{ccc}0& 1& 0\end{array}\right]\left({\tilde{\mathit{C}}}_b^n{\tilde{\mathit{\omega }}}_{ib}^b-{\mathit{\omega }}_{ie}^n\right)=\delta {\omega }_{ieE}^n=-{\omega }_{ieD}^n{\varphi }_N+{\omega }_{ieN}^n{\varphi }_D+C_{21}{\epsilon }_x^b+C_{22}{\epsilon }_y^b$$

(14)

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

$$\begin{array}{ll}{\mathit{z}}_2\left(t\right)& =\left[\begin{array}{c}{\mathit{z}}_1\left(t\right)\\ \delta {\omega }_{ieE}^n\end{array}\right]=\left[\begin{array}{ccccccc}{0}_{2\times 1}& 0_{2\times 1}& 0_{2\times 1}& {\mathit{I}}_{2\times 2}& 0_{2\times 1}& 0_{2\times 1}& 0_{2\times 2}\\ -{\omega }_{ieD}^n& 0& {\omega }_{ieN}^n& 0_{1\times 2}& C_{21}& C_{22}& 0_{1\times 2}\end{array}\right]\mathit{x}\left(t\right)+\left[\begin{array}{c}{\mathit{\upsilon }}_1\left(t\right)\\ {\upsilon }_{\omega E}\left(t\right)\end{array}\right]\\ & ={\mathit{H}}_2\left(t\right)\mathit{x}\left(t\right)+{\mathit{\upsilon }}_2\left(t\right),\hspace{1em}{\mathit{\upsilon }}_2\left(t\right)\sim N\left(0,{\mathit{R}}_2\right)\end{array}$$

(15)

where ${\mathit{R}}_2$ takes the following form:

$${\mathit{R}}_2=\left[\begin{array}{cc}{\mathit{R}}_1& 0_{2\times 1}\\ 0_{1\times 2}& r_{\omega E}^2\end{array}\right]$$

(16)

${\upsilon }_{\omega E}\left(t\right)$ represents the angular rate measurements noise about the east axis, and

$${\upsilon }_{\omega E}\left(t\right)\approx \left[\begin{array}{ccc}0& 1& 0\end{array}\right]\left({\mathit{\omega }}_{nb}^n+{\mathit{\omega }}_{en}^n\right)$$

(17)

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, ${\mathit{\omega }}_{nb}^n$ and ${\mathit{\omega }}_{en}^n$ are no longer zero, which lead to the changing of measurement noise ${\upsilon }_{\omega E}\left(t\right)$. Thus, the ARTP algorithm has limitations in these cases.

2.4. 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), the combined system model expands to 14 dimensions containing both error states in state A and state B. The system model can be described according to Equation (4):

$$\begin{array}{ll}\dot{\overline{\mathit{x}}}\left(t\right)& =\left[\begin{array}{ccc}\mathit{F}& 0_{5\times 5}& {\mathit{G}}_1\\ 0_{5\times 5}& \mathit{F}& {\mathit{G}}_2\\ 0_{4\times 5}& 0_{4\times 5}& 0_{4\times 4}\end{array}\right]\overline{\mathit{x}}\left(t\right)+\left[\begin{array}{c}{\mathit{G}}_1\mathit{w}\left(t_1\right)\\ {\mathit{G}}_2\mathit{w}\left(t_2\right)\\ 0_{1\times 4}\end{array}\right]\hspace{1em}\\ & =\overline{\mathit{A}}\overline{\mathit{x}}\left(t\right)+\left[\begin{array}{ccc}{\mathit{G}}_1& 0_{5\times 5}& 0_{5\times 4}\\ 0_{5\times 5}& {\mathit{G}}_2& 0_{5\times 4}\\ 0_{4\times 5}& 0_{4\times 5}& 0_{4\times 4}\end{array}\right]\left[\begin{array}{c}\overline{\mathit{w}}\left(t\right)\\ 0_{1\times 4}\end{array}\right]\hspace{1em}\overline{\mathit{w}}\left(t\right)\sim N\left(0,\overline{\mathit{Q}}\right)\end{array}$$

(18)

where $\overline{\mathit{x}}\left(t\right)={\left[\begin{array}{cccccccccccccc}{\varphi }_{NA}& {\varphi }_{EA}& {\varphi }_{DA}& \delta v_{NA}& \delta v_{EA}& {\varphi }_{NB}& {\varphi }_{EB}& {\varphi }_{DB}& \delta v_{NB}& \delta v_{EB}& {\epsilon }_x^b& {\epsilon }_y^b& {\nabla }_x^b& {\nabla }_y^b\end{array}\right]}^T$. The covariance matrix of $\overline{\mathit{w}}\left(t\right)$ is $\overline{\mathit{Q}}$, which is written as

$$\overline{\mathit{Q}}=\left[\begin{array}{cc}\mathit{Q}& 0_{5\times 5}\\ 0_{5\times 5}& \mathit{Q}\end{array}\right]$$

(19)

The matrixes ${\mathit{G}}_1$ and ${\mathit{G}}_2$ take the following form:

$${\mathit{G}}_1=\left[\begin{array}{cccc}-C_{11A}& -C_{12A}& 0& 0\\ -C_{21A}& -C_{22A}& 0& 0\\ -C_{31A}& -C_{32A}& 0& 0\\ 0& 0& C_{11A}& C_{12A}\\ 0& 0& C_{21A}& C_{22A}\end{array}\right],{\mathit{G}}_2=\left[\begin{array}{cccc}-C_{11B}& -C_{12B}& 0& 0\\ -C_{21B}& -C_{22B}& 0& 0\\ -C_{31B}& -C_{32B}& 0& 0\\ 0& 0& C_{11B}& C_{12B}\\ 0& 0& C_{21B}& C_{22B}\end{array}\right].$$

(20)

${\mathit{C}}_{bA}^n$ and ${\mathit{C}}_{bB}^n$ represent the attitude transformation matrix in state A and state B. $C_{ijA}\left(i,j=1,2,3\right)$ and $C_{ijB}\left(i,j=1,2,3\right)$ represent the element of attitude matrix ${\mathit{C}}_{bA}^n$ and ${\mathit{C}}_{bB}^n$, 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. ${\overline{\mathit{C}}}_{b_A}^n$ and ${\tilde{\mathit{C}}}_{b_B}^n$ are the computational values of ${\mathit{C}}_b^n$ in two states:

$${\tilde{\mathit{C}}}_{b_A}^n=\left[\mathit{I}-{\mathit{\varphi }}_A^n\times \right]{\mathit{C}}_{b_A}^n\hspace{1em}{\tilde{\mathit{C}}}_{b_B}^n=\left[\mathit{I}-{\mathit{\varphi }}_B^n\times \right]{\mathit{C}}_{b_B}^n$$

(21)

in which ${\mathit{\varphi }}_A^n=\left[\begin{array}{ccc}{\varphi }_{NA}& {\varphi }_{EA}& {\varphi }_{DA}\end{array}\right]$, ${\mathit{\varphi }}_B^n=\left[\begin{array}{ccc}{\varphi }_{NB}& {\varphi }_{EB}& {\varphi }_{DB}\end{array}\right]$. The relationship between ${\overline{\mathit{C}}}_{b_A}^n$ and ${\tilde{\mathit{C}}}_{b_B}^n$ can be written as

$$\begin{array}{l}{\mathit{C}}_{b_A}^{b_B}={\left({\mathit{C}}_{b_B}^n\right)}^T{\mathit{C}}_{b_A}^n={\left(\left[\mathit{I}+{\mathit{\varphi }}_B\times \right]{\tilde{\mathit{C}}}_{b_B}^n\right)}^T\left[\mathit{I}+{\mathit{\varphi }}_A\times \right]{\tilde{\mathit{C}}}_{b_A}^n\\ ={\tilde{\mathit{C}}}_n^{b_B}\left[\mathit{I}-{\mathit{\varphi }}_B\times \right]\left[\mathit{I}+{\mathit{\varphi }}_A\times \right]{\tilde{\mathit{C}}}_{b_A}^n\\ \approx {\tilde{\mathit{C}}}_n^{b_B}\left[\mathit{I}+\left({\mathit{\varphi }}_A-{\mathit{\varphi }}_B\right)\times \right]{\tilde{\mathit{C}}}_{b_A}^n\\ ={\tilde{\mathit{C}}}_{b_A}^{b_B}+\left[\begin{array}{ccc}{\tilde{c}}_{11B}& {\tilde{c}}_{21B}& {\tilde{c}}_{31B}\\ {\tilde{c}}_{12B}& {\tilde{c}}_{22B}& {\tilde{c}}_{32B}\\ {\tilde{c}}_{13B}& {\tilde{c}}_{23B}& {\tilde{c}}_{33B}\end{array}\right]\left[\begin{array}{ccc}0& -\left({\varphi }_{DA}-{\varphi }_{DB}\right)& \left({\varphi }_{EA}-{\varphi }_{EB}\right)\\ \left({\varphi }_{DA}-{\varphi }_{DB}\right)& 0& -\left({\varphi }_{NA}-{\varphi }_{NB}\right)\\ -\left({\varphi }_{EA}-{\varphi }_{EB}\right)& \left({\varphi }_{NA}-{\varphi }_{NB}\right)& 0\end{array}\right]\left[\begin{array}{ccc}{\tilde{c}}_{11A}& {\tilde{c}}_{12A}& {\tilde{c}}_{13A}\\ {\tilde{c}}_{21A}& {\tilde{c}}_{22A}& {\tilde{c}}_{23A}\\ {\tilde{c}}_{31A}& {\tilde{c}}_{32A}& {\tilde{c}}_{33A}\end{array}\right]\end{array}$$

(22)

Suppose the horizontal Euler angles between state A and state B are $\Delta {\gamma }_{AB}$ and $\Delta {\theta }_{AB}$, while the azimuth change is $\pi +\Delta {\phi }_{AB}$. $\Delta {\gamma }_{AB}$, $\Delta {\theta }_{AB}$ and $\Delta {\phi }_{AB}$ are seen as unknown small angles. Thus,

$$\begin{array}{ll}{\mathit{C}}_{b_A}^{b_B}& \approx \left[\begin{array}{ccc}1& 0& \Delta {\theta }_{AB}\\ 0& 1& -\Delta {\gamma }_{AB}\\ -\Delta {\theta }_{AB}& \Delta {\gamma }_{AB}& 1\end{array}\right]\left[\begin{array}{ccc}\cos \left(\pi +\Delta {\phi }_{AB}\right)& \sin \left(\pi +\Delta {\phi }_{AB}\right)& 0\\ -\sin \left(\pi +\Delta {\phi }_{AB}\right)& \cos \left(\pi +\Delta {\phi }_{AB}\right)& 0\\ 0& 0& 1\end{array}\right]\\ & =\left[\begin{array}{ccc}\cos \left(\pi +\Delta {\phi }_{AB}\right)& \sin \left(\pi +\Delta {\phi }_{AB}\right)& \Delta {\theta }_{AB}\\ -\sin \left(\pi +\Delta {\phi }_{AB}\right)& \cos \left(\pi +\Delta {\phi }_{AB}\right)& -\Delta {\gamma }_{AB}\\ -\Delta {\theta }_{AB}\cos \left(\pi +\Delta {\phi }_{AB}\right)-\Delta {\gamma }_{AB}\sin \left(\pi +\Delta {\phi }_{AB}\right)& -\Delta {\theta }_{AB}\sin \left(\pi +\Delta {\phi }_{AB}\right)+\Delta {\gamma }_{AB}\cos \left(\pi +\Delta {\phi }_{AB}\right)& 1\end{array}\right]\\ & \approx \left[\begin{array}{ccc}-1& -\Delta {\phi }_{AB}& \Delta {\theta }_{AB}\\ \Delta {\phi }_{AB}& -1& -\Delta {\gamma }_{AB}\\ \Delta {\theta }_{AB}& -\Delta {\gamma }_{AB}& 1\end{array}\right]\end{array}$$

(23)

${\tilde{\mathit{C}}}_{b_A}^{b_B}$ can be rewritten as

$${\tilde{\mathit{C}}}_n^{b_B}{\tilde{\mathit{C}}}_{b_A}^n={\tilde{\mathit{C}}}_{b_A}^{b_B}=\left[\begin{array}{ccc}{\tilde{c}}_{11AB}& {\tilde{c}}_{12AB}& {\tilde{c}}_{13AB}\\ {\tilde{c}}_{21AB}& {\tilde{c}}_{22AB}& {\tilde{c}}_{23AB}\\ {\tilde{c}}_{31AB}& {\tilde{c}}_{32AB}& {\tilde{c}}_{33AB}\end{array}\right].$$

(24)

Taking ${\tilde{c}}_{21AB}$ as the measurement, we then have

$$\begin{array}{ll}{\tilde{c}}_{21AB}& =-{\tilde{c}}_{11A}\left[{\tilde{c}}_{22B}\left({\varphi }_{DA}-{\varphi }_{DB}\right)-{\tilde{c}}_{32B}\left({\varphi }_{EA}-{\varphi }_{EB}\right)\right]\\ & -{\tilde{c}}_{21A}\left[-{\tilde{c}}_{12B}\left({\varphi }_{DA}-{\varphi }_{DB}\right)+{\tilde{c}}_{32B}\left({\varphi }_{NA}-{\varphi }_{NB}\right)\right]\\ & -{\tilde{c}}_{31A}\left[{\tilde{c}}_{12B}\left({\varphi }_{EA}-{\varphi }_{EB}\right)-{\tilde{c}}_{22B}\left({\varphi }_{NA}-{\varphi }_{NB}\right)\right]+\Delta {\phi }_{AB}\\ & =\left({\tilde{c}}_{21A}{\tilde{c}}_{12B}-{\tilde{c}}_{11A}{\tilde{c}}_{22B}\right)\left({\varphi }_{DA}-{\varphi }_{DB}\right)+\left({\tilde{c}}_{11A}{\tilde{c}}_{32B}-{\tilde{c}}_{31A}{\tilde{c}}_{12B}\right)\left({\varphi }_{EA}-{\varphi }_{EB}\right)+\\ & \left({\tilde{c}}_{31A}{\tilde{c}}_{22B}-{\tilde{c}}_{21A}{\tilde{c}}_{32B}\right)\left({\varphi }_{NA}-{\varphi }_{NB}\right)+\Delta {\phi }_{AB}\end{array}$$

(25)

Hence, the modified observation model is given by

$$\begin{array}{ll}\overline{\mathit{z}}\left(t\right)& ={\left[\begin{array}{ccccc}\delta v_{NA}& \delta v_{EA}& \delta v_{NB}& \delta v_{EB}& {\tilde{c}}_{21AB}\end{array}\right]}^T\\ & =\left[\begin{array}{ccccc}{0}_{2\times 3}& {\mathit{I}}_{2\times 2}^{}& 0_{2\times 3}& 0_{2\times 2}& 0_{2\times 4}\\ 0_{2\times 3}& 0_{2\times 2}& 0_{2\times 3}& {\mathit{I}}_{2\times 2}^{}& 0_{2\times 4}\\ {\mathit{H}}_{1\times 3}^1& 0_{1\times 2}& -{\mathit{H}}_{1\times 3}^1& 0_{1\times 2}& 0_{1\times 4}\end{array}\right]\overline{\mathit{X}}\left(t\right)+\left[\begin{array}{c}{\mathit{\upsilon }}_1\left(t\right)\\ \Delta {\phi }_{AB}\left(t\right)\end{array}\right]\\ & =\overline{\mathit{H}}\left(t\right)\overline{\mathit{X}}\left(t\right)+\overline{\mathit{\upsilon }}\left(t\right),\hspace{1em}\overline{\mathit{\upsilon }}\left(t\right)\sim N\left(0,\overline{\mathit{R}}\right)\end{array}$$

(26)

where ${\mathit{H}}_{1\times 3}^1$ is expressed as

$${\mathit{H}}_{1\times 3}^1=\left[\begin{array}{ccc}{\tilde{c}}_{31A}{\tilde{c}}_{22B}-{\tilde{c}}_{21A}{\tilde{c}}_{32B}& {\tilde{c}}_{11A}{\tilde{c}}_{32B}-{\tilde{c}}_{31A}{\tilde{c}}_{12B}& {\tilde{c}}_{21A}{\tilde{c}}_{12B}-{\tilde{c}}_{11A}{\tilde{c}}_{22B}\end{array}\right].$$

(27)

The observation noise covariance matrix $\overline{\mathit{R}}$ takes the following form:

$$\overline{\mathit{R}}=\left[\begin{array}{ccc}{\mathit{R}}_1& 0_{2\times 2}& 0_{2\times 1}\\ 0_{2\times 2}& {\mathit{R}}_1& 0_{2\times 1}\\ 0_{1\times 2}& 0_{1\times 2}& r_{\phi }^2\end{array}\right].$$

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

2.5. 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 $\mathit{P}\left(t\right)$ in the conventional TP and ARTP algorithms, which is denoted as ${\mathit{P}}_{\left(3,3\right)}\left(t\right)$, or the eighth element diagonal of the covariance matrix $\mathit{P}\left(t\right)$ in the new RATP algorithm, which is denoted as ${\mathit{P}}_{\left(8,8\right)}\left(t\right)$, 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 ${\mathit{P}}_{\left(3,3\right)}\left(t\right)$ or ${\mathit{P}}_{\left(8,8\right)}\left(t\right)$ among the three alignment schemes. The simulation parameters are set in

Table 1. Parameters for simulation.

$\mathbf{Local}\mathbf{Latitude}\mathit{L}$	28.21°
Bias instability of gyros Constant bias of gyros	${0.003}^{\circ }/\mathrm{h}$ ${0.015}^{\circ }/\mathrm{h}$
Angular random walk of gyros	${0.0005}^{\circ }/\sqrt{\mathrm{h}}$
Bias instability of accelerometers Constant bias of accelerometers	20 µg 100 µg
Noise power spectrum density of accelerometers	$20\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{H}\mathrm{z}}$
Process noise covariance parameters in $\mathit{Q},\overline{\mathit{Q}}$	$n_a=20\mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{H}\mathrm{z}}$, $n_g={0.0005}^{\circ }/\sqrt{\mathrm{h}}$
Measurement noise covariance parameters of TP in ${\mathit{R}}_1$	$r_v=0.01\mathrm{m}/\mathrm{s}$
Measurement noise covariance parameters of ARTP in ${\mathit{R}}_2$	$r_v=0.01\mathrm{m}/\mathrm{s}$, $r_g={0.01}^{\circ }/\mathrm{s}$ (Con.1), $r_g={0.1}^{\circ }/\mathrm{s}$ (Con.2), $r_g={0.2}^{\circ }/\mathrm{s}$ (Con.3)
Measurement noise covariance parameters of RATP in $\overline{\mathit{R}}$	$r_v=0.01\mathrm{m}/\mathrm{s}$, $r_{\phi }={0.012}^{\circ }$ (Con.1), $r_{\phi }={0.018}^{\circ }$ (Con.2) $r_{\phi }={0.024}^{\circ }$ (Con.3)
Initial error covariance parameters of TP and ARTP in ${\mathit{P}}_{10}^{},{\mathit{P}}_{20}^{}$	${\mathit{P}}_{10}^{}={\mathit{P}}_{20}^{}=diag\left(\begin{array}{l}{\left({0.1}^{\circ }\right)}^2,{\left({0.1}^{\circ }\right)}^2,{\left({0.5}^{\circ }\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,\\ {\left(0.015^{\circ }/\mathrm{h}\right)}^2,{\left(0.015^{\circ }/\mathrm{h}\right)}^2,{\left(100\mathsf{\mu }\mathrm{g}\right)}^2,{\left(100\mathsf{\mu }\mathrm{g}\right)}^2\end{array}\right)$
Initial error covariance parameters of RATP in ${\mathit{P}}_{30}^{}$	${\mathit{P}}_{30}^{}=diag\left(\begin{array}{l}{\left({0.1}^{\circ }\right)}^2,{\left({0.1}^{\circ }\right)}^2,{\left({0.5}^{\circ }\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,\\ {\left(0.1^{\circ }\right)}^2,{\left(0.1^{\circ }\right)}^2,{\left(0.5^{\circ }\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,{\left(0.01\mathrm{m}/\mathrm{s}\right)}^2,\\ {\left(0.015^{\circ }/\mathrm{h}\right)}^2,{\left(0.015^{\circ }/\mathrm{h}\right)}^2,{\left(100\mathsf{\mu }\mathrm{g}\right)}^2,{\left(100\mathsf{\mu }\mathrm{g}\right)}^2\end{array}\right)$

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-\sigma$ 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. Performance comparison of three alignment schemes.

Condition	$1-\mathit{\sigma }\mathbf{Final}\mathbf{Azimuth}\mathbf{Error}\mathbf{Value}\mathbf{in}180\mathbf{s}\left(\mathbf{arcmin}\right)$			$1-\mathit{\sigma }\mathbf{Final}\mathbf{Azimuth}\mathbf{Error}\mathbf{Value}\mathbf{in}300\mathbf{s}\left(\mathbf{arcmin}\right)$
	TP	ARTP	RATP	TP	ARTP	RATP
Con.1	1.51	1.01	0.72	0.54	0.50	0.40
Con.2	2.06	2.55	0.96	0.67	1.81	0.58
Con.3	2.61	4.01	1.21	0.80	2.93	0.78

and from Figure 1, Figure 2 and Figure 3. It is shown that the $1-\sigma$ 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 Figure 1, Figure 2 and Figure 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.

3. Results and Discussion

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}^{\circ }$ to state B. The experimental environment is shown in Figure 4. The main parameters of the inertial devices are shown in

Table 3. Main parameters of inertial devices.

Laser gyro	Zero-bias stability	${0.003}^{\circ }/\mathrm{h}(1\sigma )$
	Angle random walk	${0.0005}^{\circ }/\sqrt{\mathrm{h}}\left(1\sigma \right)$
Quartz pendulous accelerometer	Zero-bias stability	$20 \mathsf{\mu }\mathrm{g}\left(1\sigma \right)$
	Noise power spectrum density	$20 \mathsf{\mu }\mathrm{g}/\sqrt{\mathrm{H}\mathrm{z}}\left(1\sigma \right)$
	Sampling frequency	$500 \mathrm{H}\mathrm{z}$

.

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.

The azimuth errors of the three schemes for 180 s rapid alignment are shown in

Table 4. Azimuth error under different conditions for 180 s rapid alignment.

Num	TP (arcsec)			ARTP (arcsec)			RATP (arcsec)
	Con.1	Con.2	Con.3	Con.1	Con.2	Con.3	Con.1	Con.2	Con.3
1	−110.26	−199.00	272.94	−95.08	−52.74	−94.134	−1.76	45.31	144.47
2	−38.26	56.85	−183.61	9.02	−148.55	51.26	34.64	10.83	−69.82
3	110.46	−24.70	34.60	7.41	78.65	59.75	3.39	−58.99	14.40
4	−70.55	237.23	68.07	−59.90	−334.08	−66.70	−40.21	−53.06	90.46
5	6.14	−73.86	−254.07	35.43	217.33	−132.19	−59.04	12.49	−127.33
6	102.48	3.48	62.07	103.11	239.40	182.01	62.99	43.42	−52.17
7	−96.24	−18.10	−124.30	−4.95	−7.66	−476.15	−8.63	−20.32	−50.61
8	−84.72	−91.60	−20.84	−85.77	−157.55	−14.43	−78.16	−85.72	65.65
9	18.52	100.76	−39.40	29.03	−83.69	−191.20	28.56	37.15	−107.95
10	104.79	130.65	112.16	81.13	101.39	508.14	44.86	77.35	85.82
11	−55.33	−97.28	91.85	−45.65	−54.37	−142.47	−22.21	−40.75	−50.62
12	124.49	−24.43	−19.46	26.22	201.87	−316.57	35.58	32.29	57.71
STD value	89.26	117.23	141.83	61.96	174.01	254.55	43.75	50.75	87.83

, and Figure 7, Figure 8 and Figure 9. The $1-\sigma$ values of the final azimuth error of TP at 180 s are 89.26, 117.23, and 141.83 arcsec in the different experimental conditions. The $1-\sigma$ values of the final azimuth error of ARTP at 180 s are 61.96, 124.01, and 254.55 arcsec, respectively. The convergence rate of RATP is better than that of TP under static conditions, and its performance decreases obviously compared with TP when angular vibration increases. As shown in Table 4 and Figure 8, the $1-\sigma$ values of final 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.

4. 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.