An improved underwater TDOA/AOA joint localisation algorithm

To solve the problem of sound source localisation in underwater sensor networks, this paper constructs a pseudo-linear equation system of time difference of arrival and angle of arrival (TDOA/AOA) and then uses the weighted least squares algorithm to estimate the target position. This paper proposes a two-stage weighted least squares algorithm that uses target position error. First, the adapted TDOA equation and the existing AOA equation are combined into the ﬁrst-stage weighted least squares algorithm, which improves the estimation accuracy of the ﬁrst stage compared with the traditional algorithm. Second, this paper uses the target position error in the second stage to derive a new TDOA/AOA equation. Finally, the target position calculated in the previous stage is adjusted by the solved target position error. The performance of the proposed algorithm is veriﬁed by comparison with the Cramer–Rao lower bound. Simulation results show that the proposed algorithm still has good localisation performance even under high-angle noise.


INTRODUCTION
Underwater sensor networks have been widely used in many fields, such as underwater communication, marine detection, assisted navigation and deep-sea localisation [1][2][3][4]. The ability to locate underwater is a key requirement for most underwater applications. Mobile devices such as autonomous underwater vehicles and remotely operated underwater vehicles require precise localisation capabilities to perform long-term missions while being submerged [5].
Hydro acoustic localisation, as a core technology supporting the underwater sensor network, can provide position calibration for nodes, so that the information collected by the sensors is of practical significance [6,7]. Localisation technology can be divided into two types, range-based localisation and non-range-based localisation. Range-based localisation first performs two-point range and then uses three-sided, triangular and other geometric characteristics to locate nodes. Specific range methods include time of arrival (TOA) [8], time difference of arrival (TDOA) [9], received signal strength indication (RSSI) [10] and angle of arrival (AOA) [11]. Non-range-based localisation generally uses the received signal directly to determine the This is an open access article under the terms of the Creative Commons Attribution License, which permits use, distribution and reproduction in any medium, provided the original work is properly cited. © 2021 The Authors. IET Communications published by John Wiley & Sons Ltd on behalf of The Institution of Engineering and Technology target position [12]. All range methods can be used alone or in combination with other methods.
TOA localisation needs to maintain the time synchronisation between the target and the sensor. Compared to TOA localisation, TDOA localisation is a more common localisation method. TDOA only needs to maintain the time synchronisation between sensors, it uses at least four sensor nodes for threedimensional localisation [13]. In a complex underwater environment, RSSI can be affected by multipath effects, which limits the application range of the algorithm. AOA localisation has advantages such as high accuracy, but due to higher costs, it has not been widely used in the past. However, to take advantages of both TDOA and AOA for underwater localisation, this paper focuses on joint localisation algorithm based on TDOA and AOA.
In recent years, research on AOA localisation has focused on two-dimensional planes [14] and then extended to threedimensional planes [15]. In [11], the focus is on solving the problem of AOA localisation for unreliable bearing measurement. The way to resolve these outliers is to find them and delete them. A total least squares algorithm was proposed to solve the AOA localisation problem and improve the localisation accuracy of the least squares algorithm [16]. AOA localisation generally requires two sensors, and also highprecision angle measurements. TDOA/AOA joint localisation algorithm can reduce the number of sensors to achieve the best performance or achieve better performance under the same sensor conditions [17]. In [18], a TDOA/AOA joint localisation algorithm based on structural total least squares was proposed, which reduced the bias generated by the traditional algorithm by reconstructing a new localisation algorithm. TDOA/AOA joint localisation also has the problem of uncertain sensor position. Calibration sources can be introduced to reduce the accuracy loss caused by sensor position errors [19]. In order to determine the position of the target, an iterative form of maximum likelihood estimation method was proposed in [20]. Each step of the algorithm can be convergent, but it requires a good initial solution. This method is unclosed form, so the calculation amount of the algorithm is huge. In [21], a closed-form two-stage weighted least squares (TSWLS) algorithm was used to solve the TDOA localisation problem. It has a small amount of calculations and does not require initial iterations. Its performance is close to Cramer-Rao lower round (CRLB) when the noise is small. Various extensions of the TSWLS algorithm have been developed, such as the time difference of arrival and frequency difference of arrival(TDOA/FDOA) joint localisation algorithm [22,23] and the sensor position uncertain localisation algorithm [24,25]. A multi-input and multi-output passive radar joint localisation algorithm based on TDOA/AOA was proposed in [26]. It has good localisation performance on the z-axis, but the localisation accuracy in the first stage is not high. In [27], a three-dimensional (3D) localisation method using time-delay and AOA (TD/AOA) measurements was proposed. It greatly improves the localisation accuracy under low-angle noise conditions, but the localisation accuracy is not good under high-angle noise conditions. On the basis of the above work, this paper proposes a new closed-form two-stage weighted least squares algorithm for underwater TDOA/AOA joint localisation. In the first stage of the algorithm, the adapted TDOA equation and the existing AOA equation are combined into the first-stage weighted least squares algorithm, which improves the estimation accuracy of the first stage compared with the traditional algorithm. In the second stage, by introducing the target position error, a target position error equation is constructed, and the solved target position error is adjusted to the localisation result of the previous stage. In this paper, the CRLB of TDOA/AOA joint localisation is also derived, and the performance of the algorithm is proved by comparison with CRLB. Simulation shows that the proposed algorithm still has good localisation performance under high-angle noise conditions. This paper follows the standard notation, where boldface upper case represents matrix and boldface lowercase letter denotes vector. The subscripts (⋅) T and (⋅) −1 stand for matrix transpose and inverse operations, respectively. The symbols ‖ ⋅ ‖, | ⋅ | and ∑ (⋅) stand for the l 2 norm, absolute value and summation, respectively. Finally, diag(a) is a diagonal matrix whose diagonal entries are the elements of a. The rest of the paper is organised as follows. Section 2 shows the basic localisation theory. In Section 3, an improved underwater TDOA/AOA joint localisation algorithm is proposed. Section 4 theoretically investigates the performance of the proposed algorithm in comparison with the CRLB. Section 5 is devoted to the simulation results and compares the performance with applicable established algorithms. Finally, Section 6 concludes the paper.

PROBLEM FORMULATION
In an underwater 3D localisation scene, the true position of the target is u = [u x , u y , u z ] T , which is unknown in the localisation process. Assuming that the number of sensors participating in the localisation is M , the position of the sensor i can be expressed as s i = [x i , y i , z i ] T , i = 1, 2, … , M, where the position of the reference sensor is s 1 = [x 1 , y 1 , z 1 ] T . The sensor i can also observe the azimuth angle i and elevation angle i of the target. In this paper, TDOA/AOA measurement equations are constructed from known information, and the target position is solved. Figure 1 shows the underwater localisation geometry.
The distance from the target to the sensor i can be expressed as In underwater sound source localisation, the TDOA measurement value can be obtained by multiplying the time difference parameter i,1 extracted from the signal by the speed of sound c, which can be written as In the localisation algorithm, the number of AOA measurement values ( i , i ) is M , and the number of TDOA measurement values r 0 i,1 is M − 1. Measurements in vector form for the target and all sensors can be expressed aŝ where n is the noise vector. Assume that all elements of noise n obey a Gaussian distribution with a mean of zero and a constant variance. The covariance matrix Q can be expressed as Here, 2 T , 2 are the variances of 1 , 2 and 3 . O represents a zero matrix and the subscripts represent its dimensions.

AOA localisation
Assume that the three components of u are greater than the three components of s i . The relationship between the azimuth angle i and the target position can be found from the localisation geometry in Figure 1 as It can also be found that the relationship between the elevation angle i and the target position is The relationship between the elevation angle i and the target position can also be written as By rewriting Equations (14) and (17), the following AOA equations can be obtained: We note that Equations (18) and (19) can be written as a system of linear equations about the target where x 1 cos 1 sin 1 + y 1 sin 1 sin 1 − z 1 cos 1

TDOA localisation
Equation (2) gives a representation of the TDOA measurement.
In order to build the localisation equation, we adapt (2) to r 0 i,1 + r 0 1 = r 0 i and expand it completely squared. Finally, we can bring Equation (1) into the equation as follows: The linear equations of all sensors and target can be written as where

TDOA/AOA joint localisation
Considering Equations (20) and (24), the linear system of TDOA/AOA can be expressed as where where 0 2M × 1 is a zero vector and the subscript indicates its dimension.
The above linear equations can be solved using least squares as

IMPROVED TDOA/AOA JOINT LOCALISATION ALGORITHM
In fact, the above theories are all implemented under ideal conditions. In an actual underwater localisation scenario, the measurement values are accompanied by noise, and the actual measurement values should be as shown in Equation (3). Considering the case of noise, the system of linear equations can be solved using a weighted least squares algorithm, where the weight can be calculated by the covariance function of the noise. The localisation algorithm is divided into two stages. The first stage constructs a new TDOA equation and introduces the measurement error equation and the second stage introduces the target position error to adjust the localisation results of the first stage.
Stage 1: Considering the angle error equation̂i = i + Δ i and i = i + Δ i , there are the following equations: The following approximations are sin Δ i ≈ Δ i and cos Δ i ≈ 1. Considering the above equations, Equations (14) and (17) can be rewritten as By observing the geometric relationship of Figure 1, the following approximate equation can be obtained: where r i = r 0 i + Δr i , and Δr i is the noise error. Therefore, the final AOA measurement equation can be written as [26] ( Considering all the equations composed of sensors and target, the matrix form of the AOA localisation equations can be expressed as where Considering Equation (2), we improve the TDOA measurement Equation (23), which can be written as [9] 2(s i − s 1 Introducing the actual measurement value r i,1 = r 0 i,1 + Δr i,1 , Equation (47) can be rewritten as the following equation: The system of TDOA localisation equations composed of all sensors and target can be expressed as Combining Equations (42) and (49), the TDOA/AOA joint localisation equations are where Unlike Equation (28), the right side of Equation (53) is the error vector. Through Equation (53), the unknown target vector u can be solved by the weighted least squares algorithm and the weighted least squares solution can be expressed aŝ Here W 2 is the weight matrix, it can be written as Remark 1. The algorithm can achieve the best localisation effect with at least four sensors. This is because the TDOA/AOA localisation of the algorithm in this paper has some independence. Four sensors are needed to achieve the best localisation effect for TDOA.
Remark 2. The proposed algorithm needs to solve the distance r i . By introducing the noise covariance matrix Q through Equation (28), a rough estimated target position can be solved by a weighted least squares (WLS) algorithm, where the weight matrix is Q −1 . Using the rough estimated target position, r i can be calculated.

Stage 2:
The first stage calculates the target position by introducing measurement error. In the second stage, we will adjust the results of the previous stage by estimating the target position error. Construct the target position error vector Δu = [Δu x , Δu y , Δu z ] T and the relationship between it and the target position is Bring Equation (60) into AOA Equations (40) and (41), then there are Sorting out Equations (61) and (62) gives Considering all the equations composed of sensors and target position error, the matrix forms of Equations (63) and (64) can be written as where Bring Equation (60) into TDOA Equation (47) so there is The distance between the target and the sensor i can be derived as where a and b are vectors of the same dimension and Considering Equations (3) and (71), the TDOA Equation (70) can be written asfollows: [9] 2(s i − s 1 Therefore, the system of TDOA localisation equations composed of all sensors and target position error can be expressed as where The weighted least squares solution of Equation (78) can be expressed as where The final localisation result can be written as Table 1 shows the program summary of the algorithm.

COMPARISON WITH THE CRLB
CRLB shows the best estimation accuracy that can be obtained in unbiased estimation, so it is usually used to evaluate algorithm performance whether the localisation error of the algorithm is close to CRLB. It can be expressed as the inverse of the Fisher matrix as follows: where p(̂; u 0 ) is a probability density function about̂parameterised by the vector u 0 . When the noise obeys the Gaussian distribution, p(̂; u 0 ) is a Gaussian function with mean and covariance Q. CRLB can be written as The Appendix gives details of Equation (87). Let Δū = Δû − Δu and rewrite Equation (78) as Bring Equations (88) into (82) to get Then Δū can be written as By introducing Equations (60) and (85), the covariance matrix cov(ū) of the algorithm in this paper can be expressed as (91) where P = B −1 3 A 3 . P can be expanded into complex matrix multiplications. In the case of small noise, we can conclude that

SIMULATION RESULTS
In this section, three different scenarios are employed to demonstrate the performance of the algorithm. In the first and second scenarios, four sensors were used for simulation and the TDOA/AOA measurement value noise was changed to verify the algorithm performance. In the third scenario, the localisation performance of the proposed algorithm in the x, y and z directions is shown in a localisation scene with high-angle noise. Localisation accuracy can be expressed by root mean square error (RMSE) and bias, RMSE = Carlo experiments. When there is no specific explanation, the default is L = 10, 000. In the simulation, all noise errors follow a Gaussian distribution and the mean value is zero.  [26], the TD/AOA algorithm of [27] and CRLB with our method. Localisation RMSE and bias is shown in Figures 2 and 3.

Effect of changing TDOA measurement noise on estimator
In the simulations of Figures 2 and 3, the angular noise variance is 2 The abscissa represents the logarithmic form of the TDOA noise variance 2 T , which ranges from -20 to 20 dB. It can be seen from the RMSE curve in Figure 2 that the RMSE of the algorithm in this paper is always lower than the algorithm in [26]. When 2 T is between -20 and 15 dB, the RMSE of the proposed algorithm is slightly higher than that of the algorithm in [27]. When 2 T is greater than 15 dB, the RMSE of the proposed algorithm and the algorithm of [27] are approximately equal to the CRLB. When 2 T increases to 20 dB, the reason why the RMSE does not increase is that the angle equation still has good localisation performance when the angle noise does not change.
The bias curve in Figure 3 shows that the bias of the algorithm in this paper is similar to that of the algorithm in [27], and both are lower than that of the algorithm in [26]. The algorithm in this paper uses the TDOA equation (47) in the first stage, which reduces the inaccuracy of localisation caused by ignoring the noise square term of the measurement value, so the localisation result is better than the algorithm in [26]. The algorithm of [27] changes the TDOA localisation equation by introducing the angle equation, which makes the accuracy of the algorithm very high when the angle error is small, even better than the algorithm in this paper. In order to better reflect the performance of the proposed algorithm, Figure 4 shows the cumulative density function (CDF) at 2 T = 5 m 2 . In Figure 4, the abscissa represents the error between the target estimated position and the actual position, and the unit is m. It can be seen from Figure 4 that the CDF curve of the algorithm in this paper rises slightly faster than the algorithm in [26], but the CDF curve of the algorithm in [27] has the fastest rise. The results show that, in the case of low-angle noise, the algorithm in this paper is more accurate than the algorithm in [26], but the accuracy is slightly inferior to the algorithm in [27].

Effect of changing AOA measurement noise on estimator
In this scenario, the number of underwater sensors is still four, and the sensor distribution is the same as in the previous scenario. The performance of the proposed localisation algorithm is observed by changing only the AOA noise variance. The RMSE and bias curves of the localisation are shown in Figures 5  and 6.
In the simulations of Figures 5 and 6, the TDOA noise variance is 2 T = 5 m 2 . The abscissa represents the logarithmic form of the AOA noise variance 2 A , which ranges from -40 to 10 dB, where 2 Figure 5 shows the results of RMSE. It can be seen that the proposed algorithm has lower error rate than the algorithm of [26]. When 2 A is between -40 and -10 dB, the algorithm in this paper and the algorithm in [27] can be close to CRLB. When 2 A is between -10 and 0 dB, the algorithm in this paper is slightly inferior to the algorithm in [27]. But when 2 A is greater than 0 dB, the RMSE curve of the algorithm in this paper is much larger than that in [27]. This is mainly because the algorithm of [27] considers more angle equations, which leads to low accuracy in the case of high angle noise. Figure 6 shows the bias results. It can be seen that the bias of the proposed algorithm is small. In general, the performance of the proposed algorithm is superior when changing only AOA measurement noise. The proposed algorithm calculates the target position by introducing two errors (Equations (3) and (60)), and simulations show that this improvement is huge. Figure 7 shows the cumulative density function when 2 A = 9 o . We can see from Figure 7 that the CDF curve of the algorithm in this paper is similar to the algorithm in [26], and both are better than the algorithm in [27].

Localisation scenario with high-angle noise
In this scenario, the sensor distribution is the same as in the previous scenario. This section will introduce the localisation performance of the proposed algorithm in the x, y and z axis under high-angle noise scenarios. The simulation results are shown in Figures 8-10.  In Figures 8-10, the TDOA noise is 2 T = 5 m 2 . The abscissa represents the logarithmic form of the angular noise 2 A , ranging from 0 to 10 dB, where 2 A . It can be seen that when 2 A is between 0 and 5 dB, the RMSE curve of the proposed algorithm is close to that of [27], and both are better than the algorithm of [26]. When 2 A is between 5 and 10 dB, the algorithm in [26] is even better than the algorithm in [27], whereas the algorithm in this paper is slightly better than the algorithm in [26]. In the case of high-angle noise, the RMSE of the proposed algorithm and other algorithms cannot approach CRLB because both approximations are made: sin Δ i ≈ Δ i and cos Δ i ≈ 1. When 2 A is greater than 1, these two approximations will result higher number of errors. Simulations in the x, y and z directions prove that the algorithm in this paper still has better localisation performance than the existing algorithms in the case of high-angle noise.
It can be seen from Table 2 that the complexity of the algorithm in this paper is slightly higher than that in [26] and [27]. This is because the second stage of the algorithm in this paper considers the two equations of TDOA and AOA, which sacrifices a certain amount of running time.

CONCLUSION
This paper proposes an improved underwater TDOA/AOA joint localisation algorithm, which uses the two-stage weighted least squares algorithm to achieve target localisation. In the first stage, a measurement value error is introduced into the localisation equation, so that the localisation problem can be solved using the weighted least squares algorithm. In the second stage, a new localisation equation is constructed by introducing the target position error, and the target position estimated in the previous stage is adjusted with the solved target position error. A comparison between the covariance matrix of the proposed algorithm and CRLB prove that the performance of this algorithm is better in the noise environment. Simulations also prove the effectiveness and reliability of our proposed algorithm.