An USBL/DR Integrated Underwater Localization Algorithm Considering Variations of Measurement Noise Covariance

Ultrashort baseline (USBL) positioning system is an important part of the integrated navigation for underwater vehicles. The single USBL positioning system has problems such as reduced accuracy of azimuth measurement due to target motion and large impact of small angular variations, especially in the region where the target is close to the conical boundary, that is, the low-elevation region. To improve the positioning accuracy of the USBL-based integrated navigation system, a tightly coupled USBL/Dead Reckoning (DR) integrated localization algorithm that considers the time varying characteristics of the measurement noise was proposed, and the filtering model of the algorithm was designed. The algorithm exploits the mechanism of the azimuth measurement covariance variation of the USBL system, constructs an adaptive measurement noise estimator for the USBL system, and applies it to the integration filtering of USBL/DR data. A nonlinear extended Kalman filtering (EKF) model was used to fuse the USBL positioning and dead reckoning trajectories. A number of simulation tests with different elevation angle settings were performed to compare the performance of the proposed algorithm with that of a conventional EKF for underwater localization. The test results reveal that the proposed algorithm can effectively reduce the positioning error caused by the change in the relative azimuth of the acoustic signal owing the motion of the mother ship and the motion of the underwater vehicle.


I. INTRODUCTION
With the development of marine resources exploitation, underwater vehicles have been gradually applied in various fields such as large-area surveys and deep-sea diving [1]. The quality of the position estimate dramatically influences the capability of underwater vehicles to complete tasks in low-observability environment and precision-demanding conditions [2]. In general, a single underwater positioning aiding sensor is limited. It is difficult to meet the high-precision requirements and strong tracking need of the tasks at the same time. For example, inertial navigation systems (INSs) suffer from integration drift and acoustic positioning systems (LBL, SBL, and USBL) suffer from The associate editor coordinating the review of this manuscript and approving it for publication was Junhua Li. low update rates, etc. These limitations can be solved by multi-sensor fusion. The combination of an ultra-short baseline (USBL) and inertial navigation systems (INSs) or dead reckoning (DR) is widely used for underwater vehicle localization systems [3]- [6]. Multi-sensor fusion refers to the use of the measured positioning information from USBL to continuously correct the trajectory coming from DR. With the advantage of high accuracy of USBL and high update rate of DR, the integrated system can achieve highly precise underwater navigation in complex and dynamic underwater environments [7].
Typical architectures for fusing DR data with USBL are loosely-coupled and tightly-coupled. Loosely-coupled integration requires the absolute position coordinates of the target obtained via USBL and DR, respectively. Two independent positioning solutions are combined to form VOLUME 10, 2022 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ a filtered positioning solution. Tightly-coupled integration directly combines the slant range and inclination angles of USBL measurements with the DR solution to form a filtered positioning solution. The USBL position system physically measures the relative position information (i.e., the slant range r and the inclination angles θ x , θ y ) between the onboard transponder and hydrophone arrays. The absolute position coordinates of the target in the earth-fixed frame can be calculated based on the relative position information.
Although both the relative position information and the absolute position coordinates can be chosen as the measurements, the absolute position coordinates contain an uncertain sensor coupling error after nonlinear conversion and bias compensation. Therefore, a tightly-coupled integrated filter is more stable than a loosely-coupled integrated filter using absolute position coordinates. In addition, considering the nonlinear calculations in the localization algorithm, an extended Kalman filter (EKF) is commonly used in integrated systems.
The EKF [8] can provide a sub-optimal estimation of the state in a nonlinear system. The performance of the EKF depends largely on a prior knowledge, such as process noise and measurement noise statistics [9], [10]. The initial stage of the EKF requires covariance matrix of process noise and measurement noise. An Inaccurate covariance matrix could lead to degradation of the estimation accuracy, and even result in an unexpected divergence of the filter [4], [11], [12].
To reduce the estimation error effect, three approaches are commonly used for estimating the covariance matrix of USBL measurement noise. The first approach is to set the measurement noise covariance matrix as a fixed diagonal matrix which consists of the empirical variance of USBL measurement variables. Empirical variance can be acquired from a data set of extensive practical experiments or from the user manual of USBL [13]. The second approach is to fill the covariance matrix based on the signal-to-noise ratio (SNR). The third approach is to dynamically adjust the covariance matrix by constructing an innovative sequence of prediction residuals (also known as adaptive Kalman filters, AKF). In the first approach, it is difficult to separate the noise from the measurement signals because of the complexity of the noise mechanism. Both the first and second approaches predefine the covariance matrix and keep it constant. However, in actual applications, the statistics of the measurement noise may be time-varying owing to process dynamics of system or impact of complex environment [14]. With fixed covariance matrix, the Kalman gain cannot be adjusted in time when an underwater vehicle is accelerating, decelerating or turning. Therefore, the third approach applied to positioning systems whereas the AKF could avoid the negative effect of the uncertainty of process noise and measurement noise statistical properties.
The well-known AKFs mainly include innovation-based AKF (IAKF), Sage-Husa AKF (SHAKF) and multiple model AKF (MMAKF) [14]- [18]. The IAKF method estimate the covariance matrix based on maximum likelihood estimation (MLE). By constructing an innovation sequence, the noise covariance matrix can be adjusted and the appropriate values of the covariance matrices can be obtained. The SHAKF method introduces genetic factors based on the IAKF method. It increases the weight of recent measurements and reduces the impact of former data on estimates [20]. The MMAKF is approximation of the Bayesian method, which select the best state estimate from a bank of simultaneously operating Kalman filters. However, the MAP estimator is always in a critical stable state, which easily leads to a filter divergence whereas the other two methods require a long innovation covariance sequence. The above AKF method using sampled measurements and innovation sequence is generally applicable to various systems. The current study on the above methods is dedicated to improving speed, convergence, and stability. However, the change mechanism of the measurement noise covariance itself is ignored which causes the above estimation method to be suboptimal in some specific case.
In this paper, we found that the measurement noise magnitude of USBL in tightly-coupled integrated systems is closely related to the inclination angles of the return signal by studying the measurement mechanism. Manufacturers of USBL systems always recommend that the positioning accuracy is higher when the transponder located within the tapered area below the hydrophone arrays than in other areas. This supports our findings. However, there is no corresponding model to describe the relationship between measurement noise magnitude and measurement variables. We aim to model this relationship and apply it to estimate the measured noise covariance matrix. Theoretically, this estimation is consistent with the property of the USBL system. This relation essentially reveals prior knowledge of the measurement noise. In this study, the EKF is chosen to estimate state of the USBL/DR integrated localization algorithm. The measurement noise covariance matrix was calculated using the proposed estimator in one step without multiple recursions. This measurement noise estimator enables a simple filter construction and efficient calculations. The proposed algorithm was analyzed using simulation data. We simulated the motion of an underwater vehicle in two specific cases, and tested the positioning accuracy of the proposed algorithm.
The remainder of this paper is organized as follows. Section II introduces the measurement mechanism of USBL, and the measurement noise estimator expression is given. In Section III, we derive the prior information of the USBL measurement noise magnitude obtained from the measurement noise estimator, and analyze its impact on horizontal positioning accuracy. In Section IV, the proposed algorithm that fuses USBL and DR is derived in detail. The simulation test results are presented and analyzed in Section V. The final section provides conclusions.

II. USBL POSITIONING SYSYEM MEASUREMENT
To generate reliable estimates using the EKF, it is important to know the distributions of the measurement noise. Estimation of the measurement noise allows algorithms to adapt to the time-varying measurement system instead of using fixed parameter. A model for noisy USBL measurement assumes additive, zero mean noise, given by: where z meas and z denote the measured and ideal measurement vectors, respectively. δ z denotes the measurement noise.
The goal is to estimate the covariance matrix R of the noise δ z . In this section, we first analyze the measurement vector z of USBL. Then using the law of error propagation, the standard deviations were derived for each variable to qualify the measurement noise. Finally, the characteristic of measurement noise variation in different directions was analyzed.

A. MEASUREMENT MECHANISM
In this study, the USBL positioning system was a fiveelement array USBL. As shown in Fig. 1, the core of the USBL consists of a transducer, an array of hydrophones rigidly fixed on the mother ship and a transponder placed in an underwater vehicle. The transducer is located at the origin of the ship coordinate system. S1, S2, S3 and S4 are hydrophones, arranged symmetrically in the XOY space of the mothership coordinate system. The S1 and S3 arrays were located on the X-axis pointing to the bow. The S2 and S4 arrays are located on the Y-axis perpendicular to the bow. The spacing of arrays is d. It is assumed that the position of the underwater vehicle with acoustic transponder is T (x, y, z).
We study USBL positioning based on slant range and inclination angle model, and the measurement vector is expressed as: where r denotes the slant range between the underwater vehicle and the transducer, θ x refers the angle between − → OT and X-axis of the USBL coordinate system, and θ y refers to the angle between − → OT and Y-axis of the USBL coordinate system. Furthermore, we discuss the measurement mechanisms for these three variables.
Measurement r: USBL utilizes the time delay between signals dispatched and received by hydrophones to acquire the slant range r: where C denotes sound velocity, T denotes the time interval between signals dispatched and received by hydrophones, λ denotes wavelength of acoustic signal, and f denotes the frequency of acoustic signal.
Measurements θ x and θ y : As shown in Fig. 2, the acoustic transponder responds to the hydrophone arrays. Since the size of the hydrophone arrays is very small, under the condition of far-field acoustic wave, acoustic lines incident on the hydrophone array can be considered parallel to each other. Obviously, the signal phase received by the array S1 is ahead of that received by the array S2. The phase difference φ x is given by: Similarly, the phase difference φ x between the received signals of the array S2 and S4 is: where λ denotes wavelength. Rewriting Eq. (4) and Eq. (5), the following can be obtained: Rewriting Eq. (2), we have: The measurement noise of the USBL positioning system contains several independent sensor and environmental noises. During signal processing these noises were coupled and remained in each measurement variable. Based on the law of error propagation and the corresponding parameter formula of USBL measurements, we can deduce the measurement noise of USBL measurement variables. Measurement r: After the partial differential of r = λfT 2 , the standard deviation of r is given by: where σ λ , σ f and σ T are the standard deviations of wavelength of acoustic signal, the frequency of acoustic signal and the time interval, respectively. Due to the aid of sound velocity profiler, most of USBL positioning systems have ensured high ranging accuracy. The ranging error has no significant effect on the performance of the USBL. Thus, we consider σ r as a minimum constant O.
Measurements θ x and θ y : After the partial differential of θ x = arcos λ φ x 2πd , the standard deviation of inclination angle is given by: where σ θ x is the standard deviation of θ x , σ λ is the standard deviation of wavelength, σ φ x is the standard deviation of φ x and σ d is the standard deviation of acoustic arrays installation. If the wavelength (sound velocity) error and the installation error of acoustic arrays are neglected, we have: σ φ x is determined by the Cramer-Rao Lower Bound [13], which is obtained as follows: where SNR is signal noise ratio of system. Substituting Eq. (12) into Eq. (11), the standard deviation of θ x can be given by: Similarly, the standard deviation of θ y can be given by: Generally, the measurement vector can be summarized as follows: where δ z denotes the measurement noise, δ r , δ θ x and δ θ y denote the noises of the slant range r and the inclination angles θ x and θ y , respectively. We assume that δ r , δ θ x and δ θ y are independent and can be modelled as Gaussian distribution:

C. ANALYSIS OF MEASUREMENT NOISE VARIATION
The Eq. (13) and Eq. (14) indicates that there is a relationship between the standard deviation estimation of measurement vector and inclination angles of the return signal. Therefore, prior information of measurement noise variation can be obtained by calculating in all directions, then the three-dimensional spatial distribution of the measurement noise magnitude is obtained. We first defined the measurement noise magnitude as the geometric mean of the standard deviation of each measurement variable. The noise magnitude function is defined as follows: where σ r , σ θ x and σ θ y are given by Eq. (9), Eq. (10) and Eq. (11), respectively. Evidently Obviously, σ r does not vary with the direction of the return signal, and its incremental impact is negligible for the three-dimensional spatial distribution of the measurement noise magnitude. Therefore, Eq. (16) can be simplified as follows: To study the influence of the inclination angle of the return signal on the noise magnitude of USBL measurement, a numerical simulation was carried out. Assuming that the uniform and constant sound speed in seawater is 1500 m/s, the frequency is 35 kHz, the distance between the hydrophones is 0.1 m and the SNR is fixed at 25 dB. The return signal may come from any direction of the working space. Each return signal corresponded to a certain measurement noise magnitude which was determined only by the inclination angles relative to the hydrophone arrays. The three-dimensional spatial distribution of the measurement noise magnitude was plotted by calculating Eq. (17). As shown in Fig. 3, the color of a point on the surface represents the measurement noise magnitude in the current direction. The entire drawing space was contained in the working space of the USBL system and the red dots indicate the position of the hydrophone array.
As shown in this figure, the measurement noise magnitude of USBL system changes with the relative hydrophone arrays orientation. In general, the closer return signal is to the Z-axis, the lower the measurement noise magnitude.
In the direction below the hydrophone, the measurement noise magnitude was the lowest at, approximately 0.4 • . The closer the return signal is to X-axis or Y-axis, the higher the measurement noise magnitude. As shown in Fig. 3(b), the plots in the X-axis and Y-axis directions are blank, indicating that the measurement noise magnitude in these areas exceeds 2 • . To facilitate the analysis and guidance of practical applications, we focused on the law of measurement noise magnitude changes in the following two special cases.
Imagine a circle below the hydrophone arrays (black circle in Fig. 4(a)). Because the relative elevation angle is equal everywhere on the circle, the intersection of a cone passing through the circle (red surface in Fig. 4(a)) and the curved surface is the measurement noise magnitude distribution on the circumference. Imagine a vertical plane (red surface in Fig. 4(b)) at an angle of 45 • to both the X-axes and Y-axes. The intersection of the plane and the curved surface is the measured noise distribution in all directions in the vertical plane. Because the measurement noise magnitude is only related to the azimuth of the returned signal, the distribution of measurement noise magnitude in the horizontal and vertical plane directions are expressed in polar coordinates, as shown in Fig. 4 (c) and (d), where the color on each direction line in this figure indicates the magnitude of the measurement noise, and the polar radius of the curve represents the measurement noise value. Obviously, in the horizontal circumference, the closer the return signal is to the X-axis or Y-axis, the greater or even dispersive the measurement noise magnitude; in the vertical section, the closer the return signal is to the water surface area on either side of the hull, the greater the measurement noise magnitude. When the return signal comes directly beneath the ship, the measurement noise magnitude is minimized.

III. DYNAMIC AND MEASUREMENT MODELS
In this section, by establishing the system's state equation and measurement equation and combining with the USBL measurement noise estimator in the previous section, the EKF positioning algorithm using the fusion of USBL and DR is derived.
Consider a simulated positioning system with a continuously operated process, which can be described as: where k denotes epoch number, x(k) denotes the system state vector, m(k) denotes control vector, w(k) denotes process noise, z(k) denotes measurement vector, v(k) denotes measurement noise. The process and the modelling of the positioning system were carried out under the following simplifying assumptions: Assumption 1: The motion of the underwater vehicle is determined by three parameters: course over ground (COG), speed over ground (SOG) and changes in submergence ( z).
Under Assumption 1, a representation of the system dynamics variable can be obtained as follows: where x(k), y(k) and z(k) are the coordinates of the underwater vehicle at epoch k; COG t (k), SOG t (k) and z t (k) are the true values of COG, SOG and z at epoch k, respectively. Further, according to the existing kinematic equation of moving vehicle, we have the following equations: where is duration between the epoch of k and k + 1.

Assumption 2:
The true value of the control vector m(k) is composed of manipulated inputs and random perturbations of vehicle speed, orientation and other factors as follows: where u(k) denotes manipulated inputs vector, and w u (k) denotes random perturbations of control vector m(k). w u (k) reflects the effect of the error of dynamic parameters and environment on the vehicle motion. Assumption 3: The measurements sensors consist of the USBL and water depth pressure sensor. Furthermore, the physically measured values of USBL are used in the measurement update or correction step of the filtering algorithm without conversion.  Additional explanation for Assumption 3: The USBL system outputs raw position data in Cartesian coordinates. However, because the system physically measures the slant range to the transponder and inclination angles (Fig. 1), the error of the sensor is reflected in the independent noise of the inclination angles θ x , θ y and the slant range r. These three independent noises were mixed in the x-, y-and z-coordinates. Therefore, we directly take θ x , θ y and r into the update step.
Assumption 4: The random perturbations of control vector can be modelled as a Gaussian distribution with mean value of 0 and covariance matrix Q(k). In addition, the measurement noise can be modelled as a Gaussian distribution with a mean value of 0 and a covariance matrix R(k). We have: where E[·] denotes expectation, Cov[·] denotes covariance, and δ kj denotes the Kronecker delta function. Assumption 5: The effect of process noise on the state dynamics is additive, and the effect of measurement noise on the measured values is additive as follows: Furthermore, process noise is completely affected by random perturbations of the control vector and can be approximated as follows: where u (k) = [∂g/∂u(k)] is a Jacobian matrix that represents the optimal linear approximation of state equation to the given point. According to Assumptions 1∼5 and Eq. (18), a representation of state equation and measurement equation can be obtained at as Eq. (28) and Eq. (29).
and a, b and c are the transducer coordinate.

IV. A NOVEL LOCALIZATION ALGORITHM A. MEASUREMENT NOISE ESTIMATOR
Currently, USBL systems are equipped with inertial measurement unit (IMU) to compensate the translation and rotation of arrays. After coordinate transformation, we can obtain the original inclination angles of the return signal from the measured value. Generally, the mother ship installed with the USBL system inevitably sways on the sea due to ocean dynamics. Therefore, we consider the effects of roll ϕ and pitch θ of the ship. The positioning system requires two frames as shown in Fig. 5: the USBL hydrophone arrays frame {U } (O, X u , Y u , Z u ) and the global frame{G} (O, X g , Y g , Z g ). In the USBL hydrophone arrays frame, hydrophone S1 and S3 are located on the positive and negative semi-axes of the X-axis, respectively. Hydrophone S3 and S4 are located on the positive and negative semi-axes of the Y-axis, respectively. In the global frame, the absolute position of the target was determined and did not change with the motion of the ship.
Many studies have noted that the angle error has a greater effect on the USBL misalignment error, whereas the position error has a smaller effect on the USBL misalignment error. If the relative position error between frame {U } and the mother ship is neglected, the roll ϕ and pitch θ of the ship are the Euler angles of the frame {U } relative to the global frame{G} on the X-axes and Y-axes. The rotation matrices are as follows: And we have: where P U = x U , y U , z U T denotes the position vector in frame {U }. P G = x G , y G , z G T denotes the position vector in frame {G}. Although USBL systems are equipped with attitude sensors and gyroscopes, which can correct the posture of mother ship to ensure that the measured position output is unaffected, the direction of the return signal still changed. We have Eq. (33) where θ U x and θ U y are used to update the inclination angles of the return signal in the noise estimator to calculate the correct measurement noise magnitude. The covariance matrix of measurement noise is expressed as: Considering the nonlinearity of the measurement equation, EKF, an extension of KF for a nonlinear system, was adopted to fuse of the DR data and USBL data. The general form of EKF algorithm is as follows: x(k) = g( x(k − 1), u(k)) P(k) = F(k) P(k − 1)F(k) T + u (k)Q(k) u (k) T

K(k) = P(k)H(k) T H(k)P(k)H(k) T + R(k)
−1 where x(k), P(k) are estimated vector of coordinates of the underwater vehicle position and its covariance matrix, determined a priori for the epoch k; x(k), P(k) are estimated vector of coordinates of the underwater vehicle position and its covariance matrix, determined a posteriori for the epoch k. F(k) and u (k) are called matrix system which is calculated as a function of the Jacobian matrix g (x(k), u(k), 0). Q(k) is covariance matrix of control vector. H(k) is Jacobian matrix with function z(k) = h(x(k)).The details of the above items The expansion formula of the above items can be found in Eq. (36) ∼ (39). R(k) is the covariance matrix of measurement noise at the time k. Assuming that the standard deviation of slant range and depth are both constant, that is, σ r (k) = σ r , σ dep (k) = σ dep . We have: where σ θ x (k) and σ θ y (k) are determined by σ θ x (k) = γ csc θ x (k), σ θ y (k) = γ csc θ y (k).The measurement noise covariance matrix is calculated using the noise estimator as follows: The measurement noise estimator obtains the measurement data from the sensors to calculate the measurement noise covariance matrix at time k. Then the covariance matrix is input into the update step of the algorithm together with the measured value after coordinate transformation. The DR data are COG, SOG and z obtained from sensors on board the underwater vehicle such as INS, electro-magnetic log, etc. The DR process actuates the prediction step of the algorithm VOLUME 10, 2022 by calculating prior knowledge of state and data of onboard sensors, and then transfer the predicted state value and the predicted state noise covariance to update step. In the update step, the algorithm compares the predicted and measured data of the DR process and outputs an unbiased minimum variance estimation of the state.

V. TEST AND RESULTS
A simulation environment was built to test the algorithms presented in this study (Fig. 6). The movement of a vehicle along a specific trajectory is simulated. Based on the position of the vehicle, the USBL measurements were simulated. These measurements were used as input for the proposed algorithm with a built-in noise estimator. The proposed algorithm should operate in this highly dynamic environment since the measurement is immediately fed back to the estimator. By estimating the USBL measurement noise covariance matrix, the weight of measurement can be adjusted to aid in positioning the target.

A. TEST CONFIGURATION
The proposed algorithm was tested on two simulated scenarios. The first test was associated with the linear motion of an underwater vehicle in the direction of a ray emitted from the arrays. In this case, only the motion of the mother ship will affect the estimation of the measurement noise covariance matrix. Therefore, this testing scenario could verify the effectiveness of the algorithm under ship motion.
The second test was associated with the horizontal circular motion of underwater vehicle under arrays. In this case, the elevation angle of the measurement target was fixed, and the inclination angles were varied. Similar to the first test, effect of ship motion is taken into account. This testing scenario verified the law of the measurement noise magnitude in the horizontal circumferential direction and the performance of the proposed algorithm.
Test1: Assuming that the mother ship motion on the sea with simple harmonic roll and pitch with a fixed period and amplitude, the vehicle works under the water. The rolling and pitching periods were T 1 = 16 s and T 2 = 12 s, respectively, and their amplitudes are 12 • and 5 • , respectively. We suppose that the underwater vehicle performs a linear motion with an initial position of [20 m, 55  Test2: Assuming that the ship is wobbling with the same motion parameters of Test1, we let the underwater vehicle make a constant-speed circular motion in the XOY plane with a radius of 1000 m. Its control vector is u(k) = 2πk 4000 π 2 m/s 0 T .Note that the elevation angle (i.e. θ z ) affected the performance of the algorithm. Therefore, the test was repeated with different θ z . The DR data and USBL measurement noise are set to zeromean Gaussian white noise, which includes the velocity measurement error and direction measurement error of the DVL device as well as the ranging error and direction measurement errorr of the USBL. It is assumed that the sensors are well calibrated in the simulation, so the installation error and array spacing error of the sensors are excluded.The standard deviations noises are designed as shown in Table1. Fig. 7(a) shows the change in inclination angles. θ x and θ y fluctuate around 71 • and 25 • , respectively. Fig. 7(b), along with Fig. 7(a), shows the standard deviation of inclination angles. σ θ x and σ θ y fluctuated around 2.5 • and 1.17 • , respectively. Additionally, from partial enlarged view in Fig. 7(b), we can see that the fluctuation periods of σ θ x and σ θ y are the same as ship's roll and pitch period (T 1, T 2), and their amplitudes are related to ship's roll and pitch amplitude. It indicates that ship motion has influence on standard deviation of inclination angles.

Test1:
The root mean square error (RMSE) determined by the proposed algorithm, EKF with constant R, USBL systems and DR method are presented in Fig. 7(c). 300 Monte Carlo experiments were conducted in order to ensure the objectivity of the simulation results. For the position error determined by DR method and USBL, the error accumulates linearly with the increase of the motion course of the underwater vehicle.
In addition, we can see that the accuracy of the estimated position using either the proposed algorithm or the EKF with constant R is higher than those estimated using the DR or USBL only. Further, we compared the performance of the proposed algorithm and EKF with constant R, as shown in Fig. 7(d). The results show that the proposed algorithm performs better than EKF with constant R in the linear motion of underwater vehicle away from the acoustic arrays.
Test2: As mentioned above, we evaluate the performance of our algorithm at different θ z . Here, the test results under θ z = 20 • , θ z = 50 • , and θ z = 80 • are selected as special cases firstly. The test results are shown in Fig. 8, Fig.9, Fig. 10 and Fig. 11. In the circumferential motion of the underwater vehicle, the inclination angles varies roughly in cosine form,      as shown in Fig. 8. Comparing with them, we can see that the larger the θ z is, the more dramatic the inclination angle change is. This is determined by the relationship of the θ x , θ y and θ z , where cos 2 θ x +cos 2 θ y +cos 2 θ z = 1. Further, the inclination angle θ x and standard deviation σ θ x takes extreme around about 1000 s and 3000 s, respectively. Correspondingly, the inclination angle θ y and standard deviation σ θ y takes extreme around about 0 s, 2000 s, 4000 s. This feature can be explained by Fig. 4(c), where the standard deviation of the inclination angle reaches an extreme value in the axial position. Moreover, Fig. 9 shows similar features in experiments with different elevation angles, that is, the standard deviation shows similar periodic oscillations, which can be seen from the local enlarged graph. Thus, it can be asserted that both the ship's motion and the underwater vehicle activities change the distribution characteristics subject to this measurement noise. Fig. 10 illustrates the performance of four methods under the circumferential motion of the underwater vehicle. For the position error determined by DR method, with an increase in the motion course of the underwater vehicle, the error accumulates gradually and eventually remains at approximately 20 m.
For the position error using USBL systems, on the one hand, with the increase in θ z , the slant range of the underwater vehicle decreases while the radius remains at 1000 m, which leads to a reduction in its overall errors. It can be seen that the On the other hand, the error also shows a certain fluctuation in the time history. It peaks at about 0 s, 1000 s, 2000 s, 3000 s, 4000 s. This is because the standard deviation takes an extreme at these moments (Fig. 9). When θ z is 80 • , the peak error is even than 100 m. Therefore, it may be concluded that USBL system is limited in shallow-water areas where the θ z is larger.
Comparing of the RMSE between the proposed algorithm and the EKF with constant R, we can see that the position errors determined by the EKF with constant R exhibit some fluctuations. In particular, incident angle is 80 • , the position errors determined by EKF with constant R increased sharply at approximately 0 s, 1000 s, 2000 s, 3000 s, 4000 s, when the standard deviation of inclination angle was at the extreme( Fig. 9(c)).
By comparison, the proposed algorithm always has a better and more steady performance no matter how the inclination angles changes (Fig. 11). This is because the proposed algorithm adjusts gain K by updating covariance matrix of measurement noise in time. Thus reducing the weight of USBL measurement information. Furthermore, we analyze the average positioning errors determined by the proposed algorithm and EKF with constant R at different θ z . As shown in Fig. 12, the proposed algorithm has advantages within a large θ z , especially when θ z is close to 70 • the RMSE of position is improved by more than 10%. When θ z is range of 70 • -80 • , the RMSE of position of the proposed algorithm is reduced by 0.2-1 m compared to EKF with constant R.

VI. CONCLUSION
In this study, a novel EKF localization algorithm that considers the variation of measurement noise covariance was proposed. A measurement noise estimator is available to adapt to the measured noise covariance during the filtering process. Subsequently, a series of simulation tests were conducted to evaluate the performance of the proposed algorithm. The results reveal the variation pattern of standard deviation of inclination angle, which is related to the motion state of mother ship and underwater vehicle. The simulation results of the two scenarios demonstrate that the proposed algorithm has better positioning accuracy and is more robust than EKF with constant R in scenarios where the inclination angle is continuously varying. It is worth noting that the error of VOLUME 10, 2022 the position estimated by EKF with constant R rises sharply when exceeds 70 • , while the proposed algorithm has a better performance with more than 10% improvement in positioning accuracy the proposed algorithm has a better performance. In the follow-up research, we plan to carry out experiments in a pool or lake to practically evaluate the performance of the proposed algorithm.