An Adaptive Initial Alignment Algorithm Based on Variance Component Estimation for a Strapdown Inertial Navigation System for AUV

Abstract

As a typical navigation system, the strapdown inertial navigation system (SINS) is crucial for autonomous underwater vehicles (AUVs) since the SINS accuracy determines the performance of AUVs. Initial alignment is one of the key technologies in SINS, and initial alignment time and initial alignment accuracy affect the performance of SINS directly. As actual systems are nonlinear, the nonlinear filter is widely used to improve the accuracy of the initial alignment. Due to its higher precision and lower computational load, the cubature Kalman filter (CKF) has done well in state estimation. However, the noise characteristics need to be known exactly as prior knowledge, which is difficult or even impossible to achieve. Thus, the adaptive filter should be introduced in the initial alignment algorithm to suppress the uncertainty effect caused by the unknown system noise. Therefore, taking the nonlinearity and uncertainty into account, a novel initial alignment algorithm for AUVs is proposed in this manuscript, based on CKF and the adaptive variance components estimation (VCE) filter (VCKF). Additionally, the simulation and experiment results show that not only the accuracy, but also the convergence speed can be improved with this proposed method. The validity and superiority of this novel adaptive initial alignment algorithm based on VCKF are verified.

1. Introduction

Autonomous underwater vehicles (AUVs) are now being widely used for a variety of tasks, including oceanographic surveys, demining and bathymetric data collection in marine and riverine environments. Accurate navigation is essential to ensure the accuracy of the gathered data for these applications [1]. Since the propagation of the radio signal is restricted in the underwater environment, the satellite navigation system and its corresponding integrated navigation system cannot be used for AUVs’ navigation. The strapdown inertial navigation system (SINS), integrating measurements from accelerometers and gyroscopes unrestrictedly, can provide navigation information for AUVs. However, pure inertial navigation cannot satisfy the demands of long range and long endurance, due to the navigation error, which accumulates with time. Thus, a typical navigation scheme for AUVs is based on a high quality SINS combined with Doppler velocity log (DVL) [2,3,4].

The purpose of initial alignment is to determine the initial attitude matrix between the body frame (b-frame) and the navigation frame (n-frame) [5,6]. Since the accuracy of the initial attitude matrix determines the performance of AUVs’ SINS, initial alignment is regarded as one of the key technologies of SINS [7,8]. What is more, the alignment speed and alignment accuracy are two main technical indicators of initial alignment, which will determine the navigation and positioning accuracy [9,10]. Therefore, how to realize the initial alignment quickly and accurately has attracted many researchers recently.

In the initial alignment, the system error model is established firstly, and then, the initial alignment angle can be estimated by optimal estimation methods. Hu et al. [11] proposed modifying the error equations of the MEMS inertial measurement system to satisfy the filtering model, and Gao proposed a new alignment method based on the interacting multiple model (IMM) algorithm, considering that one model cannot accurately describe the practical system. As we all know that almost all actual systems are nonlinear, especially when AUVs are in a high maneuvering state or misalignment angles are large, using the classical linear filter will introduce errors inevitably [12,13]. Therefore, the nonlinear filter should be considered in initial alignment for AUVs’ SINS.

In [14], the SINS initial alignment was completed by the extend Kalman filter (EKF), which is a widely-used nonlinear filtering method. In EKF, the nonlinear system can be linearized utilizing the Taylor series expansion for variance propagation, while the prediction of the state vector and measurement vector is conducted using the nonlinear system [15,16,17]. Although this method is used in many nonlinear systems because of its simplicity, the precision is limited in the systems with strong nonlinearity, and the complicated Jacobian matrix should be calculated, which will inevitably increase the computational load. Therefore, the unscented Kalman filter (UKF) was proposed. With the unscented transformation (UT), UKF can approximate the mean and the variance of the Gaussian state distribution using the nonlinear function, avoiding the local linearization and the calculation of the Jacobian matrix effectively [2,18,19]. However, the covariance matrix is non-positive in high-dimensional systems sometimes, which will lead to filtering divergence. To solve the above-mentioned problems, Arasaratnam et al. [20,21] proposed the cubature Kalman filter (CKF) method based on the cubature transform to conduct the prediction using the cubature points, which have the same weight. CKF uses the cubature rule and cubature point sets to compute the mean and variance of the probability distribution without any linearization of a nonlinear model. Thus, the modeling precision can reach third-order or higher. Furthermore, this filtering solution does not demand Jacobians and Hessians, so that the computational complexity will be alleviated to a certain extent. Therefore, compared with EKF and UKF, CKF is more widely used in attitude estimation and navigation due to its high accuracy and low calculation load. Therefore, we use CKF to deal with system nonlinearity in our manuscript.

In above-mentioned filters, the estimations are the optimal only when the mathematic model is exactly known and the system process and measurement noises are white Gaussian noise. However, this cannot be satisfied easily in practical systems because of the dynamic errors caused by the AUVs’ maneuvering or the the uncertainty caused by the external environment [22,23,24]. Therefore, the adaptive filter should be considered to improve the estimated accuracy.

At present, there are various adaptive filters. One of the outstanding adaptive filters, named the Sage–Husa adaptive filter, can estimate the variance matrices of the process and measurement noises in real time [24,25]. Besides, the Huber method also can suppress the process uncertainty [26,27]. However, in fact, these adaptive filters cannot simultaneously estimate process and measurement variance matrices at the same time, which means one of them should be already known. Variance component estimation (VCE) is an adaptive method that can estimate the system noises by utilizing residual vectors [22].

Therefore, in this manuscript, taking both the nonlinear system and the uncertainty problem into account, a novel initial alignment algorithm of moving-based SINS is proposed. In this novel algorithm, CKF can solve the nonlinear problem caused by the large maneuvering of the ship, while the adaptive VCE method can restrain the effect of the unknown-noise characteristics. The rest of this manuscript is organized as follows. The fundamentals of the SINS initial alignment and nonlinear CKF are introduced in Section 2. Additionally, the adaptive VCE method and the proposed novel initial alignment based on the CKF and VCE methods, simply denoted as the VCKF method, is proposed in Section 3. Numerical simulations and experiments along with specific analyses are given in Section 4. Section 5 concludes this manuscript.

2. Basic Knowledge

2.1. Principles of SINS Initial Alignment

To deduce and understand the SINS initial alignment easily, we give several frames firstly: the inertial frame (i-frame), the Earth frame (e-frame), the carrier body frame (b-frame), the navigation frame (n-frame) and the calculation frame ($n^{\prime }$-frame). Here, we select the “east-north-up (ENU)” geographic coordinate system as the n-frame and select the “right-front-up (RFU)” coordinate as the SINS b-frame.

In general, there are misalignment angles between the n-frame and $n^{\prime }$-frame, and the n-frame can transform to the $n^{\prime }$-frame after three Euler angle rotations. We use $\mathit{\alpha }={[{\alpha }_x,{\alpha }_y,{\alpha }_z]}^{\top }$ to represent Euler error angles, and in general, it is assumed that the two horizontal misalignment angles are small. Thus, the orientation cosine matrix from n to $n^{\prime }$, denoted by ${\mathit{C}}_n^{n^{\prime }}$, can be expressed as:

$${\mathit{C}}_n^{n^{\prime }}=\left[\begin{array}{ccc}cos{\alpha }_z& sin{\alpha }_z& -{\alpha }_y\\ -sin{\alpha }_z& cos{\alpha }_z& {\alpha }_x\\ {\alpha }_{y}cos{\alpha }_z+{\alpha }_{x}sin{\alpha }_z& {\alpha }_{y}sin{\alpha }_z-{\alpha }_{x}cos{\alpha }_z& 1\end{array}\right]$$

(1)

It is assumed that the gyro measurement errors $\delta {\mathit{\omega }}_{ib}^b$ are mainly composed of the constant drift error ${\mathit{\epsilon }}^b$ and zero mean Gaussian white noise ${\mathit{w}}_g^b$; the accelerometer measurement error $\delta {\mathit{f}}_{sf}^b$ is mainly the constant bias error ${\mathbf{\nabla }}^b$ and the zero mean Gaussian white noise ${\mathit{w}}_a^b$; the gravity error term $\delta {\mathit{g}}^n$ is ignored; and ${\tilde{\mathit{v}}}^n=\delta {\mathit{v}}^n$ holds under the static base. Then, the nonlinear error equation of SINS initial alignment is obtained from [12]:

$$\left\{\begin{array}{ccc}\hfill \dot{\mathit{\alpha }}& =& {\mathit{C}}_{\omega }^{-1}\left[\left(\mathit{I}-{\mathit{C}}_n^{n^{\prime }}\right){\widehat{\mathit{\omega }}}_{in}^n+{\mathit{C}}_n^{n^{\prime }}\delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^{n^{\prime }}\delta {\mathit{\omega }}_{ib}^b\right]\hfill \\ \hfill \delta {\dot{\mathit{v}}}^n& =& {\mathit{C}}_b^{n^{\prime }}{\widehat{\mathit{f}}}_{sf}^b-{\mathit{C}}_b^n{\widehat{\mathit{f}}}_{sf}^b+{\mathit{C}}_b^n{\mathbf{\nabla }}^b-(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{en}^n)\times ({\widehat{\mathit{v}}}^n-\delta {\mathit{v}}^n)-(2{\widehat{\mathit{\omega }}}_{ie}^n+{\widehat{\mathit{\omega }}}_{en}^n)\times \delta {\mathit{v}}^n+{\mathit{C}}_b^n{\mathit{w}}_a^b\hfill \\ \hfill {\dot{\mathit{\epsilon }}}^b& =& 0\hfill \\ \hfill {\dot{\mathbf{\nabla }}}^b& =& 0\hfill \\ \hfill \delta \dot{\mathit{\lambda }}& =& \delta \mathit{\phi }tan\mathit{\phi }sec\mathit{\phi }\frac{v_x}{R_N}+sec\mathit{\phi }\frac{\delta v_x}{R_N}\hfill \\ \hfill \delta \dot{\mathit{\phi }}& =& \frac{\delta v_y}{R_M}\hfill \end{array}\right.$$

(2)

wherein ${\mathit{C}}_b^{n^{\prime }}$ denotes the orientation cosine matrix from the b-frame to the $n^{\prime }$-frame and ${\mathit{C}}_b^n={\mathit{C}}_{n^{\prime }}^n{\mathit{C}}_b^{n^{\prime }}$; $\delta {\mathit{\omega }}_{ib}^b$ is the measurement error of gyroscopes, ${\mathit{\omega }}_{in}^n$ is the rotating angular rate of the n-frame relative to i-frame and $\delta {\mathit{\omega }}_{in}^n$ is the calculated error of ${\mathit{\omega }}_{in}^n$; ${\widehat{\mathit{f}}}^b$ and $\delta {\mathit{f}}^b$ denote the accelerometer measurement and its corresponding measurement error, respectively; ${\widehat{\mathit{\omega }}}_{ie}^n$ is the calculated Earth rotating angular rate, and ${\widehat{\mathit{\omega }}}_{en}^n$ is the calculated position rate; ${\widehat{\mathit{v}}}^n$ and $\delta {\mathit{v}}^n$ denote velocity measurement and corresponding error; $\delta {\mathit{g}}^n$ is the error of gravity acceleration error; longitude error $\delta \mathit{\lambda }$ and latitude error $\delta \mathit{\phi }$; $R_M$ and $R_N$ are the Earth’s radius of the meridian plane and the prime vertical plane, respectively; and $\mathit{\phi }$ is local latitude; $v_x$ and $v_y$ are the east and north velocity, respectively. Thus, Equations (3) and (4) also can be obtained from Equation (2):

$${\widehat{\mathit{\omega }}}_{in}^n={\mathit{\omega }}_{in}^n+\delta {\mathit{\omega }}_{in}^n$$

(3)

$${\mathit{C}}_{\omega }=\left[\begin{array}{ccc}cos{\alpha }_y& 0& -sin{\alpha }_{y}cos{\alpha }_x\\ 0& 1& sin{\alpha }_x\\ sin{\alpha }_y& 0& cos{\alpha }_{y}cos{\alpha }_x\end{array}\right]$$

(4)

Taking positioning errors ($\delta \lambda ,\delta \phi$), horizontal velocities ($\delta v_x,\delta v_y$), misalignment angles (${\alpha }_x,{\alpha }_y,{\alpha }_z$), the constant gyro drifts (${\epsilon }_x,{\epsilon }_y,{\epsilon }_z$) and horizontal accelerometer biases (${\nabla }_x,{\nabla }_y$) into account, the state error vector can be expressed as:

$$\mathit{x}={\left[\begin{array}{cccccccccccc}\delta \lambda & \delta \phi & \delta v_x& \delta v_y& {\alpha }_x& {\alpha }_y& {\alpha }_z& {\nabla }_x& {\nabla }_y& {\epsilon }_x& {\epsilon }_y& {\epsilon }_z\end{array}\right]}^{\top }$$

(5)

Additionally, the noise vector is:

$$\mathit{w}={\left[\begin{array}{ccccccc}{\mathit{0}}_{1\times 2}& w_{ax}^b& w_{ay}^b& w_{gx}^b& w_{gy}^b& w_{gz}^b& {\mathit{0}}_{1\times 5}\end{array}\right]}^{\top }$$

(6)

wherein ${[\begin{array}{ccc}{w}_{gx}^b& w_{gy}^b& w_{gz}^b\end{array}]}^{\top }$ is the random drift of the gyro, while ${[\begin{array}{cc}{w}_{ax}^b& w_{ay}^b\end{array}]}^{\top }$ means the random bias of the horizontal accelerometer.

The filtering state equation is established as follows:

$$\left\{\begin{array}{ccc}\hfill \dot{\mathit{x}}(k)& =& \mathit{f}\left(\mathit{x}(k-1)\right)+\mathit{w}(k-1)\hfill \\ \hfill \mathit{z}(k)& =& \mathit{H}(k)\mathit{x}(k)+\mathit{\eta }(k)\hfill \end{array}\right.$$

(7)

In this paper, the velocity error is used as the observation:

$$\mathit{z}=\delta {\mathit{v}}^n=\left[\begin{array}{c}\delta v_x^n\\ \delta v_y^n\end{array}\right]$$

(8)

and the observing matrix $\mathit{H}$ can be indicated as:

$$\mathit{H}=\left[\begin{array}{ccc}{\mathit{0}}_{2\times 2}& {\mathit{I}}_{2\times 2}& {\mathit{0}}_{2\times 8}\end{array}\right]$$

(9)

where $\mathit{\eta }(k)$ denotes the observation noise vector.

Now, the nonlinear system model of SINS is established, and then, with the nonlinear filter, the misalignment angle can be estimated, increasing the SINS navigation accuracy.

2.2. Nonlinear Filter CKF

Since the CKF is not only simple and easy to implement, but also has high precision and good convergency, it is widely used in nonlinear estimations [12,21,28].

Let us consider a nonlinear state-space model:

$$\left\{\begin{array}{c}\mathit{x}(k)=\mathit{f}[\mathit{x}(k-1)]+\mathit{w}(k-1)\hfill \\ \mathit{z}(k)=\mathit{h}[\mathit{x}(k)]+{\mathbf{\Delta }}_k\hfill \end{array}\right.$$

(10)

where $\mathit{x}(k)$ and $\mathit{z}(k)$ denote the state vector and the measurement vector, respectively; $\mathit{f}(·)$ and $\mathit{h}(·)$ are the nonlinear state and measurement vector functions; $\mathit{w}(k-1)\sim N(0,\mathit{Q})$ and $\mathbf{\Delta }(k)\sim N(0,\mathit{R})$ represent the process noise and the observation noise vectors, respectively.

For this nonlinear system, $2n$ sampled points, named the cubature points, which have equal weight, are selected to calculate the Gaussian distribution, and then, the CKF loops through the time and measurement updates. Thus, the cubature points can be set as:

$$\left\{\begin{array}{c}{\xi }_i=\sqrt{\frac{2n}{2}}{\left[1\right]}_i\hfill \\ {\omega }_i=\frac{1}{2n}\hfill \end{array}\right.,i=1,2,\cdots ,2n$$

(11)

where n is the dimension of the state vector.

Assume that the posterior density function $\mathit{P}(k-1,k-1)=\mathit{S}(k-1,k-1){\mathit{S}}^{\top }(k-1,k-1)$ is known; the estimation process with CKF is as below [20]:

$${\mathit{X}}_i(k-1,k-1)=\mathit{S}(k-1,k-1){\mathit{\xi }}_i+\widehat{\mathit{x}}(k-1,k-1)$$

(12)

$$\widehat{\mathit{x}}(k,k-1)=\frac{1}{2N}\sum _{i=1}^{2N}\mathit{f}[{\mathit{X}}_i(k-1,k-1)]$$

(13)

$$\mathit{P}(k,k-1)=\frac{1}{2N}\sum _{i=1}^{2N}\mathit{f}[{\mathit{X}}_i^{\top }(k-1,k-1)]\mathit{f}[{\mathit{X}}_i(k-1,k-1)]-\widehat{\mathit{x}}(k,k-1){\widehat{\mathit{x}}}^{\top }(k,k-1)+\mathit{Q}(k-1)$$

(14)

$$\mathit{P}(k,k-1)=\mathit{S}(k,k-1){\mathit{S}}^{\top }(k,k-1)$$

(15)

$${\mathit{X}}_i(k,k-1)=\mathit{S}(k,k-1){\mathit{\xi }}_i+\widehat{\mathit{x}}(k,k-1)$$

(16)

$$\widehat{\mathit{z}}(k,k-1)=\frac{1}{2N}\sum _{i=1}^{2N}\mathit{h}[{\mathit{x}}_i(k,k-1)]$$

(17)

$${\mathit{P}}_{zz}(k,k-1)=\frac{1}{2N}\sum _{i=1}^{2N}\mathit{h}[{\mathit{x}}_i(k,k-1)]\mathit{h}{[{\mathit{x}}_i(k,k-1)]}^{\top }-\widehat{\mathit{z}}(k,k-1){\widehat{\mathit{z}}}^{\top }(k,k-1)+{\mathit{R}}_k$$

(18)

$${\mathit{P}}_{xz}(k,k-1)=\frac{1}{2N}\sum _{i=1}^{2N}{\mathit{X}}_i(k,k-1)\mathit{h}{[{\mathit{x}}_i(k,k-1)]}^{\top }-\widehat{\mathit{x}}(k,k-1){\widehat{\mathit{z}}}^{\top }(k,k-1)+{\mathit{R}}_k$$

(19)

Additionally, the gain matrix $\mathit{K}(k)$ is obtained by utilizing Equation (20):

$$\mathit{K}(k)={\mathit{P}}_{xz}(k,k-1){[{\mathit{P}}_{xz}(k,k-1)]}^{-1}$$

(20)

The estimation of the state vector $\widehat{\mathit{x}}(k,k)$ can be obtained:

$$\widehat{\mathit{x}}(k,k)=\widehat{\mathit{x}}(k,k-1)+\mathit{K}(k)[\mathit{z}(k)-\widehat{\mathit{z}}(k,k-1)]$$

(21)

where $\mathit{d}(k)=\mathit{z}(k)-\widehat{\mathit{z}}(k,k-1)$ is the system innovation vector of the nonlinear system and the hat operator denotes the predicted value. The variance matrix of the estimated state vector $\mathit{P}(k,k)$ is calculated by Equation (22):

$$\mathit{P}(k,k)=\mathit{P}(k,k-1)-\mathit{K}(k){\mathit{P}}_{zz}(k,k-1){\mathit{K}}^{\top }(k)$$

(22)

In the CKF method, the cubature rule and $2N$ cubature point sets $\left[{\xi }_i,{\omega }_i\right]$ are used to compute the mean and variance of the probability distribution without any model linearization. Thus, the model precision can reach third-order or higher [12]. Furthermore, compared with EKF and UKF, CKF does not demand calculating the Jacobians’ or Hessians’ matrix, so that the computational complexity will be alleviated to a certain extent. In conclusion, CKF is a state estimation algorithm with higher estimation accuracy and lower computational load. Therefore, it is appropriate for SINS initial alignment and for state estimation of high-dimensional nonlinear systems.

3. An Improved Initial Alignment Algorithm Based on Adaptive VCKF

3.1. Adaptive Filter Based on the VCE Method

The adaptive filter based on the VCE method can estimate the variance components of the process noise and the measurement noise vectors in real time using the residual vectors to decompose the system innovation vector. On the basis of the estimated variance components, the weighting matrices of the process noise and the measurement noise vectors can be adjusted, and then, their effects on the state vector can be adjusted accordingly.

Considering the standard linear system model, the state and measurement equations are:

$$\left\{\begin{array}{ccc}\hfill \mathit{x}(k)& =& \mathsf{\Phi }(k,k-1)\mathit{x}(k-1)+\mathsf{\Gamma }(k)\mathit{w}(k)\hfill \\ \hfill \mathit{z}(k)& =& \mathit{H}(k)\mathit{x}(k)+\mathbf{\Delta }(k)\hfill \end{array}\right.$$

(23)

where $\mathit{x}(k)$ and $\mathit{z}(k)$ are the state vector and the measurement vector, respectively; $\mathsf{\Phi }(k,k-1)$ and $\mathsf{\Gamma }(k)$ are the state-transition matrix, the coefficient matrix of the process noise vector, respectively; $\mathit{w}(k)$ and $\mathbf{\Delta }(k)$ denote the process noise vector and the measurement noise vector, respectively. Further, $\mathit{w}(k)$ and $\mathbf{\Delta }(k)$ are the zero mean Gaussian noises:

$$\left\{\begin{array}{ccc}\hfill \mathit{w}(k)& \sim & N(0,\mathit{Q}(k))\hfill \\ \hfill \mathbf{\Delta }(k)& \sim & N(0,\mathit{R}(k))\hfill \end{array}\right.$$

(24)

where $\mathit{Q}(k)$ and $\mathit{R}(k)$ are positive definite matrices.

Thus, the two-step update process of the Kalman filter is as follows: The time update:

$$\left\{\begin{array}{ccc}\hfill \widehat{\mathit{x}}(k,k-1)& =& \mathsf{\Phi }(k,k-1)\widehat{\mathit{x}}(k-1)\hfill \\ \hfill \mathbf{\Delta }(k)& =& N(0,\mathit{R}(k))\hfill \end{array}\right.$$

(25)

The measurement update:

$$\left\{\begin{array}{ccc}\hfill \widehat{\mathit{x}}(k)& =& \widehat{\mathit{x}}(k,k-1)+\mathit{G}(k)\mathit{d}(k)\hfill \\ \hfill {\mathit{P}}_{xx}(k)& =& [\mathit{I}-\mathit{G}(k)\mathit{H}(k)]{\mathit{P}}_{xx}(k,k-1)\hfill \end{array}\right.$$

(26)

where $\mathit{G}(k)$ and $\mathit{d}(k)$ denote the gain matrix and the system innovation vector, respectively; and $\mathit{I}$ is a unit matrix.

$$\mathit{G}(k)={\mathit{P}}_{xx}(k,k-1){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}(k)$$

(27)

$$\mathit{d}(k)=\mathit{z}(k)-\mathit{H}(k)\widehat{\mathit{x}}(k,k-1)$$

(28)

$${\mathit{P}}_{dd}(k)=\mathit{H}(k){\mathit{P}}_{xx}(k,k-1){\mathit{H}}^{\top }(k)+\mathit{R}(k)$$

(29)

The estimated state vector is optimal when $\mathit{Q}(k)$ and $\mathit{R}(k)$ are exactly known. However, they cannot be obtained easily in practice, and it is desirable to obtain their estimate by utilizing the adaptive filter in real time.

According to [22], the random information in a system can normally be divided into three independent parts: the process noise vector ${\mathit{l}}_x$, the measurement noise vector ${\mathit{l}}_w$ and the predicted states noise vector ${\mathit{l}}_z$, respectively. They are defined as follows:

$$\left\{\begin{array}{ccc}\hfill {\mathit{l}}_x(k)& =& \mathsf{\Phi }(k,k-1)\widehat{\mathit{x}}(k-1)\hfill \\ \hfill {\mathit{l}}_w(k)& =& {\mathit{w}}_0(k)\hfill \\ \hfill {\mathit{l}}_z(k)& =& \mathit{z}(k)\hfill \end{array}\right.$$

(30)

Considering their residual equations, the system in Equation (23) can be rewritten as:

$$\left\{\begin{array}{ccc}\hfill {\mathit{v}}_{{\mathit{l}}_x}(k)& =& \widehat{\mathit{x}}(k-1)+\mathsf{\Gamma }(k)\widehat{\mathit{w}}(k)-{\mathit{l}}_x(k)\hfill \\ \hfill {\mathit{v}}_{{\mathit{l}}_x}(k)& =& \widehat{\mathit{w}}(k)-{\mathit{l}}_w(k)\hfill \\ \hfill {\mathit{v}}_{{\mathit{l}}_z}(k)& =& \mathit{H}(k)\widehat{\mathit{x}}(k)-{\mathit{l}}_z(k)\hfill \end{array}\right.$$

(31)

with their measurement variance matrices as follows:

$$\left\{\begin{array}{ccc}\hfill {\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}(k)& =& \mathsf{\Phi }(k,k-1){\mathit{P}}_{xx}(k-1){\mathsf{\Phi }}^{\top }(k,k-1)\hfill \\ \hfill {\mathit{P}}_{{\mathit{l}}_w{\mathit{l}}_w}(k)& =& \mathit{Q}(k)\hfill \\ \hfill {\mathit{P}}_{{\mathit{l}}_z{\mathit{l}}_z}(k)& =& \mathit{R}(k)\hfill \end{array}\right.$$

(32)

Therefore, we can estimate the covariance matrices as long as we calculate the covariance matrices of the residual vectors. According to the steps of the Kalman filter, the estimations of the residual vectors can be calculated:

$$\left\{\begin{array}{ccc}\hfill {\mathit{v}}_{{\mathit{l}}_x{\mathit{l}}_x}(k)& =& {\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}(k){\mathit{P}}_{xx}^{-1}(k,k-1)\mathit{G}(k)\mathit{d}(k)\hfill \\ \hfill {\mathit{v}}_{{\mathit{l}}_w{\mathit{l}}_w}(k)& =& \mathit{Q}(k-1){\mathsf{\Gamma }}^{\top }(k-1){\mathit{P}}_{xx}^{-1}(k,k-1)\mathit{G}(k)\mathit{d}(k)\hfill \\ \hfill {\mathit{v}}_{{\mathit{l}}_z{\mathit{l}}_z}(k)& =& [\mathit{H}(k)\mathit{G}(k)-\mathit{I}]\mathit{d}(k)\hfill \end{array}\right.$$

(33)

Additionally, then, the corresponding variance matrices are represented as:

$$\left\{\begin{array}{ccc}\hfill {\mathit{P}}_{{\mathit{v}}_{{\mathit{l}}_x{\mathit{l}}_x}}(k)& =& \mathsf{\Phi }(k-1){\mathit{P}}_{xx}(k-1){\mathsf{\Phi }}^{\top }(k-1){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}^{-1}(k)·\hfill \\ & & \mathit{H}(k)\mathsf{\Phi }(k-1){\mathit{P}}_{xx}(k-1){\mathsf{\Phi }}^{\top }(k-1)\hfill \\ \hfill {\mathit{P}}_{{\mathit{v}}_{{\mathit{l}}_w{\mathit{l}}_w}}(k)& =& \mathit{Q}(k-1){\mathsf{\Gamma }}^{\top }(k-1){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}^{-1}(k)·\hfill \\ & & \mathit{H}(k)\mathsf{\Gamma }(k-1)\mathit{Q}(k-1)\hfill \\ \hfill {\mathit{P}}_{v_{{\mathit{l}}_z{\mathit{l}}_z}}(k)& =& \{\mathit{I}-\mathit{H}(k)\mathit{G}(k)\}\mathit{R}(k)\hfill \end{array}\right.$$

(34)

Here, the innovation vector is projected into three residual vectors associated with the three groups of the measurements. Hence, we can estimate the variance factors.

Assume that all components in ${\mathit{l}}_z(k)$ and ${\mathit{l}}_w(k)$ are uncorrelated, so both $\mathit{R}(k)$ and $\mathit{Q}(k)$ become diagonal. In this case, the redundancy index of each measurement noise factor is given by [22]:

$${\mathit{r}}_{{\mathit{z}}_i}=1-{[\mathit{H}(k)\mathit{G}(k)]}_{ii}$$

(35)

Similarly, the redundancy index of each process noise factor is equal to:

$${\mathit{r}}_{{\mathit{w}}_i}={[\mathit{Q}(k){\mathsf{\Gamma }}^{\top }(k){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}^{-1}(k)\mathit{H}(k)\mathsf{\Gamma }(k-1)]}_{ii}$$

(36)

Furthermore, the individual group redundancy contributions, or the total group redundant indexes, are equal to:

$$\left\{\begin{array}{ccc}\hfill {\mathit{r}}_x(k)& =& trace[\mathsf{\Phi }(k-1){\mathit{P}}_{xx}(k){\mathsf{\Phi }}^{\top }(k-1){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}^{-1}(k)\mathit{H}(k)]\hfill \\ \hfill {\mathit{r}}_w(k)& =& trace[\mathit{Q}(k){\mathsf{\Gamma }}^{\top }(k){\mathit{H}}^{\top }(k){\mathit{P}}_{dd}^{-1}(k)\mathit{H}(k)\mathsf{\Gamma }(k-1)]\hfill \\ \hfill {\mathit{r}}_z(k)& =& trace[\mathit{I}-\mathit{H}(k)\mathit{G}(k)]\hfill \end{array}\right.$$

(37)

On the basis of the Herlmet VCE method, the individual group variance factors of unit weight are estimated by the residual vector and the corresponding redundant index as follows:

$${\widehat{\sigma }}_{0g}^2=\frac{{\mathit{v}}_g{(k)}^{\top }{\mathit{P}}_{l_g{l}_g}(k){\mathit{v}}_g(k)}{r_g(k)}(g=x,w,z)$$

(38)

Thus, at time k, the individual variance factors of $l_z(k)$ can be calculated by:

$${\widehat{\sigma }}_{z_i}^2=\frac{v_{z_i}^2(k)}{r_{z_i}(k)}$$

(39)

Additionally, the covariance matrix of the measurement noise is as follows:

$$\mathit{R}(k)=\left[\begin{array}{ccc}{\widehat{\sigma }}_{z_1}^2(k)& & \\ & \ddots & \\ & & {\widehat{\sigma }}_{z_p}^2(k)\end{array}\right]$$

(40)

Similarly, the variance factors of ${\mathit{l}}_w(k)$ and the variance matrix $\mathit{Q}(k)$ can be calculated by the following two equations.

$${\widehat{\sigma }}_{w_j}^2=\frac{v_{w_j}^2(k)}{r_{w_j}(k)}$$

(41)

$$\mathit{Q}(k)=\left[\begin{array}{ccc}{\widehat{\sigma }}_{w_1}^2(k)& & \\ & \ddots & \\ & & {\widehat{\sigma }}_{w_m}^2(k)\end{array}\right]$$

(42)

3.2. An initial Alignment Algorithm Based on VCKF

To solve the nonlinear problem and improve the stochastic model simultaneously in SINS initial alignment, we here propose a new improved adaptive filter by considering both CKF and the VCE adaptive method, denoted as VCKF.

Taking the nonlinear system described by Equation (10) into account, three pseudo measurement groups are defined as follows:

$$\left\{\begin{array}{c}{\mathit{l}}_x(k)=\mathit{f}[\widehat{\mathit{x}}(k-1,k-1)]\hfill \\ {\mathit{l}}_w(k)=\mathit{w}(k-1)\hfill \\ {\mathit{l}}_z(k)=\mathit{z}(k)\hfill \end{array}\right.$$

(43)

The residual equation of the nonlinear system is represented as:

$$\left\{\begin{array}{c}{\mathit{v}}_{{\mathit{l}}_x}(k)=\widehat{\mathit{x}}(k,k)+\widehat{\mathit{w}}(k-1)-{\mathit{l}}_x(k)\hfill \\ {\mathit{v}}_{{\mathit{l}}_w}(k)=\widehat{\mathit{w}}(k-1)-{\mathit{l}}_w(k)\hfill \\ {\mathit{v}}_{{\mathit{l}}_z}(k)=\mathit{h}[\widehat{\mathit{x}}(k,k)]-{\mathit{l}}_w(k)\hfill \end{array}\right.$$

(44)

According to the steps of the CKF, Equation (33) can be rewritten as:

$$\left\{\begin{array}{c}{\mathit{v}}_{{\mathit{l}}_x{\mathit{l}}_x}(k)={\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}{\mathit{P}}_{k,k-1}^{-1}\mathit{K}(k)\mathit{d}(k)\hfill \\ {\mathit{v}}_{{\mathit{l}}_w{\mathit{l}}_w}(k)=\mathit{Q}(k-1){\mathit{P}}^{-1}(k,k-1)K(k)\mathit{d}(k)\hfill \\ {\mathit{v}}_{{\mathit{l}}_z{\mathit{l}}_z}(k)=[{\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1)\mathit{K}(k)-\mathit{I}]\mathit{d}(k)\hfill \end{array}\right.$$

(45)

The corresponding variance matrices are:

$$\left\{\begin{array}{ccc}\hfill {\mathit{P}}_{{\mathit{v}}_{{\mathit{l}}_x{\mathit{l}}_x}}(k)& =& {\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}(k){\mathit{P}}^{-1}(k,k-1){\mathit{P}}_{xz}(k,k-1){\mathit{P}}_{zz}^{-1}(k,k-1)·\hfill \\ & & {\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1){\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}(k)\hfill \\ \hfill {\mathit{P}}_{{\mathit{v}}_{{\mathit{l}}_w{\mathit{l}}_w}}(k)& =& \mathit{Q}(k-1){\mathit{P}}^{-1}(k,k-1){\mathit{P}}_{xz}(k,k-1){\mathit{P}}_{zz}^{-1}(k,k-1)·\hfill \\ & & {\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1)\mathit{Q}(k-1)\hfill \\ \hfill {\mathit{P}}_{{\mathit{v}}_{{\mathit{l}}_z{\mathit{l}}_z}}(k)& =& [\mathit{I}-{\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1)\mathit{K}(k)]\mathit{R}(k)\hfill \end{array}\right.$$

(46)

The individual group redundant indices are equal to:

$$\left\{\begin{array}{ccc}\hfill {\mathit{r}}_x(k)& =& trace[{\mathit{P}}_{{\mathit{l}}_x{\mathit{l}}_x}(k){\mathit{P}}^{-1}(k,k-1){\mathit{P}}_{xz}(k,k-1){\mathit{P}}_{zz}^{-1}(k,k-1)·\hfill \\ & & {\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1)]\hfill \\ \hfill {\mathit{r}}_w(k)& =& trace[\mathit{Q}(k-1){\mathit{P}}^{-1}(k,k-1){\mathit{P}}_{xz}(k,k-1){\mathit{P}}_{zz}^{-1}(k,k-1)·\hfill \\ & & {\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k|k-1)]\hfill \\ \hfill {\mathit{r}}_z(k)& =& trace[\mathit{I}-{\mathit{P}}_{xz}^{\top }(k,k-1){\mathit{P}}^{-1}(k,k-1)\mathit{K}(k)]\hfill \end{array}\right.$$

(47)

Here, the covariance factors and the variance matrices for the nonlinear system can be calculated by Equations (44)–(47), solving the uncertain problem. Since the VCKF method is executed based on the CKF frame structure, the nonlinearity of the actual SINS is no longer an issue, increasing the system accuracy.

4. Simulations and Experiments

To verify the performance of the proposed adaptive initial alignment algorithm, simulations and experiments are performed in this section.

4.1. Simulation and Analysis

Establish the swing dynamic model of AUV as:

$$\left\{\begin{array}{c}\psi ={\psi }_{m}sin({\omega }_{\psi }t)+{\psi }_k\hfill \\ \theta ={\theta }_{m}sin({\omega }_{\theta }t)+{\theta }_k\hfill \\ \gamma ={\gamma }_{m}sin({\omega }_{\gamma }t)+{\gamma }_k\hfill \end{array}\right.$$

(48)

where $\theta$, $\gamma$ and $\psi$ are pitch, roll and yaw angles, respectively; the swing amplitudes were set up as ${\theta }_m=5^{\circ }$, ${\gamma }_m=3^{\circ }$, and ${\psi }_m=8^{\circ }$; the swing periods were $T_{\theta }=8$ s, $T_{\gamma }=6$ s, $T_{\psi }=10$ s; and the initial attitudes were ${\theta }_k={\gamma }_k=0^{\circ }$, $\psi ={30}^{\circ }$. Additionally, the initial parameters of the SINS/DVL system are given as

Table 1. Simulation parameters.

Parameters	Sets
Initial latitude	$L=45.77{}^{\circ }$
Initial longitude	$\lambda =126.67{}^{\circ }$
Initial horizontal velocity	$v_x=v_y=0$ m/s
Gravity acceleration	$g_0=9.78049$ m/s${}^2$
Initial horizontal misalignment angles	${\alpha }_x={\alpha }_y=1^{\circ }$
Initial vertical misalignment angles	Group 1: ${\alpha }_z=5^{\circ }$;
	Group 2: ${\alpha }_z={10}^{\circ }$;
	Group 3: ${\alpha }_z={15}^{\circ }$;
Constant biases of the accelerometers	${\nabla }_x={\nabla }_y={10}^{-4}{g}_0$
Random noise of the accelerometers	$w_{ax}=w_{ay}=5\times {10}^{-5}{g}_0$
Constant drifts of the gyroscopes	${\epsilon }_x={\epsilon }_y={\epsilon }_z=0.01{}^{\circ }$/h
Random noise of the gyroscopes	$w_{gx}=w_{gy}=w_{gz}=0.005{}^{\circ }$/h
Sampling frequency	100 Hz

. Three sets of initial misalignment angles are set up here, as shown in Table 1, which make the system equations nonlinear.

According to Section 2.1, the state vector is composed of the velocity errors, the misalignment angles, the gyroscope constant drifts and the accelerometer constant biases. The measurement vector is the velocity error between SINS and DVL. On the basis of the equations, the model of the SINS initial alignment can be expressed clearly.

In order to verify the performance of the proposed filter, the standard CKF method is used as the reference filter. Under the same conditions, the proposed adaptive VCKF filter and the standard CKF method are both utilized to estimate misalignment angles, respectively. In our simulations, system noises are given in normal CKF, while those are unknown hypothetically in adaptive VCKF. Then, from the estimated results, we can be aware of their ability to solve the uncertainty problem. To eliminate the effects of random errors, 50 Monte Carlo simulations were performed. Additionally, we define the mean error to evaluate the performance of these two filters.

$$\lambda (k)=\frac{1}{N}\sum _{n=1}^N\left|\Delta {\alpha }_i\left(k\right)\right|$$

(49)

wherein $\Delta {\alpha }_i\left(k\right)$ is the misalignment angle error at k instant; $i=1,2,\dots ,N$ denotes the Monte Carlo simulation count, and $N=50$. Comparing the misalignment angle errors of the misalignment angle of different filters, the results are shown in Figure 1, Figure 2, Figure 3 and Figure 4.

In Figure 1, the left, middle and right are the estimated results of Group 1, Group 2 and Group 3, respectively. It is obvious that the horizontal misalignment angles are nearly the same, not only in convergence speed, but also in estimated accuracy, so we pay more attention to the yaw angle. From Figure 2, Figure 3 and Figure 4, it is easy to see that the convergence rate and the estimated accuracy are different with these two filters. In the first 50 s, it is clear that the convergence rate of the adaptive VCKF method is faster than that of the standard CKF method.

To present the estimated accuracy more clearly, we magnify the purple region in the figures, and the estimated errors of yaw angles are listed in

Table 2. Simulation results of estimated yaw angle error.

Groups	Error of Yaw Angle (${}^{\prime }$)
	CKF without VCE Method	Adaptive VCKF Method
Group 1	−4.05	−3.86
Group 2	−4.40	−4.19
Group 3	−4.71	−4.54

. It is clear that the estimated result with the adaptive VCKF method is still relatively better. That is because the system noise vector is estimated with the adaptive VCKF method, so the estimated states are closer to the true states than the one with the standard CKF.

4.2. Experiment and Analysis

To further validate the performance of the proposed initial alignment algorithm based on the adaptive VCKF method, experiments are carried out in Zhanjiang, China, as well. In this experiment, a ship was sailing with “Z-shaped” maneuvering, and the sailing trajectory of the ship is shown as in Figure 5. The whole experiment lasts 50 min. The schematic of the experimental setup and sensors is shown in Figure 6. In this experiment, the SINS developed by our own lab is fixed on the mounting-base in the cabin, closed to the high-accuracy inertial navigation system PHINS manufactured by ixBlue Company, France. Additionally, the NavQuest 600 DVL is installed at the bottom of the ship. The performance parameters of sensors are detailed in

Table 3. Key performance of inertial sensors and DVL.

	Parameters	Values
Gyroscope	Dynamic range	$\pm {100}^{\circ }$/s
	Bias stability	$0.01{}^{\circ }$/h
	random walk	$0.005{}^{\circ }$/h
	Scale factor stability	50 ppm
Accelerometer	Dynamic range	$\pm 20$ g
	Bias stability	$0.05$ mg
	random walk	$0.005$ mg
	Scale factor stability	50 ppm
DVL	Frequency	600 kHz
	Accuracy	$1\%\pm 1$ mm/s
	Maximum Altitude	110 m
	Minimum Altitude	0.3 m
	Maximum Velocity	$\pm 20$ kn
	Maximum Ping Rate	5/s

.

It is known from [29] that the heading and attitude accuracy of PHINS when it works with GPS is good enough. Therefore, the navigation information from PHINS is utilized as the reference to evaluate the performance of the novel initial alignment algorithm. Since the differences of horizontal errors with the normal CKF and the adaptive CKF based on VCE are nearly the same, we only analyze the yaw angle, as shown in Figure 7.

In Figure 7, the red solid line and the blue dotted line represent the estimated results with the normal CKF and the adaptive VCKF method, respectively. It is obvious that the blue line converges to zero rapidly in 400 s, although the trend is almost the same as the red line. Additionally, final estimated results are 1.55${}^{\prime }$ and 0.43${}^{\prime }$ with the standard CKF method and the adaptive VCKF method, respectively. Therefore, an estimation result closer to the actual value is obtained with the proposed filter without a priori knowledge about the system noise.

5. Conclusions

In order to solve the nonlinearity and uncertainty problems of initial alignment in the SINS/DVL integrated system of AUVs, a novel nonlinear and adaptive filter, named VCKF, was proposed based on the CKF and VCE methods in this paper. The main focus of this manuscript was establishing a nonlinear system model of the SINS/DVL integrated navigation system and deriving the CKF and VCE methods. On the basis of this, we presented the adaptive VCKF method combining these two method’s merits. To verify the effectiveness of the novel initial alignment algorithm, simulations and experiments were carried out, and the results showed that an estimation result closer to the actual value is obtained with the proposed filter without a priori knowledge about the system noise in the SINS/DVL integrated navigation system of AUVs.