A Pretreatment Method for the Velocity of DVL Based on the Motion Constraint for the Integrated SINS/DVL

Abstract

It is difficult for autonomous underwater vehicles (AUVs) to obtain accurate aided position information in many locations because of underwater conditions. The velocity accuracy from the Doppler velocity log (DVL) is a key element in deciding the AUV position accuracy when the integration system of Strapdown Inertial Navigation System/DVL/Magnetic Compass/Press Sensor (SINS/DVL/MCP/PS) is adopted. However, random noise and sudden noise in DVL caused by sound scattering, fishing populations, and seafloor gullies introduce level attitude errors and accumulate as position error. To restrain random noise, a velocity tracing method is designed based on the constant velocity model and the assumption of slow motion of AUV. To address sudden noise, a fault diagnosis method based on the ${\chi }^2$ rule is introduced to judge sudden changes from innovation. When a sudden change occurs, the time update of the velocity from the tracing model is used for data fusion instead of the velocity from DVL. Simulation test results indicate that with this velocity tracing algorithm, random noise in the DVL can be effectively restrained. The level attitude accuracy and the level position accuracy are also improved with the time update of the velocity when the sudden change occurs.

1. Introduction

Autonomous underwater vehicles (AUVs) are widely used for both military and civilian purposes, such as submarine resource investigation, surveying, and detection [1,2,3]. Because of the specificity of submarine conditions, especially the strong attenuation of the GNSS signals in the water of global navigation satellite system (GNSS), satellite-based technology is ineffective underwater; thus, high accurate positioning technology underwater becomes one of the key and difficult technologies of AUV [1,2,3]. The strapdown inertial navigation system (SINS) is self-directed, highly robust, has a high update frequency of enriched navigation data and small size, and is successfully used for AUV navigation [1,2,3,4]. Due to the inherent errors in sensors and initial alignment errors, the errors of SINS navigation data are accumulated with time [3]. The error accumulation problem is the inherent and is a bottleneck problem of SINS based on integrated operation.

To solve the error accumulation problem of SINS, external aided positioning data should be introduced to correct SINS regularly. The most commonly used submarine positioning methods include GNSS, long/short baseline acoustic system (L/SBL), and geophysical field database (such as terrain and gravity). To access the GNSS data, the AUV should rise or sail near to the water surface because radio signals are easily damped and not feasible in deep water. At the same time, LBL and the geophysical database rely on the presence of a known transponder network or a known terrain, that is to say, there are preconditions to using these methods [5,6,7,8].

Recently, with the development of acoustic velocity measuring technology, the measuring distance and accuracy of Doppler velocity log (DVL) have been improved significantly. Taking RDI WHN-600 as an example, the error of the velocity is within 0.2% and the maximum operating distance is 90 m with a working frequency of 600 kHz, whereas the error is 0.5% and the distance is 500–525 m with 150 kHz [5,6]. Currently, DVL is widely used for AUV navigation and the integrated SINS/DVL has become a standard device for AUV [1,2,3,5,6].

When the integrated system of SINS/DVL is used in AUV, the level position accuracies, including the latitude and longitude, are mainly determined by the accuracy of the DVL velocity due to the lack of external position reference. Whereas the accuracy of DVL is fixed, the accuracy of SINS/DVL is mainly determined by the data fusion algorithm [1,2,3]. Numerous fusion algorithms for SINS/DVL are available in the literature, including non-linear filtering, online error calibration, and DVL-aided alignment [9,10,11,12,13,14,15]. In contrast to the above problems, this paper focuses on how to address the random and sudden noise in DVL.

The velocity-matching mode is used in SINS/DVL [4,5,6,10,12]. With velocity matching, random noise in the level attitudes and velocities is introduced by the random noise in the DVL measurement, and sudden errors in level attitudes are generated by the sudden change in DVL. Errors in specific forces are generated, which will be accumulated as velocity and position errors with integrated operation [4]. To solve the random noise and sudden changes in DVL, in this paper, a constant velocity motion model is introduced to trace the measured velocity of the DVL based on the assumption of gentle motion of the AUV underwater, and a fault diagnosis method based on ${\chi }^2$ rules is introduced to judge the sudden change from innovations [16,17,18,19]. When sudden changes occur, the time update of velocity from tracing model is used to perform data fusion instead of the velocity from DVL.

In the integration of SINS/DVL, due to the low observability of yaw misalignment caused by velocity matching, a magnetic compass (MCP) is always used to provide aided yaw information. Meanwhile, for the divergence of the height and upward velocity of the SINS and the low accuracy of the upward velocity from DVL, a press sensor (PS) is used to provide aided height (or depth) information. In addition to SINS/DVL integration, MCP and PS are also used to construct an integrated navigation system to fulfill a long time and long distance navigation mission.

The rest of this paper is organized as follows. The integrated navigation system structure is described in Section 2. The disciplines and error propagation models for each subsystem are also introduced. Then, the data fusion algorithm for an integrated navigation system is described in Section 3. In Section 4, the negative effect of DVL error is analyzed, and the velocity tracing model is given based on the analysis on the characteristics of AUV motion. In Section 5, the tracing problem for the DVL velocity is simulated, and the results indicate that with this pretreatment of the DVL velocity, the navigation accuracy can be improved. Finally, some conclusions are drawn in Section 6.

2. Integrated Navigation System of SINS/DVL/MCP/PS

The integrated navigation system of SINS/DVL/MCP/PS shown in Figure 1 is adopted, which is similar to the navigation scheme presented in [5,6]. Considering the difficulty of obtaining the aided level position in underwater conditions, the position reference system in [5,6] is abandoned.

In Figure 1, the SINS constitutes the basic navigation system providing the attitude, velocity and position of AUV through integration of the angular rate and acceleration measured by gyroscopes and accelerometers, whereas DVL, MCP and PS provide velocity, yaw, and height for data fusion with a federated Kalman filter [5,6]. In the data fusion process, subsystems constructed by SINS and aided systems, including SINS/DVL, SINS/MCP, and SINS/PS, are first built with standard Kalman liking filters (such as Kalman Filter, Extended Kalman Filter, and Unscented Kalman Filter) used for data fusion; then, a federated Kalman filter is used to fuse the information from all subsystems to obtain the estimation [5,6].

The navigation solutions of the SINS and data fusion are two independent and parallel processes. As shown in Figure 1, these two processes are connected with a closed feedback mode. The estimations from the federated Kalman filter are used to reset the navigation parameters and to compensate for sensor errors. Error propagation models are the basis of data fusion. In the following subsections, error models for each subsystem will be introduced.

2.1. Error Propagation Model of SINS

The SINS is composed of an inertial measurement unit (IMU) and a navigation solution unit. In the IMU, three gyroscopes and accelerometers are orthogonally installed to measure the vehicle’s motion with six degrees of freedom. As a self-depended navigation method, SINS can provide attitude, velocity, and position with solutions for the angular rates and accelerations. The differential equations describing the solution process are as follows [4]:

$${\dot{\mathit{C}}}_b^n={\mathit{C}}_b^n\left({\mathit{\omega }}_{nb}^b\times \right)$$

(1)

$${\dot{\mathit{V}}}^n={\mathit{C}}_b^n{\mathit{f}}^b-\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{ie}^n\right)\times {\mathit{V}}^n+{\mathit{g}}^n$$

(2)

$$\dot{L}=\frac{V_N^n}{R_M+h},\dot{\lambda }=\frac{V_N^n\text{sec}L}{R_N+h},\dot{h}=V_U^n$$

(3)

where ${\mathit{\omega }}_{nb}^b={\mathit{\omega }}_{ib}^b-{\left({\mathit{C}}_b^n\right)}^{\mathrm{T}}\left({\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{en}^n\right)$, ${\mathit{\omega }}_{en}^n={\left[\begin{array}{ccc}-V_N^n/\left(R_M+h\right)& V_E^n/\left(R_N+h\right)& V_E^n\text{tan}L/\left(R_N+h\right)\end{array}\right]}^{\mathrm{T}}$. Superscripts (or subscripts) of $n$, $b$, $i$ and $e$ denote navigation, body, inertial and earth frames. The symbol ${\mathit{A}}_{BC}^D$ denotes the projected motion vector of $\mathit{A}$ from frame $C$ to frame $B$ in frame $D$, i.e., ${\mathit{\omega }}_{en}^n$ denotes the velocity rated from the navigation frame to the earth frame in the navigation frame, ${\mathit{C}}_b^n$ is the attitude matrix in the SINS describing the converting relationship from frame $b$ to frame $n$, and ${\mathit{V}}^n={\left[\begin{array}{ccc}{V}_E^n& V_N^n& V_U^n\end{array}\right]}^{\mathrm{T}}$ is the velocity vector of SINS in navigation frame. $L$, $\lambda$, and $h$ denote the latitude, longitude, and height of SINS. $R_M$ and $R_N$denote the radius of Earth in meridian and prime, respectively, whereas the symbol “$\times$” denotes the skew symmetric matrix of the vector. In the SINS solution, Equations (1)–(3) should be discretized to update the calculation; more details can be found in [4].

Errors in attitude, velocity, and position are generated because of inherent sensor errors, including the biases of gyroscopes and accelerometers, and initial alignment errors, such as initial misalignment errors and navigation solution algorithm errors. The error propagation process of the SINS can be described as follows [4]:

$$\dot{\mathit{\varphi }}=\mathit{\varphi }\times {\mathit{\omega }}_{in}^n+\delta {\mathit{\omega }}_{in}^n-{\mathit{C}}_b^n{\mathit{\epsilon }}^b$$

(4)

$$\delta {\dot{\mathit{V}}}^n=-\mathit{\varphi }\times {\mathit{f}}^b+\left(2\delta {\mathit{\omega }}_{ie}^n+\delta {\mathit{\omega }}_{ie}^n\right)\times {\mathit{V}}^n+\left(2{\mathit{\omega }}_{ie}^n+{\mathit{\omega }}_{ie}^n\right)\times \delta {\mathit{V}}^n+{\mathit{C}}_b^n{\mathit{\nabla }}^b$$

(5)

$$\delta \dot{L}=\frac{\delta V_N^n}{R_M+h}-\delta h\frac{V_N^n}{{\left(R_M+h\right)}^2},\delta \dot{\lambda }=\frac{\delta V_N^n\text{sec}L}{R_N+h}-\delta h\frac{V_N^n\text{sec}L}{{\left(R_N+h\right)}^2}+\delta L\frac{V_N^n\text{sec}L\text{tan}L}{R_N+h},\delta \dot{h}=\delta V_U^n$$

(6)

where $\mathit{\varphi }$ denotes the misalignment defined as the Euler angle describing the rotation relationship between the calculated navigation frame $n\text{'}$ and the ideal navigation frame $n$, $\delta {\mathit{V}}^n$, $\delta L$, $\delta \lambda$, and $\delta h$ denote the errors of the velocity, latitude, longitude and height, $\delta \mathit{A}$, such as $\delta {\mathit{\omega }}_{ie}^n$, denotes the error of vector $\mathit{A}$, and ${\mathit{\epsilon }}^b$ and ${\mathit{\nabla }}^b$ are the errors of gyroscopes and accelerometers projected in frame $b$. In Equations (4)–(6), the error of gravity is ignored.

The precision of inertial sensors is the key element to determine the precision of the SINS; generally, the errors in the sensors can be classified as constant and random errors. Selecting a suitable model to describe inertial sensors is a challenge; different sensors have different characteristics. In engineering, the one-order (or high-order) Markov process and white noise are used to propagate random noise. In this paper, the main target is to pretreat the DVL measurement, and white Gaussian noise is selected to simplify the discussion. Thus, the error propagation models of gyroscopes and accelerometers can be constructed as follows:

$${\dot{\mathit{\epsilon }}}^b=0,{\dot{\mathit{\nabla }}}^b=0$$

(7)

The statistical characteristics of the random noise of gyroscopes and accelerometers, denoted as ${\mathit{w}}_g^b$ and ${\mathit{w}}_a^b$, are as follows:

$${\mathit{w}}_g^b~\left(0, {\mathit{Q}}_g^b\right),{\mathit{w}}_a^b~\left(0,{\mathit{Q}}_a^b\right)$$

(8)

where ${\mathit{Q}}_g^b$ and ${\mathit{Q}}_a^b$ are the variance matrices of white noise corresponding to ${\mathit{w}}_g^b$ and ${\mathit{w}}_a^b$, respectively.

2.2. Error Propagation Model of DVL

In DVL, Doppler effects are used to measure the velocity. With four acoustic beams in fixed directions, named the Janus configuration, three velocities along the body frame axes can be measured. Generally, the accuracy of the upward velocity is lower than those in the level direction. The precision of DVL is affected by several factors, such as installation error, scale factor error, and frequency measurement errors. For the purposes of this paper, the constant errors are assumed to be well-compensated. The random error in the DVL caused by acoustic scattering and environment changing is the focus. In engineering, random error can be described by a one-order Markov process or white Gaussian noise. To simply the analysis, white Gaussian noise is selected, and the statistical characteristics are as follows [5,6]:

$${\mathit{w}}_{\text{DVL}}^b~\left(0,{\mathit{R}}_{\text{DVL}}^b\right),$$

(9)

where ${\mathit{w}}_{\text{DVL}}^b$ is the random errors in DVL, and ${\mathit{R}}_{\text{DVL}}^b$ is the variance matrix of white noise corresponding to ${\mathit{w}}_{\text{DVL}}^b$.

2.3. Error Propagation Models of MCP and PS

MCP and PS are important aided navigation systems. To shorten the length of this paper, the characteristics of the MCP and PS measurements are not given in detail but can be found in [5,6,12]. Similarly to DVL, white Gaussian noise is used to describe the measuring noise in MCP and PS as follows:

$$w_{\text{MCP}}~\left(0,R_{\text{MCP}}\right)$$

(10)

$$\delta {\dot{h}}_{\text{PS}}=0, w_{\text{PS}}~\left(0,R_{\text{PS}}\right),$$

(11)

where $w_{\text{MCP}}$ is the random errors in MPC, and $R_{\text{MCP}}$ is the variance of the white noise corresponding to $w_{\text{MCP}}$. $w_{\text{PS}}$ is the random noise in PS, and $R_{\text{PS}}$ is the variance of the white noise corresponding to $w_{\text{PS}}$.

In AUV, SINS, DVL, MPC, and PS are discrete systems and operate independently; thus, the measurement noise of each is assumed to be non-related.

3. Data Fusion in the Integration of SINS/DVL/MCP/PS

3.1. Data Fusion with the Subsystems

Considering the computational complexity of the data fusion process, the federated Kalman filter is used for data fusion. As shown in Figure 1, the subsystems are constructed with SINS and other aided systems, i.e., DVL, MCP, and PS. The standard Kalman filter is selected as the data fusion method for each subsystem. The algorithm of the Kalman filter and the state equations and measurement equation of subsystems are introduced as follows.

3.1.1. Standard Kalman Filter

The discrete random linear model without controlling terms can be described as follows:

$$\{\begin{array}{l}{\mathit{X}}_k={\mathit{\Phi }}_{k,k-1}{\mathit{X}}_{k-1}+{\mathit{W}}_k\\ {\mathit{Z}}_k={\mathit{H}}_k{\mathit{X}}_{k-1}+{\mathit{V}}_k\end{array},$$

(12)

where $\mathit{X}$ is the state vector, $\mathit{Z}$ is the measurement vector, $\mathit{\Phi }$ is the system dynamic model (or state transition matrix), $\mathit{H}$ is the measurement matrix. $\mathit{W}~\left(0,\mathit{Q}\right)$ denotes the system noise with the variance of $\mathit{Q}$, $\mathit{V}~\left(0,\mathit{R}\right)$ denotes the measurement noise with variance of $\mathit{R}$, and $k$ denotes the $k^{th}$ time-step.

When the system noise and measurement noise are non-related and $\mathit{Q}\ge 0$ and $\mathit{R}>0$, the vector ${\widehat{\mathit{X}}}_k$ can be estimated by the standard Kalman filter. The equations in the standard Kalman filter are as follows:

$$\{\begin{array}{l}{\widehat{\mathit{X}}}_{k,k-1}={\mathit{\Phi }}_{k,k-1}{\widehat{\mathit{X}}}_{k-1}\\ {\mathit{P}}_{k,k-1}={\mathit{\Phi }}_{k,k-1}{\mathit{P}}_{k-1}{\mathit{\Phi }}_{k,k-1}^{\mathrm{T}}+{\mathit{Q}}_{k-1}\\ {\mathit{K}}_k={\mathit{P}}_{k,k-1}{\mathit{H}}_k^{\mathrm{T}}{\left[{\mathit{H}}_{k,k-1}{\mathit{P}}_{k,k-1}{\mathit{H}}_{k,k-1}^{\mathrm{T}}+{\mathit{R}}_k\right]}^{-1}\\ {\widehat{\mathit{X}}}_k={\widehat{\mathit{X}}}_{k,k-1}+{\mathit{K}}_k\left[{\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k,k-1}\right]\\ {\mathit{P}}_k=\left[\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right]{\mathit{P}}_{k,k-1}{\left[\mathit{I}-{\mathit{K}}_k{\mathit{H}}_k\right]}^{\mathrm{T}}+{\mathit{K}}_k{\mathit{R}}_k{\mathit{K}}_k^{\mathrm{T}}\end{array}$$

(13)

where $\widehat{\mathit{X}}$ is the estimation of $\mathit{X}$, $\mathit{P}$ is the matrix of state variance, ${\widehat{\mathit{X}}}_{k,k-1}$ and ${\mathit{P}}_{k,k-1}$ are the one-step predictions at time $k$ with ${\widehat{\mathit{X}}}_{k-1}$ and ${\mathit{P}}_{k-1}$ at time $k-1$, $\mathit{K}$ denotes the gain matrix, and $\mathit{I}$ is a unit matrix with corresponding dimensions.

3.1.2. System and Measurement Equations of SINS/DVL

Assuming the constant error in DVL is completely compensated, misalignment angles, velocity and position errors, and gyroscope and accelerometer biases in SINS are chosen as components of the state vector ${\mathit{X}}_{\text{SINS/DVL}}$:

$$\mathit{X}={\left[\begin{array}{ccccccccccccccc}\delta V_E^n& \delta V_N^n& \delta V_U^n& {\varphi }_E& {\varphi }_N& {\varphi }_U& \delta L& \delta \lambda & \delta h& {\epsilon }_x^b& {\epsilon }_y^b& {\epsilon }_z^b& {\nabla }_x^b& {\nabla }_y^b& {\nabla }_z^b\end{array}\right]}^{\mathrm{T}}$$

(14)

When the elements of the state vector are fixed, the state transition matrix defined in Equation (12) can be acquired by the discretization of Equations (3)–(6), and the variance matrix $\mathit{Q}$ can be set according to the sensor precision.

Take the velocity from DVL as the aided velocity, and the measurement vector can be constructed as:

$${\mathit{Z}}_{\text{SINS/DVL}}={\left[\begin{array}{ccc}{V}_E^n-V_{\text{DVL}E}^n& V_N^n-V_{\text{DVL}N}^n& V_U^n-V_{\text{DVL}U}^n\end{array}\right]}^{\mathrm{T}}$$

(15)

where $V_{\text{DVL}E~U}^n$ denotes the projected velocity in frame $n$, which can be calculated as ${\mathit{V}}_{\text{DVL}}^n={\mathit{C}}_b^n{\mathit{V}}_{\text{DVL}}^b$, and ${\mathit{V}}_{\text{DVL}}^b$denotes the measured DVL velocity in frame $b$.

According to the relationship between the measurement and state vector, ${\mathit{H}}_{\text{SINS/DVL}}\left(t\right)$ can be expressed as:

$${\mathit{H}}_{\text{SINS/DVL}}=\left[\begin{array}{cccc}1& 0& 0& \\ 0& 1& 0& 0_{3\times 12}\\ 0& 0& 1& \end{array}\right]$$

(16)

The variance matrix $\mathit{R}$ corresponding to measurement noise $\mathit{V}$ in Equation (12) is set according to the random noise in ${\mathit{V}}_{\text{DVL}}^n$. From the projection process ${\mathit{V}}_{\text{DVL}}^n={\mathit{C}}_b^n{\mathit{V}}_{\text{DVL}}^b$, it can be observed that there is cross-noise between ${\mathit{C}}_b^n$ and ${\mathit{V}}_{\text{DVL}}^b$. In this paper, the enlarged variance of $\mathit{V}$ is used to include the crossed noise, and more details to address this noise can be found in [12]. In the following sections, this noise is not considered, and SINS and DVL are assumed to be independent.

3.1.3. System and Measurement Equations of SINS/MCP and SINS/PS

Similarly, the system and measurement equations of SINS/MCP and SINS/PS can be constructed. In Equation (14), only the error in SINS is considered, so the system equations of SINS/MCP and SINS/PS are the same with that of SINS/DVL. The measurement vector of SINS/MCP and ${\mathit{H}}_{\text{SINS/MCP}}\left(t\right)$ can be constructed as:

$$Z_{\text{SINS/MCP}}=\psi -{\psi }_{\text{MCP}}$$

(17)

$${\mathit{H}}_{\text{SINS/DVL}}=\left[\begin{array}{ccc}{0}_{1\times 5}& -1& 0_{1\times 9}\end{array}\right],$$

(18)

where $\psi$ and ${\psi }_{\text{MCP}}$ denote the yaw in SINS and MCP, respectively. The variance matrix $\mathit{R}$ corresponding to measurement noise $\mathit{V}$ in Equation (12) is set according to the random noise in ${\psi }_{\text{MCP}}$.

The measurement vector of SINS/PS and ${\mathit{H}}_{\text{SINS/PS}}\left(t\right)$ can be constructed as:

$$Z_{\text{SINS/PS}}=h-h_{\text{PS}}$$

(19)

$${\mathit{H}}_{\text{SINS/DVL}}=\left[\begin{array}{ccc}{0}_{1\times 8}& -1& 0_{1\times 6}\end{array}\right],$$

(20)

where h and h_PS are the heights in SINS and PS, respectively. The variance matrix R corresponding to measurement noise $\mathit{V}$ in Equation (12) is set according to the random noise in h_PS.

3.2. Data Fusion of SINS/DVL/MCP/PS

The estimated vectors ${\widehat{\mathit{X}}}_{\text{SINS/DVL}}$, ${\widehat{\mathit{X}}}_{\text{SINS/MCP}}$, and ${\widehat{\mathit{X}}}_{\text{SINS/PS}}$ from the subsystems should be further fused in the federated Kalman filter, named the main filter, to acquire the optimal estimation. As shown in Figure 2, the federated filter with non-feedback to the subsystems is chosen. The main advantage of this structure lies in its low complexity and non-cross interaction between each subsystem.

With no feedback, the main filter performs data fusion without filtering. The algorithm running in the main filter is as follows:

$$\{\begin{array}{l}{\mathit{P}}_G={\left({\mathit{P}}_{\text{SINS/DVL}}^{-1}+{\mathit{P}}_{\text{SINS/MCP}}^{-1}+{\mathit{P}}_{\text{SINS/PS}}^{-1}\right)}^{-1}\\ {\widehat{\mathit{X}}}_G={\mathit{P}}_G\left({\mathit{P}}_{\text{SINS/DVL}}^{-1}{\widehat{\mathit{X}}}_{\text{SINS/DVL}}+{\mathit{P}}_{\text{SINS/MCP}}^{-1}{\widehat{\mathit{X}}}_{\text{SINS/MCP}}+{\mathit{P}}_{\text{SINS/PS}}^{-1}{\widehat{\mathit{X}}}_{\text{SINS/PS}}\right)\end{array}$$

(21)

The above equation requires that all subsystems are independent and that there is cross coupling. The DVL, MPC, and PS in this paper are run independently and satisfy this requirement.

4. Velocity Tracing Method for DVL Based on Motion Constraint

4.1. Analysis of the Relationship between the DVL Error and SINS Navigation Accuracy

With the above integrated navigation scheme, the errors in DVL, MCP, and PS introduce estimation errors in both the subsystems and the main filter, and navigation errors of the estimation are fed-back to SINS. In SINS/DVL/MCP/PS, because there is no external position-aided information, the accuracy of the level position is mainly determined by the accuracy of DVL.

In the integration of SINS/DVL, constant error in the aided velocity leads to constant error in the measurement vector through velocity matching. Moreover, the constant error in measurement vector leads to estimation error in the velocity, i.e., error in the SINS velocity. Then, the velocity error generates constant errors in misalignment and constant errors in attitude, which cause analytical errors in the specific force. With integration, errors in the specific force cause velocity and position errors. Due to the lack of aided position information, the position errors cannot be corrected.

Due to the significantly negative role of the constant errors in DVL, they should be effectively compensated in engineering. For the specific working conditions shown in Figure 3, taking the work-mode against the seafloor of DVL as an example, the following scenarios exist: (1) when the AUV sails across the seafloor gully, the velocity from the DVL suddenly changes to zero because the distance between the AUV and the seafloor exceeds the measuring range; (2) when the AUV sails across a fish population, the velocity from DVL suddenly changes from the relative velocity between the AUV and the seafloor to that between the AUV and the fish. Unfortunately, these sudden changes cannot be predicted. As explained above, the sudden changes in DVL velocity cause position errors that cannot be corrected. How to judge and compensate these sudden changes plays an important role in increasing the positioning accuracy. In engineering, these sudden changes are considered as faults, and fault diagnosis based on ${\chi }^2$ rules can be adopted to evaluate them. However, when these sudden changes are identified and isolated, finding a substitution to data fusion becomes a new problem.

Similarly to constant errors, random errors in DVL have also attracted significant attention. Several self-adaptive filters have been presented from the aspects of error parameter modeling. Optimal estimation can be obtained only when the model and statistical parameters are consistent with actual ones [3,4,5,6,12,14]. However, how to restrain random noise with mathematical methods has received little attention. The random noise in DVL can be treated as a constant error lasting for a short time, which also introduces errors in the SINS attitude, velocity, and position. Due to the short duration, the random noise with positive and negative changes is smoothed with integrated operation; thus, the position error caused by random noise is small. However, the errors in attitude and velocity caused by DVL random noise cannot be smoothed. Therefore, the variances of the SINS attitude and the velocity errors are partly determined by the variances of the DVL random noise. When SINS/DVL acts as an attitude reference system, random noise in DVL decreases its accuracy.

Similar analysis can be performed with MCP and PS. For the purposes of this paper, the constant errors in MCP and PS are assumed to be fully compensated, the random errors are well modeled, and the static parameters are effective.

To solve the problems of sudden changes and random noise in DVL, a velocity tracing algorithm is proposed based on the moving characteristics of the AUV in the next section.

4.2. Velocity Tracing Algorithm for DVL Based on AUV Motion Constraints

When the AUV sails, it is driven by an engine, steering, and currents. It is difficult to construct an accurate model describing the motion relationship between the input and output. However, for AUVs conducting long time and distance sailing tasks, they prefer to sail at an economic velocity. Therefore, the driving force from the engine and steering is constant. If the circle wave is ignored and only linear current is considered, the velocity and (or) acceleration of the AUV along the body frame can be addressed as a constant or slowly changing value [17].

To simplify the analysis, the current is assumed to be zero. In this condition, the velocity along the forward direction of the AUV is a fixed value. When the “right-forward-up” frame is adopted as the body frame, the velocity along the y-axis can be considered as a constant value and acceleration as zero. In this condition, the velocity $V_{by}$ and acceleration $a_{by}$ can be selected as the state vector, and the measured velocity from the DVL can be used as the measurement vector. Then, the system and measurement equations can be constructed as:

$$\{\begin{array}{l}{\dot{\mathit{X}}}_{by}\left(t\right)=\left[\begin{array}{c}{V}_{by}\\ a_{by}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]{\mathit{X}}_{by}\left(t\right)+\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]{\mathit{w}}_{by}\left(t\right)\\ {\mathit{Z}}_{by}\left(t\right)=\left[\begin{array}{cc}1& 0\end{array}\right]{\mathit{X}}_{by}\left(t\right)+{\mathit{v}}_{by}\left(t\right)\end{array},$$

(22)

where ${\mathit{Z}}_{by}=\sqrt{{\left(v_{\text{DVL}}^{bx}\right)}^2+{\left(v_{\text{DVL}}^{by}\right)}^2+{\left(v_{\text{DVL}}^{bz}\right)}^2}$, ${\mathit{w}}_{by}={\left[\begin{array}{cc}{w}_{v_{by}}& w_{a_{by}}\end{array}\right]}^{\mathrm{T}}$ denotes the disturbance caused by the excitation of the wave, which can be considered as system noise, and ${\mathit{v}}_{by}$ is considered measurement noise. After discretization of Equation (22), the time and measurement can be updated with Equation (13), and the instant DVL velocity ${\widehat{V}}_{by}$ is estimated. When the line current is considered, the velocity along both the x-axis and z-axis should be managed with the same model of Equation (22).

When the estimation ${\widehat{V}}_{by}$ along the y-axis of the AUV is obtained, the velocity of the AUV in the body frame can be constructed as ${\widehat{\mathit{V}}}_{\text{DVL}}^b={\left[\begin{array}{ccc}0& {\widehat{V}}_{by}& 0\end{array}\right]}^{\mathrm{T}}$. After this velocity vector is projected into the navigation frame with the SINS attitude matrix, ${\mathit{C}}_b^n{\widehat{\mathit{V}}}_{\text{DVL}}^b$ can be used for data fusion in the SINS/DVL.

Based on the above model, the random noise is effectively restrained, which will be proved in the next section. When there is a short, sudden change in DVL velocity, the time update velocity can be used as the substitute velocity to perform data fusion. Then, how to judge this sudden change becomes a problem.

Fault diagnosis with ${\chi }^2$ rules is a common method to detect sudden changes in engineering [16]. In this method, the measurement at time $k-1$ is assumed without a sudden change, and the prediction ${\widehat{\mathit{X}}}_{k,k-1}$ with state vector at $k-1$ is without error. If the measurement at $k$ has only random noise, the innovation (also defined as the residual error) is a white noise sequence. However, when the measurement ${\mathit{Z}}_k$ at $k$ has a sudden change, the innovation undergoes a large change. When the change is above a particular threshold, it can be deduced that a sudden change occurs.

Defining the innovation as

$${\mathit{r}}_k={\mathit{Z}}_k-{\mathit{H}}_k{\widehat{\mathit{X}}}_{k,k-1}$$

(23)

and the covariance matrix as

$${\mathit{A}}_k={\mathit{H}}_k{\mathit{P}}_{k,k-1}{\mathit{H}}_k^T+{\mathit{R}}_k,$$

(24)

the detected parameter can be defined as:

$${\gamma }^k={\mathit{r}}_k^{\mathrm{T}}{\mathit{A}}_k^{-1}{\mathit{r}}_k.$$

(25)

When threshold $D$ is selected, the decision rules can be constructed as:

$$\{\begin{array}{l}{\gamma }^k\ge D\text{\hspace{1em}with\hspace{0.17em}a\hspace{0.17em}sudden\hspace{0.17em}change}\\ {\gamma }^k<D’\text{\hspace{0.17em}without\hspace{0.17em}a\hspace{0.17em}sudden\hspace{0.17em}change}\end{array}.$$

(26)

5. Simulations

5.1. Simulation Setting

The sensors used in SINS are assumed to have mid-precision. The constant bias and random noise of gyroscopes and accelerometers are illustrated in

Table 1. Sensor errors.

Axes	Gyro Bias (°/h)		Acce Bias (ug)
	Constant	Random (White Noise)	Constant	Random (White Noise)
x-axis	0.01	0.01	50	50
y-axis	0.01	0.01	50	50
z-axis	0.01	0.01	50	50

. The AUV sails along the y-axis with a velocity of 5 m/s. Driven by interference of the current, the AUV sails with a swinging motion, and the swinging motion has the function $A\text{ sin}(2\pi f\cdot t+{\eta }_0)+{\theta }_0$, in which $A$ and $f$ are the amplitude and frequency of the swinging and ${\eta }_0$ and ${\theta }_0$ are the initial phase and swinging center. The swinging parameters are shown in

Table 2. Swinging parameters.

Parameters	Pitch	Roll	Yaw
Amplitude (°)	1.5	1.5	1.0
Cycle (s)	8	7.5	6
Initial Phase (°)	0	0	0
Swinging center (°)	0	0	0

. The initial longitude, latitude, and height of the AUV are set as 118° E, 32° N, and −20 m.

Based on the ideal motion, the theoretical sensor outputs can be obtained by back-stepping of the navigation solution. When the sensor errors are added into the theoretical outputs, the real sensor data can be simulated. To assess the navigation accuracy, the ideal motion can be used as a reference. During the simulation, the update frequency of the sensor data and navigation solution are both 100 Hz.

In the simulation, without considering the constant error, the random noise in DVL, MCP, and PS is assumed as white noise with variances of 0.5 m/s, 2°, and 0.5 m. When these errors are added to the ideal motion, the real outputs of the DVL, MCP, and PS can be simulated. During simulation, the update frequency of the DVL, MPC, and PS is set to 10 Hz.

Straight line trajectory is chosen in the simulation, and the velocity is set as above because this trajectory has no canceling effect on error growth [5,6]. To simulate the sudden velocity change in DVL, velocities of 2 m/s, 5 m/s, and 1 m/s are added to the real output of the DVL along three directions during the period 2000–2010 s.

When the initial parameters for SINS are set, the integrated navigation, including the initial alignment, is executed. Among those initial parameters, the initial velocity and position are set without any error, whereas the attitude has misalignment angles of 0.2°, 0.2°, and 1.0° in the pitch, roll and yaw.

The parameters for the Kalman filter in Section 2 are set as:

$${\mathit{X}}_{\text{SINS/DVL0}}={\left[\begin{array}{ccccccccccccccc}0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]}^{\mathrm{T}},$$

$${\mathit{P}}_{\text{SINS/DVL}0}=\text{diag}\left[\begin{array}{cccccccccc}{\left(0.5\text{m}/\text{s}\right)}^2& {\left(0.5\text{m}/\text{s}\right)}^2& {\left(0.5\text{m}/\text{s}\right)}^2& {\left(5^{\circ }\right)}^2& {\left(5^{\circ }\right)}^2& {\left({15}^{\circ }\right)}^2& {\left(300/R_e\right)}^2& {\left(300/R_e\right)}^2& {\left(1\text{m}\right)}^2& \\ & & & & {\left(500\text{ug}\right)}^2& {\left(500\text{ug}\right)}^2& {\left(500\text{ug}\right)}^2& {\left(5^{\circ }/\text{h}\right)}^2& {\left(5^{\circ }/\text{h}\right)}^2& {\left(5^{\circ }/\text{h}\right)}^2\end{array}\right],$$

$${\mathit{Q}}_{\text{SINS/DVL}}=\text{diag}\left[\begin{array}{ccccccccccccccc}{\left(50\text{ug}\right)}^2& {\left(50\text{ug}\right)}^2& {\left(50\text{ug}\right)}^2& {\left({0.01}^{\circ }\right)}^2& {\left({0.01}^{\circ }\right)}^2& {\left({0.01}^{\circ }\right)}^2& {\left(30/R_e\right)}^2& {\left(30/R_e\right)}^2& {\left(0.1m\right)}^2& 0& 0& 0& 0& 0& 0\end{array}\right],$$

$${\mathit{X}}_{\text{SINS/DVL0}}={\mathit{X}}_{\text{SINS/MCP0}}={\mathit{X}}_{\text{SINS/PS0}},{\mathit{P}}_{\text{SINS/DVL}0}={\mathit{P}}_{\text{SINS/MCP}0}={\mathit{P}}_{\text{SINS/PS}0},{\mathit{Q}}_{\text{SINS/DVL}}={\mathit{Q}}_{\text{SINS/MCP}}={\mathit{Q}}_{\text{SINS/PS}},$$

$${\mathit{R}}_{\text{SINS/DVL}}=\text{diag}\left[\begin{array}{ccc}{\left(0.5\text{m}/\text{s}\right)}^2& {\left(0.5\text{m}/\text{s}\right)}^2& {\left(0.5\text{m}/\text{s}\right)}^2\end{array}\right],{\mathit{R}}_{\text{SINS/MCP}}={\left({0.5}^{\circ }\right)}^2\text{ and }{\mathit{R}}_{\text{SINS/PS}}={\left(0.5m\right)}^2$$

Two schemes are compared. In Scheme 1, the data fusion method introduced in Section 2 is simulated. In Scheme 2, the data fusion method with the velocity tracing method in Section 3, with the detecting threshold set at 50, is adopted.

In Scheme 2, the parameters for the velocity tracing filter are set as ${\mathit{X}}_{by0}={\left[\begin{array}{cc}{V}_{by}& 0\end{array}\right]}^{\mathrm{T}}$, ${\mathit{P}}_{by0}={\left[\begin{array}{cc}1000& 1000\end{array}\right]}^{\mathrm{T}}$, ${\mathit{Q}}_{by0}={\left[\begin{array}{cc}0.0005& 0.005\end{array}\right]}^{\mathrm{T}}$ and ${\mathit{R}}_{by0}={\left(0.5\sqrt{3}\right)}^2$.

5.2. Simulation Results and Analysis

The simulation runs for 3600 s, and the simulation results are given in Figure 4, Figure 5, Figure 6, Figure 7 and Figure 8. The velocity and traced velocity in the body frame along the y-axis and in the navigation frame are listed in Figure 4 and Figure 5, where solid lines and dashed lines denote the velocity before and after tracing, respectively, named Schemes 1 and 2. The statistic results for Schemes 1 and 2 are shown in

Table 3. Statistical results of the Doppler Velocity Log (DVL) velocity (m/s).

Statistical Items	Velocity along the y-axis			East Velocity			North Velocity			Upward Velocity
	Sch. 1	Sch. 2	Ideal	Sch. 1	Sch. 2	Ideal	Sch. 1	Sch. 2	Ideal	Sch. 1	Sch. 2	Ideal
Mean	5.0002	5.0033	5	0.0005	0	0	5.0009	5.0005	5.0008	0.0004	0	0
Std	0.4975	0.0220	0	0.4996	0.0617	0.0618	0.5001	0.0020	0.0220	0.5153	0.0926	0.0925

. In Scheme 1, the data during 2000–2010 s are omitted because of the sudden change in velocity.

The curves in Figure 4 and the statistic results in Table 3 indicate that before and after tracing, the means of both velocities are similar, but the standard variances are significantly different, which means the random noise in the DVL velocity can be significantly restrained by the velocity tracing algorithm. The curves in Figure 5 indicate that when the traced velocity in the body frame is projected into the navigation frame, the random noise can be restrained. The partial enlarged drawing in Figure 4 and Figure 5 indicates that the traced velocities almost coincide with the theoretical ones. When a sudden change occurs, the fault diagnosis algorithm based on ${\chi }^2$ rules can judge this sudden change immediately, and the time update of the tracing algorithm can be used as a substitute output of the DVL.

The navigation results of the integrated SINS/DVL/MCP/PS are shown in Figure 6, Figure 7 and Figure 8, where the attitude, velocity and position errors are identified, and solid lines and dashed lines denote Schemes 1 and 2, respectively. To acquire a direct display, the unit of the position error in the level position is transformed from degrees to meters. The curves in Figure 6c coincide with each other because the same yaw from MCP is used in both schemes.

The curves in Figure 6, Figure 7 and Figure 8 indicate that when there is a sudden change in the DVL velocity, with Scheme 1, sudden changes in attitude, velocity, and position are introduced. When the sudden change in velocity is eliminated, the sudden change in attitude and velocity can be restrained gradually, whereas the sudden change in the level position cannot be restrained because it is an open loop. The height error is restrained because the calculation for height is a closed loop. With the height from the PS, the sudden change in height caused by a sudden change in DVL velocity is corrected.

The curves in Figure 6, Figure 7 and Figure 8 further indicate that with Scheme 2, the amplitude of the attitude and velocity errors is decreased, and the sudden change in DVL velocity does not generate a sudden change in attitude, velocity and position, as in Scheme 1. The statistical results are shown in

Table 4. Statistic results of the navigation errors.

Statistical Items		Pitch	Roll	Yaw	East Velocity	East Velocity	East Velocity	Height
Scheme 1	Mean	−0.2208	0.0935	−0.3450	0	0.0015	−0.0012	−0.4873
	Std	1.0416	1.0835	3.9327	0.0406	0.0418	0.0073	0.2589
Scheme 2	Mean	−0.2232	0.1092	−0.3507	0	0.0020	−0.0015	−0.4907
	Std	0.6773	0.5885	4.0719	0.0056	0.0231	0.0061	0.2540

. Considering the adjustment period when a sudden change disappears, only the data in the period of 500–2000 s are used. The statistics in Table 4 show that the tracing algorithm can effectively restrain the random noise in the DVL velocity.

From the above analysis, it can be concluded that with the velocity tracing method, the random noise in the DVL velocity can be effectively restrained, and the accuracy of the attitude and velocity can be increased. With the substituted velocity from the velocity tracing method, the position accuracy can be improved when a sudden change in the DVL velocity occurs.

6. Conclusions

An integrated navigation scheme of SINS/DVL/MCP/PS for AUVs is presented in this paper, and the error propagation models and data fusion schemes are studied. The analysis indicates that under submarine conditions, the position accuracy is mainly determined by the accuracy of the DVL because of the shortness of the external reference level position information. The negative effects caused by the random noise and sudden change in the measurements of the DVL velocity are investigated. The analysis shows the random noise in the DVL causes random oscillation in the level attitude and velocity and decreases their accuracy. In addition, the sudden change caused by seafloor gullies and fish populations generate sudden changes in the level position that cannot be corrected and significantly decrease the accuracy of the level position.

To solve these problems, the characteristics of the AUV when sailing for a long time and a long distance are analyzed, and it is concluded that the AUV always sails at a constant and economic velocity or changes slowly. The acceleration along the axes of the body frame can be assumed to be as zero or nearly zero. Then, a velocity tracing algorithm based on a constant velocity motion model is designed to restrain the random noise in the velocity from the DVL, in which the velocity disturbance caused by the wave is considered as system noise. Then, the fault diagnosis based on ${\chi }^2$ rules is introduced to judge the sudden change, and the time update velocity in the model is used to perform data fusion instead of the velocity from the DVL when the sudden change occurs.

Simulation results based on the straight line trajectory indicate that with this velocity tracing algorithm, the random noise and sudden change in DVL can be effectively restrained, the standard variance in the level attitude and velocity can be decreased, and the level position accuracy can be ensured.