AUV Positioning Method Based on Tightly Coupled SINS/LBL for Underwater Acoustic Multipath Propagation

This paper researches an AUV (Autonomous Underwater Vehicle) positioning method based on SINS (Strapdown Inertial Navigation System)/LBL (Long Base Line) tightly coupled algorithm. This algorithm mainly includes SINS-assisted searching method of optimum slant-range of underwater acoustic propagation multipath, SINS/LBL tightly coupled model and multi-sensor information fusion algorithm. Fuzzy correlation peak problem of underwater LBL acoustic propagation multipath could be solved based on SINS positional information, thus improving LBL positional accuracy. Moreover, introduction of SINS-centered LBL locating information could compensate accumulative AUV position error effectively and regularly. Compared to loosely coupled algorithm, this tightly coupled algorithm can still provide accurate location information when there are fewer than four available hydrophones (or within the signal receiving range). Therefore, effective positional calibration area of tightly coupled system based on LBL array is wider and has higher reliability and fault tolerance than loosely coupled. It is more applicable to AUV positioning based on SINS/LBL.


Introduction
Autonomous underwater vehicle (AUV) is a tool that is competent for various underwater tasks, such as detection, attack, convey, salvage, etc. It has become an important research direction of military marine technology at home and abroad because of its wide scope, small volume, lightweight and high hidden ability. High-accuracy underwater autonomous navigation, locating and tracking technology of AUV are precondition and key to accomplish underwater tasks [1].
Currently, SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) integrated navigation is the main underwater autonomous navigation technology used in AUV in both China and foreign countries. With calibrations based on global positioning system (GPS), acoustic positioning system and magnetic compass, it can improve navigation accuracy of an AUV system effectively [2][3][4]. Acoustic positioning system can be divided into long base line, short base line and ultra-short base line systems according to distance between elements. Among them, the long base line (LBL) acoustic positioning system determines AUV position according to distance between sound source on the carrier and seabed transponder array. It has been widely used in underwater vehicles for its wide reach and high positional accuracy [5,6].
SINS/LBL integrated navigation can be divided into loosely coupled and tightly coupled in view of involved physical parameters. In some previous studies, SINS/LBL loosely coupled navigation [7,8] inhibits divergence of position error by taking difference of location information between SINS and LBL as quantity measurement of Kalman filter. Although it could increase positional accuracy of AUV effectively, the LBL could not complete the positioning when there are fewer than four available hydrophones because of carrier movement or shoal of fish. Research on SINS/LBL tightly coupled navigation is represented by Lee et al. [9] from South Korean Vessel and Marine Engineering Institute. He proposed an AUV integrated navigation system based on inertial sensor and pseudo-LBL acoustic transponder. It added pseudo-LBL system based on the SINS/DVL system. The pseudo-LBL system is composed of sound source on AUV and two hydrophones at seabed. The distance between sound source and each hydrophone is called pseudo-range. The differences between SINS derived pseudo-ranges and LBL measured pseudo-ranges are used as observation variables of Kalman filter. This method improves effectiveness, robustness and navigation accuracy of SINS/DVL integrated navigation system. The observed information increased by Lee et al. is the distance, which is obtained by the sound velocity multiplying the time of arrival (TOA) between sound source and hydrophone. Acquisition of time requires strict synchronization on time for transponder and hydrophones, which is difficult in practice. Therefore, distance measurement error is inevitable. Method based on time difference of arrival (TDOA) determines position of carrier according to time differences for sound source signal arriving to different hydrophones, which lowers requirements on time synchronization and avoids error brought by time synchronization between sound source and hydrophones. Therefore, this paper applied TDOA-based LBL acoustic system to assist SINS positioning.
TDOA is often acquired through generalized cross-correlation operation to signals receiving by two hydrophones. Since sound signal sent from sound source will make a series of refraction and reflection during the propagation process, signals received by hydrophones are actually mutual interference superposition of multipath signals, thus resulting in multiple correlation peaks with similar amplitudes. This is known as fuzzy correlation peak problem. To solve this problem, An et al. [10] proposed a defuzzification algorithm based on correlation peaks identification and stable correlation peaks tracking according to stability and distribution law of correlation peaks generated by different paths. This algorithm could correct TDOA estimation error brought by multipath propagation. However, the distribution of the correlation peak varies with the change of the water environment, so the proposed tracking algorithm may still cause estimation error.
To solve these problems, this paper put forward an AUV positioning system based on SINS/LBL tightly coupled algorithm of underwater acoustic propagation multipath. This system is composed of SINS, DVL, MCP, sound source and LBL acoustic positioning array. Hydrophones in LBL array receive sound source signal from AUV. A group of fuzzy correlation peaks could be gained from correlation operation of the received signals. These correlation peaks are distinguished through location information of SINS and assistance of LBL, thus getting TDOA. Later, TDOA is changed into slant-range difference. Difference between this slant-range difference and the slant-range difference calculated from SINS position is taken as the observation variables of tightly coupled system, thus correcting accumulative position error of AUV. This system uses SINS assisting to solve TDOA calculation difficulty caused by multipath propagation of underwater sound signal. Furthermore, using slant-range difference as an observation variable for filtering correction of system error, the system expands navigational correction area of AUV and makes navigation track more flexible.
In this paper, Section 2 introduces principle and structure of the system. Then, calculation method of TDOA of underwater acoustic propagation multipath and SINS/LBL tightly coupled model based on slant-range difference are introduced. Finally, some simulation experiments are implemented to prove effectiveness of the proposed algorithm.

Principle of TDOA-Based LBL Underwater Positioning
LBL system consists of sound source on AUV and hydrophones at seabed. Distance between hydrophones is generally 100~6000 m ( Figure 1).
Common positioning algorithms in LBL acoustic positioning system include TOA positioning algorithm and TDOA positioning algorithm. TOA positioning algorithm locates AUV according to distances from sound source to hydrophones. This algorithm is easy to operate, but requires strict time synchronization between sound source and hydrophones. The TDOA positioning algorithm locates AUV according to time difference for sound signal to arrive to different hydrophones. It avoids error caused by poor time synchronization. As a result, TDOA-based LBL underwater positioning was used in this paper. Its basic principle is introduced in the following text.
Suppose AUV sends sound signal at time 0 t and hydrophone i receives this sound signal at time i t . Then, propagation time from sound source to hydrophone is: Distance between sound source and hydrophone i is: where c is sound velocity, which is assumed a constant to simplify the analysis. Assumed positions of hydrophones 0, 1, 2 and 3 in Figure 1 are known, which are, respectively, expressed as 0 0 0 0 ( , , ) P x y z , 1 1 1 1 ( , , ) P x y z , 2 2 2 2 ( , , ) P x y z and 3 3 3 3 ( , , ) P x y z . Position of AUV ( , , ) P x y z is unknown that has to be calculated. Then, distance between sound source and hydrophone i could be expressed as: where c and  i t are known. Therefore, ( , , ) P x y z can be obtained by solving the following nonlinear equation: Common positioning algorithms in LBL acoustic positioning system include TOA positioning algorithm and TDOA positioning algorithm. TOA positioning algorithm locates AUV according to distances from sound source to hydrophones. This algorithm is easy to operate, but requires strict time synchronization between sound source and hydrophones. The TDOA positioning algorithm locates AUV according to time difference for sound signal to arrive to different hydrophones. It avoids error caused by poor time synchronization. As a result, TDOA-based LBL underwater positioning was used in this paper. Its basic principle is introduced in the following text.
Suppose AUV sends sound signal at time t 0 and hydrophone i receives this sound signal at time t i . Then, propagation time from sound source to hydrophone is: Distance between sound source and hydrophone i is: where c is sound velocity, which is assumed a constant to simplify the analysis. Assumed positions of hydrophones 0, 1, 2 and 3 in Figure 1 are known, which are, respectively, expressed as P 0 px 0 , y 0 , z 0 q, P 1 px 1 , y 1 , z 1 q, P 2 px 2 , y 2 , z 2 q and P 3 px 3 , y 3 , z 3 q. Position of AUV Ppx, y, zq is unknown that has to be calculated. Then, distance between sound source and hydrophone i could be expressed as: b px´x i q 2`p y´y i q 2`p z´z i q 2 " c¨∆t i where c and ∆t i are known. Therefore, Ppx, y, zq can be obtained by solving the following nonlinear equation: px´x 1 q 2`p y´y 1 q 2`p z´z 1 q 2´b px´x 0 q 2`p y´y 0 q 2`p z´z 0 q 2 " cp∆t 1´∆ t 0 q b px´x 2 q 2`p y´y 2 q 2`p z´z 2 q 2´b px´x 0 q 2`p y´y 0 q 2`p z´z 0 q 2 " cp∆t 2´∆ t 0 q b px´x 3 q 2`p y´y 3 q 2`p z´z 3 q 2´b px´x 0 q 2`p y´y 0 q 2`p z´z 0 q 2 " cp∆t 3´∆ t 0 q Equation (4) is a hyperboloid locating equation. It reflects that LBL 3D positioning requires at least four hydrophones.

Working Principle of the System
Working principle of the system is shown in Figure 2. Firstly, hydrophones receive signals transmitted from the sound source on AUV. Generalized cross-correlation operation to signals received by hydrophones i and j (x i ptq and x j ptq) was implemented. Due to refractions and reflections of sound signals during underwater propagation, a group of fuzzy correlation peaks are gained from the generalized cross-correlation operation. Next, slant-range difference between hydrophones (∆R SI NS ) is estimated according to current AUV position information (P SI NS ) of SINS/DVL/MCP system, and time difference of sound source signal arrival to hydrophone i and j (t 1 ij ) is calculated. Based on t 1 ij , fuzzy correlation peaks are screened by the correlation peak screening module, getting the ideal time difference of arrival (t ij Equation (4) is a hyperboloid locating equation. It reflects that LBL 3D positioning requires at least four hydrophones.

Working Principle of the System
Working principle of the system is shown in Figure 2. Firstly, hydrophones receive signals transmitted from the sound source on AUV. Generalized cross-correlation operation to signals received by hydrophones i and j ( ( )   Figure 2. Working principle of the system.

Fuzzy Correlation Peaks in Multipath Propagation
Underwater sound signal will make a series of refractions and reflections during propagation and signal received by hydrophones is mutual interference superposition of multipath signals. Sound signal propagation channel can be modeled as a mutual interference multipath channel [11].

Fuzzy Correlation Peaks in Multipath Propagation
Underwater sound signal will make a series of refractions and reflections during propagation and signal received by hydrophones is mutual interference superposition of multipath signals. Sound signal propagation channel can be modeled as a mutual interference multipath channel [11]. Figure 3 shows a simplified underwater sound multipath propagation model, which is composed of one sound source S and two hydrophones R1 and R2. Except for the straight propagation channels P d1 and P d2 from sound source to hydrophones, sound signal is reflected by sea surface (P s1 and P s2 ) and seabed (P b1 and P b2 ) once.  Figure 3 shows a simplified underwater sound multipath propagation model, which is composed of one sound source S and two hydrophones R1 and R2. Except for the straight propagation channels Pd1 and Pd2 from sound source to hydrophones, sound signal is reflected by sea surface (Ps1 and Ps2) and seabed (Pb1 and Pb2) once.

Cross-correlation function of signals 1 ( )
x t and 2 ( ) x t received by R1 and R2 is shown in Figure   4. Sampling period is 5 × 10 −6 s. Except for peaks (main peaks) at TDOA of straight paths, there are other peaks (secondary peaks). The phenomenon that there are many peaks in cross-correlation function and it is impossible to estimate TDOA of signals is called fuzzy cross-correlation peak.

SINS-Assisted Method to Search the Optimum TDOA
According to above analysis, multipath effect of sound signal propagation brings several correlation peaks with similar amplitudes in generalized cross-correlation operation. Due to high propagation speed of sound signal in water, tiny time difference error will cause large slant-range difference error. Using the largest peak as the main correlation peak will surely cause large errors. Therefore, it is necessary to increase calculation accuracy of ideal time difference. Combining characteristics of underwater integrated navigation system, this paper uses SINS location Cross-correlation function of signals x 1 ptq and x 2 ptq received by R1 and R2 is shown in Figure 4. Sampling period is 5ˆ10´6 s. Except for peaks (main peaks) at TDOA of straight paths, there are other peaks (secondary peaks). The phenomenon that there are many peaks in cross-correlation function and it is impossible to estimate TDOA of signals is called fuzzy cross-correlation peak.  Figure 3 shows a simplified underwater sound multipath propagation model, which is composed of one sound source S and two hydrophones R1 and R2. Except for the straight propagation channels Pd1 and Pd2 from sound source to hydrophones, sound signal is reflected by sea surface (Ps1 and Ps2) and seabed (Pb1 and Pb2) once. Cross-correlation function of signals 1 ( ) x t and 2 ( ) x t received by R1 and R2 is shown in Figure   4. Sampling period is 5 × 10 −6 s. Except for peaks (main peaks) at TDOA of straight paths, there are other peaks (secondary peaks). The phenomenon that there are many peaks in cross-correlation function and it is impossible to estimate TDOA of signals is called fuzzy cross-correlation peak.

SINS-Assisted Method to Search the Optimum TDOA
According to above analysis, multipath effect of sound signal propagation brings several correlation peaks with similar amplitudes in generalized cross-correlation operation. Due to high propagation speed of sound signal in water, tiny time difference error will cause large slant-range difference error. Using the largest peak as the main correlation peak will surely cause large errors. Therefore, it is necessary to increase calculation accuracy of ideal time difference. Combining characteristics of underwater integrated navigation system, this paper uses SINS location

SINS-Assisted Method to Search the Optimum TDOA
According to above analysis, multipath effect of sound signal propagation brings several correlation peaks with similar amplitudes in generalized cross-correlation operation. Due to high propagation speed of sound signal in water, tiny time difference error will cause large slant-range difference error. Using the largest peak as the main correlation peak will surely cause large errors. Therefore, it is necessary to increase calculation accuracy of ideal time difference. Combining characteristics of underwater integrated navigation system, this paper uses SINS location information to assist LBL system to search the optimum time difference under multipath propagation conditions, thus increasing positioning accuracy of the system. In the LBL system, set px i , y i , z i q be position of the i-th hydrophone and px S , y S , z S q be the AUV position (sound source is on AUV) output by SINS. Then, distance between AUV and hydrophones is: The slant-range difference between the distances of any two hydrophones to the sound source is: The calculated time difference of signal arrival between two hydrophones is: where c ijpkq is equivalent sound velocity of propagation path at the time k [12]. The influence factors of sound channel structure mainly include water temperature, salinity and depth. Comparing with the time k´1, these factors are essentially unchanged, so the current equivalent sound velocity c ijpkq can be represented by the equivalent sound velocity c ijpk´1q of the last time cycle, which is: The equivalent sound velocity is the ratio of the slant-range difference between AUV which position is output by the SINS/LBL tightly coupled system and hydrophones to the time difference. The calculation process is as follows.
Set the AUV position output by the SINS/LBL tightly coupled system at time k´1 as P SI NS{LBLpk´1q px S{Lpk´1q , y S{Lpk´1q , z S{Lpk´1q q, distance between AUV and hydrophones is: The slant-range difference between AUV and any two hydrophones (i and j) is: If the time difference between AUV and two hydrophones at time k´1 is ∆t ijpk´1q , then the equivalent sound velocity of propagation path is: Substitute c ijpk´1q into Equation (7), and time difference ∆t 1 ij at time k can be calculated. The peak closest to ∆t 1 ij is found out from the fuzzy correlation peaks in Figure 4. Time difference of this peak is viewed as the ideal time difference ∆t ij .

Establishing LBL-Based Slant-Range Difference Model
In TDOA method, signal arrival time difference between hydrophones (1, 2, 3) and hydrophone 0 could be gained through the method in Section 3.2, thus getting the corresponding slant-range difference [9,13,14] ∆R 1 i " R i´R0 pi " 1, 2, 3q: ∆R i is truth value of slant-range difference and δR i is error of slant-range difference, which can be expressed by the first-order Gauss-Markov process. τ δR i and υ δR i are correlation time and driven white noise of the first-order Gauss-Markov process. υ ∆R i is Gaussian white noise.

State Equation and Measurement Equation of the SINS/LBL Tightly Coupled System
where X SI NS and X LBL are state variables of SINS and LBL; F SI NS and F LBL are SINS and LBL system matrixes; and W SI NS and W LBL are system noises of SINS and LBL.
Considering the environment of AUV, velocity along the height direction and location information could not be neglected. State variable of SINS is chosen as: where δV E , δV N , δV U are velocity errors of SINS toward east, north and up, respectively; φ E , φ N , φ U are misalignment angles of SINS toward east, north and up, respectively; δL, δλ, δh are SINS latitude, longitude and height errors, respectively; ∇ bx , ∇ by , ∇ bz are biased errors of accelerometers along three axial directions, respectively; and ε bx , ε by , ε bz are three axial drifts of gyroscopes. F SI NS can be determined by the SINS error equation.
The measurement equation of tightly coupled system is: where ∆R SI NSi´∆ R LBLi is the slant-range difference error between SINS and LBL. η SI NS{LBL is measurement noise. Let px S , y S , z S q be the AUV position estimated by SINS. Then, slant-range difference is: px S´xi q 2`p y S´yi q 2`p z S´zi q 2´b px S´x0 q 2`p y S´y0 q 2`p z S´z0 q 2 (18) In relative to the real AUV position px, y, zq, it can get from Taylor linearization of Equation (18): Slant-range difference of LBL is: Therefore, ∆R SI NSi´∆ R LBLi " e ix δx`e iy δy`e iz δz´δR i´ν∆R i δx, δy, δ z in Equation (25) are expressed by δL, δλ, δh: Substitute Equation (26) into Equation (25) and the measurement matrix in Equation (17) could be gained: where Elements in H 1 are: where ipi " 1, 2, 3q, R E is radius of curvature in the normal plane perpendicular to meridian plane; and e is ovality of the ellipsoid. Figure 5 is federated Kalman filter structure of underwater integrated navigation system. It contains SINS/LBL tightly coupled sub-system, SINS/DVL sub-system and SINS/MCP sub-system.

SINS/LBL Tightly Coupled Sub-System
State equation and measurement equation of SINS/LBL tightly coupled sub-system are introduced in Sections 4.2 and 4.3.

SINS/DVL Sub-System
(1) State equation of SINS/DVL sub-system is described as: where the state vector of SINS ( SINS X ) and system matrix SINS F are same as those in Section 4.2. , SINS DVL W W are system noises of SINS and DVL, respectively.
is velocity measurement error caused by topographical changes, which could be described approximately by the first-order Gauss-Markov process: is correlation time of the first-order Gauss-Markov process.

SINS/LBL Tightly Coupled Sub-System
State equation and measurement equation of SINS/LBL tightly coupled sub-system are introduced in Section 4.2.

SINS/DVL Sub-System
(1) State equation of SINS/DVL sub-system is described as: where the state vector of SINS (X SI NS ) and system matrix F SINS are same as those in Section 4.2. W SI NS , W DVL are system noises of SINS and DVL, respectively.
where δV Di pi " x, y, zq is velocity measurement error caused by topographical changes, which could be described approximately by the first-order Gauss-Markov process: where τ δdi pi " x, y, zq is correlation time of the first-order Gauss-Markov process.
(2) Measurement equation of SINS/DVL sub-system: where H SI NS{DVL is measurement matrix of SINS/DVL sub-system and η SI NS{DVL is measurement noise.
Velocity difference between SINS and DVL is taken as the observation variable. Since DVL measures AUV speed in the carrier coordinate system directly, it shall transform output velocity of DVL into geographic coordinate system.
where V n SI NS and δV n SI NS are truth value and error of SINS velocity in geographic coordinate system, respectively; V b DVL and δV b DV L are truth value and error of DVL velocity in carrier coordinate system, respectively; V n is AUV speed; ϕ is misalignment angle; and C n b is attitude transfer matrix from carrier coordinate to geographic coordinate.
where V E , V N , V U are velocity of AUV toward east, north and up direction, respectively. C ij pi " 1, 2, 3; j " 1, 2, 3q are elements in the attitude transfer matrix C b n .

SINS/MCP Sub-System
State equation of SINS/MCP sub-system is described as: State sector of SINS (X SI NS ) and system matrix F SINS are same as those in Section 4.2.
Magnetic compass uses only one state variable-heading angle error: δψ MCP can be set a random constant. Therefore, Heading angle difference between SINS and MCP is used as the observation variable, and the measurement equation of SINS/MCP sub-system is: where H SI NS{MCP is measurement matrix of SINS/MCP sub-system, H SI NS{MCP " " 0 1ˆ5 1 0 1ˆ15´1 ı and η SI NS{MCP is measurement noise.

Simulation of SINS-Assisted LBL Tracking Optimum Time Difference under Static Conditions
Hydrophones and sound source layouts are shown in Figure 6. Longitudes and latitudes of four hydrophones are (118.01˝, 32˝), (118.01˝, 32.02˝), (118.02˝, 32.01˝) and (118˝, 32.01˝), respectively. They are all places at a depth of 30 m. Position coordinates of sound source is (118.01˝, 32.0925˝) and its depth is 10 m.
The cross-correlation function curves of received signals by hydrophones T 1 and T 0 are show in Figure 7 Due to multiple reflections of sound signals underwater, many correlation peaks are gained from cross-correlation operation. Many correlation peaks have similar amplitude, which makes it difficult to determine which peak shows the correct time difference. difference error. The proposed algorithm selects the ideal time difference according to the position output by SINS/DVL/MCP system [12]. Comparisons between this improved algorithm and the traditional algorithm are shown in Tables 1 and 2. The improved algorithm solves interference of multipath effect to the optimum TDOA estimation and has higher TDOA calculation accuracy than the traditional algorithm.     difference error. The proposed algorithm selects the ideal time difference according to the position output by SINS/DVL/MCP system [12]. Comparisons between this improved algorithm and the traditional algorithm are shown in Tables 1 and 2. The improved algorithm solves interference of multipath effect to the optimum TDOA estimation and has higher TDOA calculation accuracy than the traditional algorithm.     Since sound signal is reflected many times during underwater propagation, the generalized cross-correlation operation will bring many correlation peaks. In traditional algorithms, time difference corresponding to the correlation peak with the largest generalized cross-correlation function value (Point C in Figure 7) is used as TDOA of different hydrophones. It reveals from Figure 7 that TDOA with traditional method differs from truth value (Point A in Figure 7, red star) significantly, while TDOA with SINS aiding method (Point B in Figure 7, red diamond) is close to the true time difference. Thus, choosing the peak nearest to point B could get the minimum time difference error. The proposed algorithm selects the ideal time difference according to the position output by SINS/DVL/MCP system [12]. Comparisons between this improved algorithm and the traditional algorithm are shown in Tables 1 and 2. The improved algorithm solves interference of multipath effect to the optimum TDOA estimation and has higher TDOA calculation accuracy than the traditional algorithm. Note: t T i´t T 0 , pi " 1, 2, 3q is TDOA between hydrophone T i and hydrophone T 0 .  Table 2, R T i´R T 0 , pi " 1, 2, 3q is difference between sound source distance to the hydrophone T i and sound source distance to the hydrophone T 0 .

Simulation of SINS/LBL Tightly Coupled Algorithm
To study effectiveness of the proposed SINS/LBL tightly coupled algorithm and effect of the number of available hydrophones on positioning accuracy, simulation experiments of SINS combining with 5, 4 and 2 hydrophones are carried out. In Figure 8, five hydrophones are installed at T 0 (118.005˝, 32.005˝), T 1 (118.005˝, 32˝), T 2 (118.01˝, 32.005˝), T 3 (118˝, 32.01˝) and T 4 (118.005˝, 32.01˝), at a depth of 30 m. AUV depth is 10 m. From the initial position (118.001˝, 32.0085˝), AUV travels at a constant speed (1 m/s) along 135˝north by east. The random drift and constant drift of the gyroscope are both 0.04˝/h. Random drift and constant drift of the accelerometer are 50µg and 100µg, respectively. The initial misalignment angles are: 0.01˝pitching angle, 0.01˝angle of roll and 0.1˝heading angle. DVL velocity error is 0.1 m/s. Heading angle error of MCP is 0.3˝and the simulation time is 1200 s. In the simulation, the number of hydrophones is set through software. When SINS combines with five hydrophones, it uses T 0 , T 1 , T 2 , T 3 and T 4 . When SINS combines with four hydrophones, it uses T 0 , T 1 , T 2 and T 3 . When SINS combines with two hydrophones, it only uses T 2 and T 3 . Simulation results are shown in Figure 9 and Table 3.  Note: In Table 2, is difference between sound source distance to the hydrophone Ti and sound source distance to the hydrophone T0.

Simulation of SINS/LBL Tightly Coupled Algorithm
To study effectiveness of the proposed SINS/LBL tightly coupled algorithm and effect of the number of available hydrophones on positioning accuracy, simulation experiments of SINS combining with 5, 4 and 2 hydrophones are carried out. In Figure 8 Figure 9 and Table 3.    It can be seen in Figure 9 and Table 3 that when four hydrophones are installed at seabed, LBL provides three slant-range differences to form a tightly coupled system with SINS. Position error of SINS/LBL tightly coupled system is small (<4 m) and convergent. At this moment, navigation performance is satisfying. When there are five hydrophones, position error is almost equal to that when there are four hydrophones, indicating that in this situation increase of hydrophones does not improve positioning accuracy significantly. When there are two hydrophones, LBL provides only one slant-range difference for SINS. Position error is large and fluctuates violently. However, it could still compensate for position error compared to the system without hydrophones. This technology could ensure that AUV can compensate for position error quickly when it just enters into the LBL array.

Simulation of AUV Dynamic Positioning Based on Federated Filter System
Simulation of AUV dynamic positioning was implemented in order to further verify effectiveness of SINS/LBL tightly coupled algorithm. Four hydrophones are installed at seabed. They are put at T0(118°, 32.01°), T1(118.01°, 32°), T2(118.02°, 32.01°) and T3(118.01°, 32.02°) at a depth of 30 m. Suppose AUV travels from the initial position (117.98°, 32.036°) at a constant speed (1 m/s) along 135° north by east. Rest simulation conditions are same as above and simulation time is set 3 h.
In the simulation, transmission range of sound source is 1.5 km. From 2000~3300 s, AUV approaches the hydrophone array, so T2 and T3 could receive sound signal. From 3300~4700 s, AUV enters into the hydrophone array and all four hydrophones could receive sound signal. From 4700~6000 s, AUV departs away from the hydrophone array gradually, when only T0 and T1 could receive sound signal. After 6000 s, AUV leaves. Simulation results are shown in Figures 10 and 11. Figure 10 is SINS/LBL dynamic simulation results. When AUV approaches the hydrophone array, movement track of SINS/LBL tightly coupled system deviates from the real track slightly, while movement track of SINS/LBL loosely coupled system deviates significantly. When AUV is at center of the hydrophone array, movement tracks of both SINS/LBL tightly and loosely coupled systems are similar with the real track. When AUV begins to depart away from the hydrophone array, movement track of SINS/LBL tightly coupled system keeps a small error with the real track for  It can be seen in Figure 9 and Table 3 that when four hydrophones are installed at seabed, LBL provides three slant-range differences to form a tightly coupled system with SINS. Position error of SINS/LBL tightly coupled system is small (<4 m) and convergent. At this moment, navigation performance is satisfying. When there are five hydrophones, position error is almost equal to that when there are four hydrophones, indicating that in this situation increase of hydrophones does not improve positioning accuracy significantly. When there are two hydrophones, LBL provides only one slant-range difference for SINS. Position error is large and fluctuates violently. However, it could still compensate for position error compared to the system without hydrophones. This technology could ensure that AUV can compensate for position error quickly when it just enters into the LBL array.

Simulation of AUV Dynamic Positioning Based on Federated Filter System
Simulation of AUV dynamic positioning was implemented in order to further verify effectiveness of SINS/LBL tightly coupled algorithm. Four hydrophones are installed at seabed. They are put at T 0 (118˝, 32.01˝), T 1 (118.01˝, 32˝), T 2 (118.02˝, 32.01˝) and T 3 (118.01˝, 32.02˝) at a depth of 30 m. Suppose AUV travels from the initial position (117.98˝, 32.036˝) at a constant speed (1 m/s) along 135n orth by east. Rest simulation conditions are same as above and simulation time is set 3 h. In the simulation, transmission range of sound source is 1.5 km. From 2000~3300 s, AUV approaches the hydrophone array, so T 2 and T 3 could receive sound signal. From 3300~4700 s, AUV enters into the hydrophone array and all four hydrophones could receive sound signal. From 4700~6000 s, AUV departs away from the hydrophone array gradually, when only T 0 and T 1 could receive sound signal. After 6000 s, AUV leaves. Simulation results are shown in Figures 10 and 11. a while, while movement track of the SINS/LBL loosely coupled system deviates from the real track gradually.   Figure 11 shows position error comparison of SINS/LBL dynamics simulations. It reflects position errors of two algorithms during the whole process intuitively. From 0~2000 s, AUV is far away from the hydrophone array. During this period, it is impossible to use LBL to compensate position error and the system only involves SINS/DVL/MCP. As a result, position error accumulates gradually, reaching 8 m. From 2000~3300 s, AUV approaches the signal area of the hydrophone array gradually. T2 and T3 provide one slant-range difference for SINS, which corrects position error of AUV to a certain extent. Position error decreases to 5 m. Since SINS/LBL loosely coupled algorithm requires at least four hydrophones, it is invalid during this period and position error continues to accumulate, reaching 10 m. From 3300~4700 s, AUV enters into the hydrophone array completely and all four hydrophones could receive sound signals. LBL provides three slant-range differences, which reduces position error quickly. Meanwhile, loosely coupled algorithm is effective and inhibits divergence of position error. Therefore, both two algorithms achieve equivalent position    Figure 11 shows position error comparison of SINS/LBL dynamics simulations. It reflects position errors of two algorithms during the whole process intuitively. From 0~2000 s, AUV is far away from the hydrophone array. During this period, it is impossible to use LBL to compensate position error and the system only involves SINS/DVL/MCP. As a result, position error accumulates gradually, reaching 8 m. From 2000~3300 s, AUV approaches the signal area of the hydrophone array gradually. T2 and T3 provide one slant-range difference for SINS, which corrects position error of AUV to a certain extent. Position error decreases to 5 m. Since SINS/LBL loosely coupled algorithm requires at least four hydrophones, it is invalid during this period and position error continues to accumulate, reaching 10 m. From 3300~4700 s, AUV enters into the hydrophone array completely and all four hydrophones could receive sound signals. LBL provides three slant-range differences, which reduces position error quickly. Meanwhile, loosely coupled algorithm is effective and inhibits divergence of position error. Therefore, both two algorithms achieve equivalent position  Figure 10 is SINS/LBL dynamic simulation results. When AUV approaches the hydrophone array, movement track of SINS/LBL tightly coupled system deviates from the real track slightly, while movement track of SINS/LBL loosely coupled system deviates significantly. When AUV is at center of the hydrophone array, movement tracks of both SINS/LBL tightly and loosely coupled systems are similar with the real track. When AUV begins to depart away from the hydrophone array, movement track of SINS/LBL tightly coupled system keeps a small error with the real track for a while, while movement track of the SINS/LBL loosely coupled system deviates from the real track gradually. Figure 11 shows position error comparison of SINS/LBL dynamics simulations. It reflects position errors of two algorithms during the whole process intuitively. From 0~2000 s, AUV is far away from the hydrophone array. During this period, it is impossible to use LBL to compensate position error and the system only involves SINS/DVL/MCP. As a result, position error accumulates gradually, reaching 8 m. From 2000~3300 s, AUV approaches the signal area of the hydrophone array gradually. T 2 and T 3 provide one slant-range difference for SINS, which corrects position error of AUV to a certain extent. Position error decreases to 5 m. Since SINS/LBL loosely coupled algorithm requires at least four hydrophones, it is invalid during this period and position error continues to accumulate, reaching 10 m. From 3300~4700 s, AUV enters into the hydrophone array completely and all four hydrophones could receive sound signals. LBL provides three slant-range differences, which reduces position error quickly. Meanwhile, loosely coupled algorithm is effective and inhibits divergence of position error. Therefore, both two algorithms achieve equivalent position error accuracy, less than 4 m. From 4700~6000 s, AUV departs away from the hydrophone array and only T 0 and T 1 could receive sound signals. Although LBL provides only one slant-range difference, the SINS/LBL integrated algorithm still could correct position error to a certain extent. Position error is controlled within 5 m. During this period, loosely coupled algorithm could not work normally and has the larger position error (7 m). After 6000 s, AUV leaves the hydrophone array completely and position information is provided by SINS/DVL/MCP. It reveals that there is still effective compensation of position error.

Conclusions
This paper puts forward a SINS/LBL tightly coupled navigation system to solve position error accumulation of AUV. It uses slant-range difference as an observation variable for filtering correction of system error. Moreover, to solve fuzzy correlation peaks caused by underwater propagation multipath of LBL sound signal, a SINS-assisted LBL method to search the optimal TDOA is proposed. This method is effective and easy to operate. Test results demonstrate that the SINS/LBL tightly coupled system is more reliable than SINS/LBL loosely coupled system and could still correct position error when there are fewer than four available hydrophones. It expands navigational area of AUV and makes navigation track more flexible.
All simulations in this paper assume that undersea hydrophones are laid in a diamond shape and the hydrophone line up is single, and the next step could be studying different ways hydrophones line up to verify the effectiveness of the SINS/LBL tightly coupled algorithm. Due to the limitations of experimental conditions, the algorithm has not yet been verified in actual conditions. If the conditions are met, we will do further work in the future.