Underwater 3D Doppler-Angle Target Tracking with Signal Time Delay

The traditional target tracking is a process of estimating the state of a moving target using measurement information obtained by sensors. However, underwater passive acoustic target tracking will confront further challenges, among which the system incomplete observability and time delay caused by the signal propagation create a great impact on tracking performance. Passive acoustic sensors cannot accurately obtain the target range information. The introduction of Doppler frequency measurement can improve the system observability performance; signal time delay cannot be ignored in underwater environments. It varies with time, which has a continuous negative impact on the tracking accuracy. In this paper, the Gauss–Helmert model is introduced to solve this problem by expanding the unknown signal emission time as an unknown variable to the state. This model allows the existence of the previous state and current state at the same time, while handling the implicit equations. To improve the algorithm accuracy, this paper further takes advantage of the estimated state and covariance for the second stage iteration and propose the Gauss–Helmert iterated Unscented Kalman filter under a three-dimensional environment. The simulation shows that the proposed method in this paper shows superior estimation accuracy and more stable performance compared with other filtering algorithms in underwater environments.


Introduction
In recent decades, multiple new techniques have been applied to underwater tracking systems in the literature. Among various underwater tracking methods, the passive target tracking methods have attracted great interest [1][2][3][4][5] by the research community. Compared to the active system, the costs of operation and maintenance for passive systems are lower [6]. In some special applications, a passive system has the advantage of keeping covert and hardly being detected [7]. The angle-only tracking and Doppler-angle tracking problems relied on acoustic sensors and belonged to the passive target tracking [8,9]. The main problem is estimating the unknown target state through the noisy measurements acquired by acoustic sensors [10,11].
The high nonlinearity of the measurement system makes the Doppler-angle tracking problem complex and difficult to solve [12,13]. Various nonlinear filtering algorithms for Doppler-angle tracking can be roughly divided into two types. One approach is the pseudo linearization method, which has less calculation with a wide range of application compared to other methods [14][15][16]. However, the result of the estimation is biased, which will continuously affect the tracking process even if the three-dimensional space. A novel method is proposed to deal with the underwater Doppler-angle tracking problem.
The remainder of the paper is planned as the following five sections. Section 2 investigates the Doppler-angle target tracking problem with the system state and measurement model in an underwater three-dimension environment. In Section 3, the problems caused by signal time delay is investigated. The new Gauss-Helmert Iterated Unscented Kalman Filter (GH-IUKF) algorithm is derived in Section 4. Section 5 provides a comparative analysis of the performance for the proposed algorithm and other different algorithms under various conditions. Finally, the conclusion is drawn in Section 6.

System Model Description
Assuming the target is moving at a constant velocity in an underwater environment. We deploy a single maneuvering observer, which includes three passive sensors together. The location of the observer is considered to be known in advance. These sensors are used for receiving the bearing information, elevation information and frequencies of the target. Define x T = (x T , y T , z T ) as the location andẋ T = (ẋ T ,ẏ T ,ż T ) as the velocity of the target, f is the Doppler frequency from the target while t k is the time of k th observer sampling. Then the target state vector can be expressed as Similarly, the observer motion state is defined as The relationship of the target state between two consecutive times t k and t k−1 can be described by the discrete state transition model as where F( t k |t k−1 ) denotes the state transition matrix and w( t k |t k−1 ) denotes the zero-mean Gaussian white noise, Γ( t k |t k−1 ) is the system noise-driven noise matrix. For the constant velocity (CV) model where T is the observer sampling interval. The measurement equation can be written as where h(x(t k ))=[θ(t k ) ϕ(t k ) f S (t k )] denotes nonlinear measurement function, v(t k ) is the measurement noise.

Doppler-Angle Tracking in Three-Dimension Space
In this section, we analyze the geometric relationship between the target and the observer, then apply the frequency information to the three-dimensional space.
As shown in Figure 1, the target and observer are located at points A and O respectively. The target is moving from point A towards B at the instant t k . α denotes the XOY plane, β denotes the plane parallel to the XOY plane and passing through point A. θ represents the horizontal bearing angle and ϕ denotes the vertical pitch angle.
. According to the trigonometry, the relative radial velocity V r XOY in the β plane can be expressed as Then the relative radial velocity V r in three-dimension space is calculated by where where l(t k ) = (x T (t k ) − x S (t k )) 2 + (y T (t k ) − y S (t k )) 2 represents the horizontal distance between the target and observer. Similarly, When the target moves as a certain radial velocity relative to the observer, the frequency of the target signal will be affected by the Doppler shift during the propagation, then the frequency measurements f S obtained by the observer without noise is given by where c is the underwater sound speed. Substituting Equations (7)-(10) into (11), the frequency measurements without noise can be expressed as Similarly, the angle measurement functions can be expressed as ).
Then the measurements for the Doppler-angle only system satisfy the following equations as where v θ (t k ), v ϕ (t k ), and v f (t k ) are the random measurement errors, which follows the zero-mean Gaussian distribution respectively.

Signal Propagation Delay Analysis
In this section, we analyzed the effect of unknown signal propagation delay on the tracking system and introduced a nonlinear Gauss-Helmert model to solve this problem.
In Figure 2, t e k denotes the signal emission time within one interval of sampling. t S k is the time when the sensor receives signal and t d k represents the signal time delay. The signal emission time is given by The relationships among t S k and t e k are given by where ∆ e k is the interval between two signal emission time, T is the interval of sampling, and τ is a varying time offset.
According to the description of Figure 3, the time sequence of the tracking system under signal time delay is distinct from the one without delay. The signal transmission delay will have a strong impact on the system, which leads to ∆ e k being not equal to the sensor interval. As the result, ∆ e k will be a time varying parameter according to the target state. In Section 2, the discrete dynamic model requires that time intervals between the adjacent state vectors should always be the same. When the We regard t e k as an unknown variable in the state vector. However, two adjacent state vectors x T (t e k ) and x T (t e k−1 ) will exist in the state transition function simultaneously, which cannot be solved by the nonlinear GMM.
Here, we introduce the implicit equations based on GHM to handle this problem as where 0 n represents the (n) × (n) zero vector, and n is the state dimension. w(t e k−1 ) is the noise which follows the Gaussian distribution, Γ( t e k t e k−1 ) is the system noise gain matrix. The state vector x T (t e k ) in (1) is extended as The specific form of the implicit state equations under the CV model are given by where where d(t e k ) denotes the distance from the target to the observer and c is the constant underwater sound speed. Functions s 1 to s 6 are the constraint functions for the CV model, which denotes the state transition of target motion parameters between two neighbor instants t e k and t e k−1 . s 7 represents the signal frequency which is varying by the Doppler effect. s 8 is the time delay constraint, which describes the relationship for the time series.
Affected by signal propagation delay, the state transition matrix for the CV model is rewritten as To describe the uncertainty of the variable, the process noise gain matrix and the covariance of w(t e k−1 ) are given by where σẍ, σÿ and σz are the noise standard deviations to describe the unknown small acceleration for the x, y and z axis, respectively. σ f and σ te are the system process noise standard deviations to show the random disturbance of the system. The system process noise matrix is Note that the geometric relationship in the measurement Equations (16)- (18) will not change here, however, the measurements received by the observer have a different time axis from the target state. The measurement equations for the Doppler-angle only system with signal propagation delay are given by

Gauss-Helmert Iterated Unscented Kalman Filter
The traditional nonlinear filtering is mainly based on the EKF, which has a wide range of applications and low calculation cost. However, it also has the disadvantages of low accuracy and poor stability, and is only suitable for the weak nonlinear Gaussian environments. Particle filtering can be used for nonlinear non-Gaussian systems, however, the main problem is that a great number of samples are required to approximate the system posterior probability density. The complexity of the algorithm is positively related to the number of samples which describes the posterior probability distribution. UKF combines the unscented transformation and Kalman filter. UKF approximates the n-dimensional target sampling points by designing the weighted point σ, and calculates these σ points propagated by the nonlinear function. Then, the updated state by the nonlinear equation is obtained. Different from the EKF filter, the state processed by UKF reserves more nonlinear characteristics and keeps better estimation performance. For the non-Gaussian measurements, the estimation accuracy of UKF approximates to at least the second-order of the Taylor series expansion. For the non-Gaussian noise, the estimation accuracy of UKF approximates at least the second-order of the Taylor series expansion. The UKF can further approximate the third order of Taylor series expansion for all nonlinear equations with Gaussian noise [21]. Moreover, the computational complexity of UKF is much smaller than that of PF. Therefore, we choose the UKF as the main framework of the filter to deal with the Doppler-angle tracking problem with signal time delay.
In the previous section, under the influence of the signal propagation delay, the unknown parameters of the target state, the state transition equations and the measurement equations are different from the usual Doppler-angle tracking. Similarly, the Iterated Unscented Kalman filter (IUKF) implementation process also needs to make a response adjustment. The estimation of x * (t e k ) can be acquired from the following steps.

Select the sigma sample points
For a given state x * (t e k−1 ) and corresponding error covariance P(t e k−1 ) at the instant t e k−1 , the sigma points matrix ξ(t e k−1 ) with associated weights w i are calculated as where n denotes the state dimension. λ = α 2 (n + κ) − n is a scaling parameter which can affect the estimation accuracy. α and κ are the parameters to control the spread of the sigma points. β is determined by the state vector distribution.

The predicted state
According to (23), the function group s(·) contains state vectors at the instant t e k−1 and t e k simultaneously. The sigma points cannot be propagated by these implicit functions. The solution of the above equations can be equivalently treated as an unconstrained optimization problem. In numerical optimization, iterative methods are generally used to solve such problems. In this paper, we adopt the Gauss-Newton method to calculate the local optimum solution. For the each predicted sigma point vector ξ i (t e k |t e k−1 ) (ξ i represent the (i + 1)th column of the matrix ξ. i = 0, 1, 2, · · · , 2n), the iterative process is given by where l is the iteration index, J s is the Jacobian matrix given by For simplicity, the time label at the last row of the matrix in Equation (48) is omitted. Define the sum of squared residuals for the Gauss-Newton method as For the given iteration convergence threshold parameter a, the accuracy criterion for iteration is denoted as The the initial value for iteration process can be calculated according to [36], or it can be approximately replaced by The convergence criteria of the Gauss-Newton method is given in Appendix A.
The prediction state and corresponding error covariance are given bŷ where Q(t e k |t e k−1 ) is correlates with the state, but this dependence is neglectable during the calculation process.

Measurement update
For the prediction statex(t e k |t e k−1 ) and covariance P(t e k |t e k−1 ) in step 2, re-propagate the sigma points according to The one-step measurement prediction is propagated by the following measurement equationŝ where ζ i represent the (i + 1)th column of the matrix ζ.
The state and covariance for the first stage of GH-IUKF are given bỹ where R(t S k ) denotes the variance of measurement noise at the sampling instant t S k .

The iterated stage
On the Basis of the UKF algorithm, an iteration method in [21] is adopted to estimate the target state and covariance matrix.
(iv) Define the judgment criteria equations (v) The termination criteria for iterative process: If the following inequality satisfies or if j reaches the N max , stop the iterative and outputx(t e k ) = [x(t e k )] j , P(t e k ) = [P(t e k )] j . Otherwise, let g = η · g, j = j + 1 and go back to step (iii).
Different from the standard Gauss-Helmert Unscented Kalman Filter (GH-UKF) method, GH-IUKF corrects the measurement to adjust the estimation of state and adaptively approximate the true value. The estimation error of state is expected lower than GH-UKF after the iteration is terminated. In addition, GH-IUKF can respond to the iterated measurements rapidly, and keep a faster convergence speed even if the initial error is large. Algorithm 1 summarizes the main process of the GH-IUKF algorithm.

Iterated stage:
(1) Let the initial value [x(t e k )] 1 =x(t e k ), [P(t e k )] 1 = P(t e k ). (2) Iterate the state and error covariance until they are satisfied with the termination criteria j=2, g=1;

Numerical Simulation
This section considers a maneuvering observer (equipped with three sensors, which is applied to obtain the bearing angle, elevation angle and Doppler frequency, respectively) to track an underwater target in three-dimensional space. The underwater target scenarios are modified according to the test case in [3,34]. The initial position of single maneuvering observer (x S 0 , y S 0 , z S 0 ) is (−500, 0, 0) m and its' trajectory is selected to move on a curve in the horizontal plane, where the velocity in the x-axis direction maintains atẋ S (t S k ) = 12 m/s. Then the relationships of observer motion parameters are defined as the following equations In this scenario, we assume a single target moves forward with the constant velocity (−20, 10, 0)m/s, and its' initial location is (2000, 1200, 200)m. The underwater sound propagation speed c is 1480 m/s, the initial source frequency from the target is f 0 = 365 Hz, and the initial signal emission time t e 0 is calculated according to (19). Both of the bearing angle and elevation angle measurement noise standard deviation are σ v θ = σ v ϕ = 3 • , Doppler frequency measurement error standard deviation is σ v f = 5 Hz. The sensors sampling interval is T= 1 s, and the observer obtains measurement data from t s = 6 s to t s = 105 s. The system process noise covariance matrix is given by The initial state x * 0 is randomly generated by the true value and the initial error covariance according to [30] The position root mean square error (RMSE) is given by where N is the numbers of Monte-Carlo simulation,x(t e k ),ŷ(t e k ) andẑ(t e k ) are the estimation of position, x(t e k ), y(t e k ) and z(t e k ) are the true target position. The statistical test for filter consistency based on the average normalized estimation error squared (ANEES) which is given by [39] where n is the dimension of the state vector, (x i (t e k ) −x i (t e k )) and P(t e k ) are the filter estimation error and covariance for the ith Monte-Carlo simulation at the instantt e k . The acceptance interval threshold σ 1 and σ 2 at level α = 0.05 are calculated by [40] Three extra algorithms are tested to compare with the proposed algorithm in this paper.
• GH-IUKF: The proposed algorithm in this paper. It considers the existence of signal time delay between the target and the observer, using the estimated state and its covariance matrix to perform the second-stage iterative process. It extends the emission times t e k to the state by GHM in both of the two stages of filtering. The iterate number is set to N max = 3.
• GH-UKF: This is the existing approach which uses the GHM framework, and it expands t e k as a variable into the state.
• IUKF-D: This method only takes t d k as an unknown parameter in the first stage, while it will become a certain parameter in the iterated stage. The iterate number is set to N max = 3.
• UKF-D: The state transition model of this method is based on the GMM framework, and the signal propagation delay t d k is treated as an unknown parameter, which is obtained by an iterative method while the state is updated. Figure 4 depicts the target trajectory without noise and the estimated trajectory in 3-D space. The observer maneuvers forward on the surface of water, while the target keeps moving underwater with a constant speed at a certain depth. It can be seen that all approaches can effectively track the target during the entire tracking process. Where GH-IUKF is superior to other approaches in both accuracy and convergence speed, the performance of UKF-D is the worst of all, and it is not sensitive to the measurement update and the tracking trajectory will always be unable to get close to the true target trajectory.  Figure 5 shows the position RMSE of the target with the above four algorithms under 1000 Monte Carlo runs. In order to compare the performance of all algorithms more clearly, the posterior Cramer-Rao lower bound (PCRLB) in [41] is selected as the performance criteria for comparison (the signal emission time is assumed to be known while calculating the bound). The observer starts to receive measurements at t s = 6 s and end at t s = 105 s with a total number of 100 data for all methods. It can be seen that GH-IUKF shows the highest accuracy of estimation among the four algorithms, and it is very close to PCRLB. This is because the algorithm adopts two stages filtering, which can adjust the state estimate and adaptively decrease the errors. Furthermore, the algorithm applies the GHM state transition model, which is superior to GMM. The signal emission time is expanded as a state variable and the additional information can provide better performance. Correspondingly, the overall trend of GH-UKF and GH-IUKF are basically the same. The RMSE of these two algorithms has a significant drop when t s = 50 s, and the convergence speed is faster than the other two algorithms. However, its accuracy is not as good as GH-IUKF and cannot be closer to PCRLB. The tracking performance of IUKF-D is slightly better than GH-UKF before t s = 50 s. However, due to the disadvantage of the model, it was overtaken by the latter in the second half of the tracking process.  As the initial error covariance of the filter is relatively large, the RMSE will change rapidly at the beginning from t s = 6 s to t s = 9 s, and the difference of estimation performance between different algorithms is very small. In order to make the curves in the graph easier to distinguish, the horizontal axis of the following graphs related to RMSE will start from t s = 10 s. Figure 7 shows the RMSE components (position and velocity) with all stated algorithms. The target is moving close to the observer on the x-axis, which will cause the increment in varying rates of the measurement angle. Therefore, in Figure 6a, the performance of all algorithms is stable, and eventually converges to PCRLB; comparatively, the target is far away from the observation station on the y-axis. As shown in Figure 6b, the tracking performance of the entire system decreases, which has a great impact on all algorithms. Only GH-IUKF can resist this negative effect and the RMSE eventually approaches PCRLB. Both the target and the observer are kept at a certain depth. In Figure 6c, the system performance on the z-axis is very stable, and all algorithms converge quickly. Figure 8 describes the RMSE of the estimated signal emission time. GH-IUKF and GH-UKF regard the emission times as a state variable, while IUKF-D and UKF-D calculate the emission time by (19) after the target state is estimated because these latter two methods do not expand the unknown signal emission time to the state. Figure 9 depicts the angle and frequency calculated based on GH-IUKF estimate and it is compared with the measurement and true value. By the influence of noise, the measurements will fluctuate widely compared with the true value. The measurement estimation calculated based on the estimated state value by the GH-IUKF algorithm filter proposed in this paper is very close to the real measurements, which further proves the high accuracy of the GH-IUKF algorithm. Table 1 gives a performance comparison for the above methods, which includes time costs for each simulation and average RMSE for 1000 Monte-Carlo runs with different angle measurement noise standard deviations.    (b) Figure 9. (a) The true angle, the angle measurement and estimated angle for GH-IUKF; (b) the true frequency, the frequency measurement and estimated frequency for GH-IUKF. Figure 10 depicts the position of the RMSE of the target without Doppler frequency. The PCRLB-with frequency in this figure is to compare the effect of the Doppler frequency measurement information for the filter on the overall estimation performance. Compared with Figure 5, we found that the additional Doppler frequency measurement information has slightly improved the accuracy of all methods. The underwater acoustic propagation speed is approximately five times that in air, but the velocity of the underwater target is much slower than the ground or air target, so the Doppler measurement has limited improvement for the underwater target tracking accuracy. However, by comparing the two PCRLBs in Figure 9, we found that the Doppler measurement can still improve the overall performance of the system and make the system more stable.

Conclusions
Acoustic signal propagation delay cannot be ignored in underwater target tracking problems, and it varies with time, which has a continuous negative impact on the tracking effect. In this paper, GHM is introduced to expand the unknown signal transmission time as an unknown variable to the state, and the solution is based on the optimization method. On the basis, this paper further utilizes the estimated state and covariance for the second stage iteration to improve the algorithm accuracy. In addition, we also apply Doppler frequency measurement to improve the overall performance of the system and expand it into a three-dimensional space. The simulation shows that the proposed method GH-IUKF in this paper equips higher accuracy and more stable performance in underwater environments than other algorithms. Aiming at the complex underwater environmental characteristics, future research will focus on the initial value calculation method of target tracking based on underwater signal propagation delay.

Conflicts of Interest:
The authors declare no conflict of interest.