1 Introduction

Strapdown inertial navigation system (SINS) has been widely used in aerospace, military, industrial and consumer fields because of its self-containment, anti-jamming capability, high sampling rate and good concealment [1]. In recent years, the application of SINS in vehicles has attracted increasing attention. The initial alignment of the SINS is one of the key technologies that affect the accuracy of vehicle navigation, and the accuracy and speed of the initial alignment directly affect the accuracy of the SINS, so the initial alignment of SINS has been a hot research topic.

To implement the in-motion initial alignment procedure efficiently, much effort has been devoted to investigating the novel alignment methods. An optimization-based alignment (OBA) method was proposed for GPS-aided high-accuracy SINS [2]. In the OBA method, the attitude matrix is decomposed into two time-varying attitude matrices and a constant attitude matrix. The two time-varying attitude matrices are calculated by the body angular rate and the navigation angular rate, respectively. Finally, the constant attitude matrix is obtained based on the constructed vector observations using Davenport’s q-method [3]. However, most of the existing OBA methods do not take the IMU bias into account, which will affect the accuracy of the constructed vector observations. Therefore, the OBA method is not applicable to low-cost SINS. For large initial alignment errors, the unscented-transformation-based unscented Kalman filter (UKF) is used for in-motion initial alignment because of its easy implementation, moderate computational cost and appropriate performance [4]. The UKF in its quaternion application form is proposed to avoid the singularity problem and the norm constraint of the quaternion in practical applications called the unscented quaternion estimator (USQUE) [5]. The USQUE is approved as a method that can replace the OBA method due to its capability of estimating other parameters other than the attitude and handing the noise in the model. USQUE converges slowly with large unknown initial attitude error. As a variant of UKF, it lacks the adaptive ability to system noise and easily leads to a non-positive definite covariance matrix.

Considering that the velocity measurement accuracy of the odometer and DVL is not high enough, our research group has proposed a variety of laser Doppler velocimetry (LDV) structures for vehicle velocity measurement in integrated navigation [6, 7]. As a new type of velocity sensor, LDV has the advantages of high accuracy, rapid dynamic response, non-contact measurement, good directional sensitivity, complete autonomy and good spatial resolution [8]. At present, there are few reports about the application of LDV in the field of navigation. In this paper, a new robust in-motion alignment method, named robust square-root unscented quaternion estimator (RSRUSQUE), is proposed to improve the alignment accuracy and speed.

2 In-Motion Alignment for SINS/LDV

In this paper, the local-level navigation frame is denoted as the n frame, the vehicle body frame is denoted as the b frame, the inertial nonrotating frame is denoted as the i frame and the earth frame is denoted as the e frame.

The velocity kinematic equation in the n frame is given by

$$ \dot{\user2{\upsilon }}^{n} = {\varvec{C}}_{b}^{n} {\varvec{f}}^{b} - \left( {2{\varvec{\omega}}_{ie}^{n} + {\varvec{\omega}}_{en}^{n} } \right) \times {\varvec{\upsilon}}^{n} + {\varvec{g}}^{n} $$
(31.1)

where \({\varvec{\upsilon}}^{n} = \left[ {\begin{array}{*{20}c} {\upsilon_{E}^{n} } & {\upsilon_{N}^{n} } & {\upsilon_{U}^{n} } \\ \end{array} } \right]^{T}\) is the ground velocity in n frame, \({\varvec{f}}^{b}\) is the specific force in b frame, \({\varvec{g}}^{n}\) is the gravity vector in n frame, \(\left( \cdot \right) \times\) means to solve the antisymmetric matrix, \({\varvec{\omega}}_{ie}^{n}\) is the earth rotation rate with respect to the i frame and \({\varvec{\omega}}_{en}^{n}\) is the navigational rotating rate in n frame relative to e frame.

According to the coordinate transformation method, it has

$$ \dot{\user2{\upsilon }}^{n} = \left( {{\varvec{C}}_{b}^{n} {\varvec{\upsilon}}^{b} } \right)^{\prime } = {\varvec{C}}_{b}^{n} \left( {\dot{\user2{\upsilon }}^{b} + {\varvec{\omega}}_{nb}^{b} \times {\varvec{\upsilon}}^{b} } \right) $$
(31.2)

where \({\varvec{\omega}}_{nb}^{b}\) is body angular rate with respect to the n frame.

Substituting (31.4) into (31.1) yields

$$ {\varvec{C}}_{b}^{n} \left( {\dot{\user2{\upsilon }}^{b} + \left( {{\varvec{\omega}}_{ib}^{b} + {\varvec{\omega}}_{ie}^{b} } \right) \times {\varvec{\upsilon}}^{b} - {\varvec{f}}^{b} } \right) = {\varvec{g}}^{n} $$
(31.3)

2.1 Process Model

Considering the velocity provided by LDV is accurate, it is necessary to incorporate it into the process model and measurement model. In order to achieve this, denote the initial b frame as an inertial frame. The attitude matrix \({\varvec{C}}_{b}^{n} \left( t \right)\) and attitude update equations can be written as

$$ {\varvec{C}}_{b}^{n} \left( t \right) = {\varvec{C}}_{i}^{n\left( t \right)} {\varvec{C}}_{b\left( t \right)}^{i} $$
(31.4)
$$ \dot{\user2{C}}_{b\left( t \right)}^{i} = {\varvec{C}}_{b\left( t \right)}^{i} \left( {{\varvec{\omega}}_{ib}^{b} \times } \right) $$
(31.5)
$$ \dot{\user2{C}}_{n\left( t \right)}^{i} = {\varvec{C}}_{n\left( t \right)}^{i} \left( {{\varvec{\omega}}_{in}^{n} \times } \right) $$
(31.6)

where

$$ {\varvec{\omega}}_{in}^{n} = {\varvec{\omega}}_{ie}^{n} + {\varvec{\omega}}_{en}^{n} $$
(31.7)

Equation (31.6) is the process model of the proposed method.

2.2 Measurement Model

Substituting (31.4) into (31.3) and multiplying \({\varvec{C}}_{n\left( t \right)}^{i}\) on both sides, we get

$$ {\varvec{C}}_{b\left( t \right)}^{i} \left( {\dot{\user2{\upsilon }}^{b} + \left( {{\varvec{\omega}}_{ie}^{b} + {\varvec{\omega}}_{ib}^{b} } \right) \times {\varvec{\upsilon}}^{b} - {\varvec{f}}^{b} } \right) = {\varvec{C}}_{n\left( t \right)}^{i} {\varvec{g}}^{n} $$
(31.8)

Integrating by the time interval of interest on both sides of (31.8), the following can be obtained:

$$ \begin{gathered} {\varvec{C}}_{b\left( t \right)}^{i} {\varvec{\upsilon}}^{b} \left( t \right) - {\varvec{\upsilon}}^{b} \left( 0 \right) + \int_{0}^{t} {{\varvec{C}}_{b\left( \tau \right)}^{i} } \left( {{\varvec{\omega}}_{ie}^{b} \times {\varvec{\upsilon}}^{b} } \right)d\tau - \int_{0}^{t} {{\varvec{C}}_{b\left( \tau \right)}^{i} {\varvec{f}}^{b} d\tau } \hfill \\ = {\varvec{C}}_{n\left( t \right)}^{i} {\varvec{C}}_{n\left( 0 \right)}^{n\left( t \right)} \int_{0}^{t} {{\varvec{C}}_{n\left( \tau \right)}^{n\left( 0 \right)} {\varvec{g}}^{n} d\tau } \hfill \\ \end{gathered} $$
(31.9)

Define two vectors as

$$ \left\{ \begin{gathered} {\varvec{\alpha}}\left( t \right) = {\varvec{C}}_{b\left( t \right)}^{i} {\varvec{\upsilon}}^{b} \left( t \right) - {\varvec{\upsilon}}^{b} \left( 0 \right) + \int_{0}^{t} {{\varvec{C}}_{b\left( \tau \right)}^{i} } \left( {{\varvec{\omega}}_{ie}^{b} \times {\varvec{\upsilon}}^{b} } \right)d\tau - \int_{0}^{t} {{\varvec{C}}_{b\left( \tau \right)}^{i} {\varvec{f}}^{b} d\tau } \hfill \\ {\varvec{\beta}}\left( t \right) = {\varvec{C}}_{n\left( 0 \right)}^{n\left( t \right)} \int_{0}^{t} {{\varvec{C}}_{n\left( \tau \right)}^{n\left( 0 \right)} {\varvec{g}}^{n} d\tau } \hfill \\ \end{gathered} \right. $$
(31.10)

The measurement model can be given by

$$ {\varvec{\alpha}}\left( t \right) = {\varvec{C}}_{n\left( t \right)}^{i} {\varvec{\beta}}\left( t \right) $$
(31.11)

3 Proposed Robust Square-Root Unscented Quaternion Estimator Algorithm

In this section, an RSRUSQUE algorithm based on the square-root form of an unscented quaternion estimator with adaptive measurement noise covariance matrix is proposed for vehicle in-motion initial alignment.

The direct implementation of a UKF with a quaternions-based state is not suitable because the quaternion estimate is determined using the weighted quaternions averaging operation. Thus, no guarantees can be made that the quaternion will have a unit norm. To represent an attitude error quaternion preserving the constraint of quaternion propagation, the quaternion is used for attitude propagation, and the unconstrained three-component vectors of generalized Rodrigues parameters are used for filtering and local attitude error representation.

Denote the error of the attitude error quaternion by \(\delta {\varvec{q}} = \left[ {\delta q_{0} ,\delta {\varvec{q}}_{1:3}^{T} } \right]^{T} = \left[ {\delta q_{0} ,\delta {\varvec{\rho}}^{T} } \right]^{T} ,\) and the corresponding GRP representation \(\delta \Re\) is given by

$$ \delta {\mathbf{\Re }} = f\frac{{\delta {\varvec{\rho}}}}{{a + \delta q_{0} }} $$
(31.12)

where a is a parameter from 0 to 1 and f is a scale factor. The GRP is used to place the singularity of the attitude representation in a certain angle range, and different combinations of a and f have different physical meanings. For example, when \(a = 0\) and \(f = 1\), (31.12) gives the Gibbs vector, and when \(a = f = 1\), (31.12) gives the standard vector of modified Rodrigues parameters.

The inverse transformation from \(\delta {\mathbf{\Re }}\) to \(\delta {\varvec{q}}\) is given by

$$ \left\{ \begin{gathered} \delta q_{0} = \frac{{ - a\left\| {\delta {\mathbf{\Re }}} \right\|^{2} + f\sqrt {f^{2} + \left( {1 - a} \right)^{2} \left\| {\delta {\mathbf{\Re }}} \right\|^{2} } }}{{f^{2} + \left\| {\delta {\mathbf{\Re }}} \right\|^{2} }} \hfill \\ \delta {\varvec{\rho}} = f^{ - 1} \left( {a + \delta q_{0} } \right)\delta {\mathbf{\Re }} \hfill \\ \end{gathered} \right. $$
(31.13)

In RSRUSQUE, the filtering state is defined as

$$ \hat{\user2{X}}_{k} = \left[ {\begin{array}{*{20}c} {\delta {\mathbf{\Re }}_{k}^{T} } & {\hat{\user2{X}}_{k}^{eT} } \\ \end{array} } \right]^{T} $$
(31.14)

where \(\hat{\user2{X}}_{k}^{e}\) are the components of the state besides the quaternion. In this paper, considering the high precision SINS and LDV adopted and the short alignment time, the other parameters are not estimated to reduce the calculation amount.

Write the measurement model (31.13) as follows:

$$ {\varvec{y}}_{k} = {\varvec{\alpha}}\left( k \right) - {\varvec{C}}_{n\left( k \right)}^{i} {\varvec{\beta}}\left( k \right) = {\varvec{h}}\left( {{\varvec{X}}_{k} } \right) + {\varvec{v}}_{k} $$
(31.15)

where \({\varvec{X}}_{k}\) is the ideal value of the filtering state at time instant k, and \({\varvec{v}}_{k}\) is the measurement noise.

The state estimation at time instant k is (31.14), and the corresponding covariance is \({\varvec{P}}_{x,k}\). The RSRUSQUE algorithm for initial alignment is described as follows.

3.1 Time Update

The generated sigma points and the weights corresponding to the expectation and covariance matrix are given by

$$ {\varvec{\chi}}_{k} \left( i \right) = \left\{ \begin{gathered} \hat{\user2{X}}_{k} ,\quad i = 0 \hfill \\ \hat{\user2{X}}_{k} + \left[ {\sqrt {\left( {n + \lambda } \right)} {\varvec{S}}_{x,k} } \right]_{i} ,\quad i = 1,\;2, \ldots ,\;n \hfill \\ \hat{\user2{X}}_{k} - \left[ {\sqrt {\left( {n + \lambda } \right)} {\varvec{S}}_{x,k} } \right]_{i} ,\quad i = n + 1,\;n + 2, \ldots ,\;2n \hfill \\ \end{gathered} \right. $$
(31.16)
$$ W^{m} \left( i \right) = \left\{ \begin{gathered} \frac{\lambda }{n + \lambda },\quad i = 0 \hfill \\ \frac{1}{{2\left( {n + \lambda } \right)}},\quad i = 1,\; \ldots ,\;2n \hfill \\ \end{gathered} \right. $$
(31.17)
$$ W^{c} \left( i \right) = \left\{ \begin{gathered} \frac{\lambda }{n + \lambda } + 1 - \sigma^{2} + \varsigma ,\quad i = 0 \hfill \\ \frac{1}{{2\left( {n + \lambda } \right)}},\quad i = 1,\; \ldots ,\;2n \hfill \\ \end{gathered} \right. $$
(31.18)

where \({\varvec{S}}_{x,k}\) is the lower triangular matrix obtained by using the Cholesky factorization of \({\varvec{P}}_{x,k}\). \(n\) is the dimension of the state \(\hat{\user2{X}}_{k}\), \(\lambda = \sigma^{2} \left( {n + \kappa } \right) - n\). \(\sigma\) is a scale factor, usually a small quantity greater than zero, \(\kappa\) is a tuning parameter which is usually set to 0 and 3 − n to capture some higher-order information of the distribution, \(\varsigma\) is used to incorporate prior information on the probability density function of the states.

\({\varvec{\chi}}_{k} \left( i \right)\) can be divided as follows:

$$ {\varvec{\chi}}_{k} \left( i \right) = \left[ {\begin{array}{*{20}c} {{\varvec{\chi}}_{k}^{\delta \Re } \left( i \right)^{T} } & {{\varvec{\chi}}_{k}^{e} \left( i \right)^{T} } \\ \end{array} } \right]^{T} $$
(31.19)

The quaternion error corresponding to \({\varvec{\chi}}_{k}^{\delta \Re } \left( i \right)\) is given by

$$ {\varvec{\chi}}_{k}^{\delta q} \left( i \right) = \left[ {\begin{array}{*{20}c} {\delta q_{k,0} \left( i \right)} & {\delta {\varvec{\rho}}_{k} \left( i \right)^{T} } \\ \end{array} } \right]^{T} $$
(31.20)

which can be calculated by (31.13).

Denote the quaternion-based sigma points by multiplying the error quaternion by the current attitude quaternion

$$ {\varvec{\chi}}_{k}^{q} \left( i \right) = {\varvec{\chi}}_{k}^{\delta q} \left( i \right) \otimes {\varvec{q}}_{k} $$
(31.21)

The sigma points obtained by (31.21) propagate forward through process model (31.6), and the propagated quaternion error is calculated by the following equation:

$$ {\varvec{\chi}}_{k + 1\left| k \right.}^{\delta q} \left( i \right) = {\varvec{\chi}}_{k + 1\left| k \right.}^{q} \left( i \right) \otimes \left[ {{\varvec{\chi}}_{k + 1\left| k \right.}^{q} } \right]^{ - 1} $$
(31.22)

The predicted GRP sigma points \({\varvec{\chi}}_{k + 1\left| k \right.}^{\delta \Re } \left( i \right)\) corresponding to \({\varvec{\chi}}_{k + 1|k}^{\delta q} \left( i \right)\) can be calculated by (31.12). The propagated sigma points of the state can be determined as

$$ {\varvec{\chi}}_{k + 1|k\left( i \right)} = \left[ {\begin{array}{*{20}c} {{\varvec{\chi}}_{k + 1|k}^{\delta \Re } \left( i \right)^{T} } & {{\varvec{\chi}}_{k + 1|k}^{e} \left( i \right)^{T} } \\ \end{array} } \right]^{T} $$
(31.23)

The corresponding state prediction and covariance matrix can be calculated, respectively, as

$$ \hat{\user2{X}}_{k + 1\left| k \right.} = \sum\limits_{i = 0}^{2n} {W^{m} } \left( i \right){\varvec{\chi}}_{k + 1\left| k \right.} \left( i \right) $$
(31.24)
$$ {\varvec{S}}_{x,k + 1|k} = qr\left\{ {\left[ {\begin{array}{*{20}c} {\sqrt {W^{c} \left( {1:2n} \right)} \left( {{\varvec{\chi}}_{k + 1\left| k \right.} \left( {1:2n} \right) - \hat{\user2{X}}_{k + 1\left| k \right.} } \right)} & {\sqrt {{\varvec{Q}}_{k} } } \\ \end{array} } \right]^{T} } \right\} $$
(31.25)
$$ \begin{gathered} {\varvec{S}}_{x,k + 1|k} = cholupdate\left\{ {{\varvec{S}}_{x,k + 1|k} ,\sqrt {\left| {W^{c} \left( 0 \right)} \right|} \left( {{\varvec{\chi}}_{k + 1\left| k \right.} \left( 0 \right) - \hat{\user2{X}}_{k + 1\left| k \right.} } \right),} \right. \hfill \\ \quad \;\;\quad \;\;\;\left. {{\text{sgn}} \left( {W^{c} \left( 0 \right)} \right)} \right\} \hfill \\ \end{gathered} $$
(31.26)

3.2 Measurement Update

Similar to (31.16) and (31.19)–(31.21), the new quaternion-based sigma points \({\varvec{\chi}}_{k + 1\left| k \right.}^{ * q} \left( i \right)\) can be obtained, and define a new set of sigma points as

$$ {\varvec{\gamma}}_{k + 1\left| k \right.}^{ * } \left( i \right) = \left[ {\begin{array}{*{20}c} {{\varvec{\chi}}_{k + 1\left| k \right.}^{ * q} \left( i \right)^{T} } & {{\varvec{\chi}}_{k + 1|k}^{ * e} \left( i \right)^{T} } \\ \end{array} } \right]^{T} $$
(31.27)

The sigma points in (31.27) are propagated directly through the measurement model (31.15), and the predicted sigma points are denoted as \({\varvec{Z}}_{k + 1\left| k \right.} \left( i \right)\).

The predicted mean of measurement, the covariance matrix of measurement and the cross-covariance matrix of the state and measurement are calculated, respectively, as

$$ \hat{\user2{Y}}_{k + 1} = \sum\limits_{i = 0}^{2n} {W^{m} } \left( i \right){\varvec{Z}}_{k + 1\left| k \right.} \left( i \right) $$
(31.28)
$$ {\varvec{S}}_{y,k + 1} = qr\left\{ {\left[ {\begin{array}{*{20}c} {\sqrt {W^{c} \left( {1:2n} \right)} \left( {{\varvec{Z}}_{k + 1\left| k \right.} \left( {1:2n} \right) - \hat{\user2{Y}}_{k + 1} } \right)} & {\sqrt {{\varvec{R}}_{k} } } \\ \end{array} } \right]^{T} } \right\} $$
(31.29)
$$ \begin{gathered} {\varvec{S}}_{y,k + 1} = cholupdate\left\{ {{\varvec{S}}_{z,k + 1} ,\sqrt {\left| {W^{c} \left( 0 \right)} \right|} \left( {{\varvec{Z}}_{k + 1|k} \left( 0 \right) - \hat{\user2{Y}}_{k + 1} } \right),} \right. \hfill \\ \left. {\quad \;\;\quad \;\;\;{\text{sgn}} \left( {W^{c} \left( 0 \right)} \right)} \right\} \hfill \\ \end{gathered} $$
(31.30)
$$ {\varvec{P}}_{xy,k + 1} = \sum\limits_{i = 0}^{2n} {W^{c} } \left( i \right)\left\{ {\left( {{\varvec{\chi}}_{k + 1\left| k \right.}^{ * } \left( i \right) - \hat{\user2{X}}_{k + 1\left| k \right.} } \right)} \right.\left. { \times \left( {{\varvec{Z}}_{k + 1\left| k \right.} \left( i \right) - \hat{\user2{Y}}_{k + 1} } \right)^{T} } \right\} $$
(31.31)

The innovation vector is

$$ {\varvec{e}}_{k + 1} = {\varvec{y}}_{k + 1} - \hat{\user2{Y}}_{k + 1} $$
(31.32)

The measurement noise covariance matrix \({\varvec{R}}_{k + 1}\) can be expressed as

$$ {\varvec{R}}_{k + 1} = (1 - \eta_{k + 1} ){\varvec{R}}_{k} + \eta_{k + 1} \left( {{\varvec{e}}_{k + 1} {\varvec{e}}_{k + 1}^{T} - {\varvec{S}}_{z,k + 1} {\varvec{S}}_{z,k + 1}^{T} + {\varvec{R}}_{k} } \right) $$
(31.33)
$$ \eta_{k + 1} = \frac{{\eta_{k} }}{{\eta_{k} + c}} $$
(31.34)

where \(\eta_{0} = 1\), and \(0 < c < 1\) is called a fading factor, which is usually \(c = 0.9 - 0.999\).

Using (31.29)–(31.31), the update equations of the state vector and covariance matrix are determined by

$$ \hat{\user2{X}}_{k + 1} = \hat{\user2{X}}_{k + 1|k} + {\varvec{P}}_{xy,k + 1} \left( {{\varvec{S}}_{y,k + 1} {\varvec{S}}_{y,k + 1}^{T} } \right)^{ - 1} {\varvec{e}}_{k + 1} $$
(31.35)
$$ {\varvec{U}}_{k + 1} = {\varvec{P}}_{xy,k + 1} \left( {{\varvec{S}}_{y,k + 1} {\varvec{S}}_{y,k + 1}^{T} } \right)^{ - 1} {\varvec{S}}_{y,k + 1} $$
(31.36)
$$ {\varvec{S}}_{x,k + 1} = cholupdate\left( {{\varvec{S}}_{x,k + 1|k} ,{\varvec{U}}_{k + 1} , - 1} \right) $$
(31.37)

3.3 Attitude Update

The updated status vector is expressed as

$$ \hat{\user2{X}}_{k + 1} = \left[ {\begin{array}{*{20}c} {\delta {\mathbf{\Re }}_{k + 1}^{T} } & {{\varvec{X}}_{k + 1}^{eT} } \\ \end{array} } \right]^{T} $$
(31.38)

The quaternion error corresponding to \(\delta {\mathbf{\Re }}_{k + 1}\) is given by

$$ \delta {\varvec{q}}_{k + 1} = \left[ {\begin{array}{*{20}c} {\delta q_{k + 1,0} } & {\delta {\varvec{\rho}}_{k + 1}^{T} } \\ \end{array} } \right]^{T} $$
(31.39)

which can be calculated according to Formula (31.13).

The attitude quaternion is updated through

$$ {\varvec{q}}_{k + 1} = \delta {\varvec{q}}_{k + 1} \otimes {\varvec{\chi}}_{k + 1\left| k \right.}^{ * q} $$
(31.40)

Reset \(\delta {\mathbf{\Re }}_{k + 1}\) to zeros and go to the next filtering cycle.

4 Vehicle-Mounted Field Test

To verify the performance of the proposed RSRUSQUE, the vehicle tests were carried out in Changsha. Figure 31.1 shows the test equipment, which includes a self-developed inertial measurement unit (IMU), a dual-antenna GPS receiver, a navigation computer and a self-made LDV. The movement trajectories of vehicles and the outputs of LDV are shown in Figs. 31.2 and 31.3, respectively.

Fig. 31.1
figure 1

Installation diagram of the experimental system

Full size image
Fig. 31.2
figure 2

Trajectory of the vehicle in the field test

Full size image
Fig. 31.3
figure 3

Velocity curve of LDV output

Full size image

In the vehicle test, the initial value of attitude quaternion is all set as [1 0 0 0]T. For RSRUSQUE, the initial covariance matrix of attitude error is set as diag([3° 3° 12°]T)2. The attitude errors are shown in Figs. 31.4, 31.5 and 31.6.

Fig. 31.4
figure 4

Pitch angle error

Full size image
Fig. 31.5
figure 5

Roll angle error

Full size image
Fig. 31.6
figure 6

Heading angle error

Full size image

As can be seen from Figs. 31.4, 31.5 and 31.6, at the first 60 s of alignment, the pitch error of the two schemes does not converge, but the RSRUSQUE scheme fluctuates less. At 61–180 s of alignment, the pitch error convergence of RSRUSQUE is more gentle than OBA. Different from the pitch error, the roll error of the two schemes has close convergence speed and accuracy. As for heading error, it can be clearly seen from Fig. 31.6 that RSRUSQUE has a faster convergence speed, higher accuracy and smaller fluctuation than OBA. In conclusion, compared with the mainstream OBA method, RSRUSQUE has stronger robustness, faster convergence rate and higher alignment accuracy.

5 Conclusion

This paper proposes a new fast in-motion initial alignment method for LDV-aided SINS. First, the process model and measurement model for LDV-aided SINS in-motion alignment is derived through skillfully attitude matrix decomposition and velocity kinematic equation reconstruction. In order to improve the robustness of the in-motion alignment process and pursue faster alignment speed and accuracy, the RSRUSQUE is proposed. One group of vehicle field test was carried out to evaluate the performance of RSRUSQUE. Experimental results show that RSRUSQUE has higher alignment accuracy, faster convergence speed and stronger robustness than OBA.