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

Abstract

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.

1. 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 number of measurements increased. Another one is the recursive nonlinear filter method, including the extended Kalman filter (EKF) and unscented Kalman filter (UKF) based methods. The basic idea of the former method is to utilize the first-order or second-order Taylor series expansion, which is easy to implement in various problems [17,18]. However, the disadvantage of linearization is that it will greatly increase the filtering error when handling highly nonlinear systems, which may eventually lead to divergence of the tracking system. The latter method is to fit the probability density distribution of the nonlinear equations with fixed parameters through unscented transform [19,20,21]. UKF avoids the loss of higher-order terms in Taylor series expansion caused by linearization and improves accuracy [22,23].

The frequency measurements can provide extra target motion information, which makes the Doppler-angle tracking better than angle-only tracking in observability and estimation performance [24,25]. The condition is that the target’s radiation frequency must remain constant during the tracking period [26]. In this case, as long as the system satisfies the observability condition for a single observer, even the observers are not required to maneuver, which is very useful in practical applications [27].

Passive target tracking often relies on acoustic sensors to obtain the angle measurements, and acoustic signals cannot be immediately propagated from the target to the acoustic sensors. However, the time delay caused by a signal spread in the medium is usually considered by having little effect on filtering results in the general Doppler-angle tracking problem [28]. In other words, the signal is often regarded as being transmitted from the target to the sensors instantly. This assumption is valid when the signal speed is much greater than the target motion speed. For example, in the radar systems, the propagation speed of radio waves is approximately equal to $3\times 8$ m/s. In this case, the signal propagation delay is generally negligible. However, this assumption is too ideal in the underwater environment. As the only reliable signal that can propagate long distances in an underwater environment, the underwater acoustic speed is about 1480 m/s, which is much less than the propagation speed of light or radio. Under this circumstance, the impact of signal propagation delay is more apparent. The position of the target continuously varies while the signal travels to the observer [29]. As a consequence, the estimated target state and its true value will be remarkably different if the time delay is neglected [30].

The main issues that make the passive target tracking with signal propagation a challenging problem are time varying delay and the incomplete observability of the system [31,32,33]. In order to get the target state, multiple strategies have been applied in this domain. In early researches, the interval between two consecutive signals is regarded as an unknown parameter [34,35], which is calculated recursively through a linear search method. This method has two disadvantages, for example, the signal transmission time interval is calculated directly during the iteration process without considering noise while iterative calculation may be excessive due to the linear search method. Another strategy extends the state, and the signal emission time becomes one of the variables [36,37,38]. Unlike the traditional Gauss–Markov model (GMM), the Gauss–Hermit model (GHM) in these algorithms can be used to describe strongly complex state equations, which allows the existence of the previous state and the current state at the same time. Compared with the first method, the GHM method equipped with Unscented transformation has better estimation performance. However, the application of these methods are relatively limited. In [38], the method is mainly aimed at eliminating the unknown time offset between two stationary observers under the influence of time propagation delay. The approach in [36] utilizes a maneuvering single observation station to deal with the bearing-only target tracking. Due to the lack of special optimization, the performance improvement of this method for underwater targets is not significant, and it cannot be directly applied in 3D space. Meanwhile, the approach has been extended to the Doppler-bearing tracking in [37], which rely on an electronic support measures/electro-optical (ESM/EO) sensor. However, it cannot be deployed in an underwater environment.

In this paper, we investigate the Doppler-angle tracking with the propagation delay by the single maneuvering observer in an underwater environment, and expand the application to 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.

2. Problem Description

2.1. 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 ${\mathbf{x}}^T=(x^T,y^T,z^T)$ as the location and ${\dot{\mathbf{x}}}^T=({\dot{x}}^T,{\dot{y}}^T,{\dot{z}}^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

$${\mathbf{x}}^T(t_k)={[x^T(t_k)y^T(t_k)z^T(t_k){\dot{x}}^T(t_k){\dot{y}}^T(t_k){\dot{z}}^T(t_k)f^T(t_k)]}^{\prime },$$

(1)

Similarly, the observer motion state is defined as

$${\mathbf{x}}^S(t_k)={[x^S(t_k)y^S(t_k)z^S(t_k){\dot{x}}^S(t_k){\dot{y}}^S(t_k){\dot{z}}^S(t_k)]}^{\prime }.$$

(2)

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

$${\mathbf{x}}^T(t_k)=\mathbf{F}(t_k\left|t_{k-1}\right.){\mathbf{x}}^T(t_{k-1})+\mathbf{\Gamma }(t_k\left|t_{k-1}\right.)\mathbf{w}(t_k\left|t_{k-1}\right.),$$

(3)

where $\mathbf{F}(t_k|t_{k-1})$ denotes the state transition matrix and $\mathbf{w}(t_k|t_{k-1})$ denotes the zero-mean Gaussian white noise, $\mathbf{\Gamma }(t_k|t_{k-1})$ is the system noise-driven noise matrix. For the constant velocity (CV) model

$$\mathbf{F}(t_k\left|t_{k-1}\right.)=\left[\begin{array}{ccccccc}1& 0& 0& T& 0& 0& 0\\ 0& 1& 0& 0& T& 0& 0\\ 0& 0& 1& 0& 0& T& 0\\ 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 1\end{array}\right]$$

(4)

$$\mathbf{\Gamma }=\left[\begin{array}{cccc}{T}^2/2& 0& 0& 0\\ 0& T^2/2& 0& 0\\ 0& 0& T^2/2& 0\\ T& 0& 0& 0\\ 0& T& 0& 0\\ 0& 0& T& 0\\ 0& 0& 0& 1\end{array}\right]$$

(5)

where T is the observer sampling interval. The measurement equation can be written as

$$\mathbf{z}(t_k)=h(\mathbf{x}(t_k))+\mathbf{v}(t_k).$$

(6)

where $h(\mathbf{x}(t_k))$=${[\theta (t_k)\phi (t_k)f^S(t_k)]}^{\prime }$ denotes nonlinear measurement function, $\mathbf{v}(t_k)$ is the measurement noise.

2.2. 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$. $\alpha$ denotes the $XOY$ plane, $\beta$ denotes the plane parallel to the $XOY$ plane and passing through point A. $\theta$ represents the horizontal bearing angle and $\phi$ denotes the vertical pitch angle.

Assuming that $\stackrel{⟶}{AB}$ represents the instantaneous velocity of the target, $B^{\prime }$ is the projection of point B in the $\beta$ plane, then $\stackrel{⟶}{AC}={\dot{x}}^T(t_k)-{\dot{x}}^S(t_k),\stackrel{⟶}{C{B}^{\prime }}={\dot{y}}^T(t_k)-{\dot{y}}^S(t_k)$. According to the trigonometry, the relative radial velocity $V_{XOY}^r$ in the $\beta$ plane can be expressed as

$$V_{XOY}^r(t_k)=\stackrel{⟶}{AF}+\stackrel{⟶}{D{B}^{\prime }}=({\dot{x}}^T(t_k)-{\dot{x}}^S(t_k))sin\theta (t_k)+({\dot{y}}^T(t_k)-{\dot{y}}^S(t_k))cos\theta (t_k),$$

(7)

Then the relative radial velocity $V^r$ in three-dimension space is calculated by

$$V^r(t_k)=V_{XOY}^r(t_k)/cos\phi (t_k)=\frac{({\dot{x}}^T(t_k)-{\dot{x}}^S(t_k))sin\theta (t_k)+({\dot{y}}^T(t_k)-{\dot{y}}^S(t_k))cos\theta (t_k)}{cos\phi (t_k)},$$

(8)

where

$$sin\theta (t_k)=\frac{x^T(t_k)-x^S(t_k)}{l(t_k)}$$

(9)

$$cos\theta (t_k)=\frac{x^T(t_k)-x^S(t_k)}{l(t_k)}$$

(10)

$$cos\phi (t_k)=\frac{l(t_k)}{d(t_k)}.$$

(11)

where $l(t_k)=\sqrt{{(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, $d(t_k)=\sqrt{{(x^T(t_k)-x^S(t_k))}^2+{(y^T(t_k)-y^S(t_k))}^2+{(z^T(t_k)-z^S(t_k))}^2}$ denotes the distance between the target and observer.

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

$$f^S(t_k)=f^T(t_k)(1+\frac{V^r(t_k)}{c}),$$

(12)

where c is the underwater sound speed. Substituting Equations (7)–(10) into (11), the frequency measurements without noise can be expressed as

$$\begin{array}{cc}\hfill f^S(t_k)=& f^T(t_k)(1+\frac{d(t_k)[({\dot{x}}^T(t_k)-{\dot{x}}^S(t_k))(x^T(t_k)-x^S(t_k))+({\dot{y}}^T(t_k)-{\dot{y}}^S(t_k))(y^T(t_k)-y^S(t_k))]}{[{(x^T(t_k)-x^S(t_k))}^2+{(y^T(t_k)-y^S(t_k))}^2]c}),\hfill \end{array}$$

(13)

Similarly, the angle measurement functions can be expressed as

$$\begin{array}{cc}\hfill \theta (t_k)& =arctan(\frac{x^T(t_k)-x^S(t_k)}{y^T(t_k)-y^S(t_k)})\hfill \end{array}$$

(14)

$$\begin{array}{cc}\hfill \phi (t_k)& =arctan(\frac{z^T(t_k)-z^S(t_k)}{l(t_k)}).\hfill \end{array}$$

(15)

Then the measurements for the Doppler-angle only system satisfy the following equations as

$$\begin{array}{cc}\hfill z_{\theta }(t_k)& =h_1(\mathbf{x}(t_k))+v_{\theta }(t_k)=arctan(\frac{x^T(t_k)-x^S(t_k)}{y^T(t_k)-y^S(t_k)})+v_{\theta }(t_k)\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill z_{\phi }(t_k)& =h_2(\mathbf{x}(t_k))+v_{\phi }(t_k)=arctan(\frac{z^T(t_k)-z^S(t_k)}{l(t_k)})+v_{\phi }(t_k)\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill z_f(t_k)& =h_3(\mathbf{x}(t_k))+v_f(t_k)=f^T(t_k)(1+\frac{V^r(t_k)}{c})+v_f(t_k).\hfill \end{array}$$

(18)

where $v_{\theta }(t_k)$, $v_{\phi }(t_k)$, and $v_f(t_k)$ are the random measurement errors, which follows the zero-mean Gaussian distribution respectively.

3. 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_k^e$ denotes the signal emission time within one interval of sampling. $t_k^S$ is the time when the sensor receives signal and $t_k^d$ represents the signal time delay. The signal emission time is given by

$$t_k^e=t_k^S-t_k^d$$

(19)

The relationships among $t_k^S$ and $t_k^e$ are given by

$$\begin{array}{cc}\hfill & t_k^e-t_{k-1}^e={\Delta }_k^e\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill & t_k^S-t_{k-1}^S=T\hfill \end{array}$$

(21)

$$\begin{array}{cc}\hfill & T={\Delta }_k^e+\tau .\hfill \end{array}$$

(22)

where ${\Delta }_k^e$ is the interval between two signal emission time, T is the interval of sampling, and $\tau$ 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 ${\Delta }_k^e$ being not equal to the sensor interval. As the result, ${\Delta }_k^e$ 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 signal propagation delay cannot be ignored, the discrete dynamic Equation (3) is not suitable for this situation.

We regard $t_k^e$ as an unknown variable in the state vector. However, two adjacent state vectors ${\mathbf{x}}^T(t_k^e)$ and ${\mathbf{x}}^T(t_{k-1}^e)$ 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

$$s\left[{\mathbf{x}}^{*}(t_k^e),{\mathbf{x}}^{*}(t_{k-1}^e)\right]+\mathbf{\Gamma }(t_k^e\left|t_{k-1}^e\right.)\mathbf{w}(t_{k-1}^e)={\mathbf{0}}_n.$$

(23)

where ${\mathbf{0}}_{\mathbf{n}}$ represents the $(n)\times (n)$ zero vector, and n is the state dimension. $\mathbf{w}(t_{k-1}^e)$ is the noise which follows the Gaussian distribution, $\mathbf{\Gamma }(t_k^e\left|t_{k-1}^e\right.)$ is the system noise gain matrix. The state vector ${\mathbf{x}}^{\mathbf{T}}(t_k^e)$ in (1) is extended as

$${\mathbf{x}}^{*}(t_k^e)={[x^{\mathrm{T}}(t_k^e)y^T(t_k^e)z^T(t_k^e){\dot{x}}^T(t_k^e){\dot{y}}^T(t_k^e){\dot{z}}^T(t_k^e)f_k^T{t}_k^e]}^{\prime }.$$

(24)

The specific form of the implicit state equations under the CV model are given by

$$s(·)={[s_1(·)s_2(·)s_3(·)s_4(·)s_5(·)s_6(·)s_7(·)s_8(·)]}^{\prime },$$

(25)

where

$$\begin{array}{cc}\hfill s_1& =x^T(t_k^e)-x^T(t_{k-1}^e)-{\dot{x}}^T(t_{k-1}^e){\Delta }_k^e\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill s_2& =y^T(t_k^e)-y^T(t_{k-1}^e)-{\dot{y}}^T(t_{k-1}^e){\Delta }_k^e\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill s_3& =z^T(t_k^e)-z^T(t_{k-1}^e)-{\dot{z}}^T(t_{k-1}^e){\Delta }_k^e\hfill \end{array}$$

(28)

$$\begin{array}{cc}\hfill s_4& ={\dot{x}}^T(t_k^e)-{\dot{x}}^T(t_{k-1}^e)\hfill \end{array}$$

(29)

$$\begin{array}{cc}\hfill s_5& ={\dot{y}}^T(t_k^e)-{\dot{y}}^T(t_{k-1}^e)\hfill \end{array}$$

(30)

$$\begin{array}{cc}\hfill s_6& ={\dot{z}}^T(t_k^e)-{\dot{z}}^T(t_{k-1}^e)\hfill \end{array}$$

(31)

$$\begin{array}{cc}\hfill s_7& =f^T(t_k^e)-f^T(t_{k-1}^e)\hfill \end{array}$$

(32)

$$\begin{array}{cc}\hfill s_8& =t_k^e+t_k^d-t_k^s\hfill \end{array}$$

(33)

in addition

$$t_k^d=d(t_k^e)/c$$

(34)

$$d(t_k^e)=\sqrt{{(x^T(t_k^e)-x^S(t_k^S))}^2+{(y^T(t_k^e)-y^{\mathrm{S}}(t_k^S))}^2+{(z^T(t_k^e)-z^{\mathrm{S}}(t_k^S))}^2}.$$

(35)

where $d(t_k^e)$ 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_k^e$ and $t_{k-1}^e$. $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

$$\mathbf{F}(t_k^e\left|t_{k-1}^e\right.)=\left[\begin{array}{cccccccc}1& 0& 0& {\Delta }_k^e& 0& 0& 0& 0\\ 0& 1& 0& 0& {\Delta }_k^e& 0& 0& 0\\ 0& 0& 1& 0& 0& {\Delta }_k^e& 0& 0\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 0& 0& 1\end{array}\right],$$

(36)

To describe the uncertainty of the variable, the process noise gain matrix and the covariance of $\mathbf{w}(t_{k-1}^e)$ are given by

$$\mathbf{\Gamma }(t_k^e\left|t_{k-1}^e\right.)=\left[\begin{array}{ccccc}{\Delta }_k^{e^2/2}& 0& 0& 0& 0\\ 0& {\Delta }_k^{e^2/2}& 0& 0& 0\\ 0& 0& {\Delta }_k^{c^2/2}& 0& 0\\ {\Delta }_k^e& 0& 0& 0& 0\\ 0& {\Delta }_k^c& 0& 0& 0\\ 0& 0& {\Delta }_k^c& 0& 0\\ 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 1\end{array}\right],$$

(37)

$$\mathbf{q}=diag({\sigma }_{\ddot{x}}^2{\sigma }_{\ddot{y}}^2{\sigma }_{\ddot{z}}^2{\sigma }_f^2{\sigma }_{te}^2),$$

(38)

where ${\sigma }_{\ddot{x}}$, ${\sigma }_{\ddot{y}}$ and ${\sigma }_{\ddot{z}}$ are the noise standard deviations to describe the unknown small acceleration for the x, y and z axis, respectively. ${\sigma }_f$ and ${\sigma }_{te}$ are the system process noise standard deviations to show the random disturbance of the system.

The system process noise matrix is

$$\mathbf{Q}(t_k^e\left|t_{k-1}^e\right.)=\mathbf{\Gamma }(t_k^e\left|t_{k-1}^e\right.)\mathbf{q}\mathbf{\Gamma }(t_k^e\left|t_{k-1}^e\right.).$$

(39)

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

$$\begin{array}{cc}\hfill z_{\theta }(t_k^S)& =h_1(\mathbf{x}(t_k^{*}))+v_{\theta }(t_k^S)\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill z_{\phi }(t_k^S)& =h_2(\mathbf{x}(t_k^{*}))+v_{\phi }(t_k^S)\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill z_f(t_k^S)& =h_3(\mathbf{x}(t_k^{*}))+v_f(t_k^S).\hfill \end{array}$$

(42)

4. 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 $\sigma$, and calculates these $\sigma$ 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 ${\mathbf{x}}^{*}(t_k^e)$ can be acquired from the following steps.

1. Select the sigma sample points

For a given state ${\mathbf{x}}^{*}(t_{k-1}^e)$ and corresponding error covariance $\mathbf{P}(t_{k-1}^e)$ at the instant $t_{k-1}^e$, the sigma points matrix $\mathit{\xi }(t_{k-1}^e)$ with associated weights $w^i$ are calculated as

$$\begin{array}{cc}\hfill \mathit{\xi }(t_{k-1}^e)& =[{\widehat{\mathbf{x}}}^{*}(t_{k-1}^e){\widehat{\mathbf{x}}}^{*}(t_{k-1}^e)+\sqrt{(n+\lambda )\mathbf{P}(t_{k-1}^e)}{\widehat{\mathbf{x}}}^{*}(t_{k-1}^e)-\sqrt{(n+\lambda )\mathbf{P}(t_{k-1}^e)}],\hfill \end{array}$$

(43)

$$\begin{array}{cc}\hfill w_0^m& =\frac{\lambda }{n+\lambda },\hfill \end{array}$$

(44)

$$\begin{array}{cc}\hfill w_0^c& =\frac{\lambda }{n+\lambda }+(1-{\alpha }^2+\beta ),\hfill \end{array}$$

(45)

$$\begin{array}{cc}\hfill w_i^m& =w_i^c=\frac{\lambda }{2(n+\lambda )},i=1,2,\cdots ,2n.\hfill \end{array}$$

(46)

where n denotes the state dimension. $\lambda ={\alpha }^2(n+\kappa )-n$ is a scaling parameter which can affect the estimation accuracy. $\alpha$ and $\kappa$ are the parameters to control the spread of the sigma points. $\beta$ is determined by the state vector distribution.

2. The predicted state

According to (23), the function group $s(·)$ contains state vectors at the instant $t_{k-1}^e$ and $t_k^e$ 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 ${\mathit{\xi }}_i(t_k^e|t_{k-1}^e)$$({\mathit{\xi }}_i$ represent the $(i+1)th$ column of the matrix $\mathit{\xi }.i=0,1,2,\cdots ,2n)$, the iterative process is given by

$$\begin{array}{cc}\hfill {[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^l& ={[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^{l-1}-{({\mathbf{J}}_s^T{\mathbf{J}}_s)}^{-1}{\mathbf{J}}_s^{T}s\left({[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^{l-1},{\mathit{\xi }}_i(t_{k-1}^e)\right),\hfill \end{array}$$

(47)

where l is the iteration index, ${\mathbf{J}}_s$ is the Jacobian matrix given by

$$\begin{array}{cc}\hfill {\mathbf{J}}_s& =\frac{\partial s\left({[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^{l-1},{\mathit{\xi }}^i(t_{k-1}^e)\right)}{\partial {[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^{l-1}}\hfill \\ \hfill & =\left[\begin{array}{cccccccc}1& 0& 0& 0& 0& 0& 0& -{\dot{x}}^T(t_{k-1}^e)\\ 0& 1& 0& 0& 0& 0& 0& -{\dot{\mathrm{y}}}^T(t_{k-1}^e)\\ 0& 0& 1& 0& 0& 0& 0& -{\dot{z}}^T(t_{k-1}^e)\\ 0& 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 0& 1& 0& 0& 0\\ 0& 0& 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 0& 0& 1& 0\\ \frac{x^T-x^S}{dc}& \frac{y^T-y^S}{dc}& \frac{z^T-z^S}{dc}& 0& 0& 0& 0& 1\end{array}\right].\hfill \end{array}$$

(48)

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

$$SS{R}^l={\displaystyle \sum _{i=0}^n}{\left[0-s\left({[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e)]}^{l-1},{\mathit{\xi }}_i(t_{k-1}^e)\right)\right]}^2,$$

(49)

For the given iteration convergence threshold parameter a, the accuracy criterion for iteration is denoted as

$$\left|\frac{SS{R}^l-SS{R}^{l-1}}{SS{R}^l}\right|<a.$$

(50)

The the initial value for iteration process can be calculated according to [36], or it can be approximately replaced by

$$[{\mathit{\xi }}_i(t_k^e|t_{k-1}^e{)]}^0={\widehat{\mathit{\xi }}}_i(t_k^e|t_{k-1}^e),$$

(51)

The convergence criteria of the Gauss–Newton method is given in Appendix A.

The prediction state and corresponding error covariance are given by

$$\begin{array}{c}\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)={\displaystyle \sum _{i=0}^{2n}}{w}_i^m{\widehat{\mathit{\xi }}}_i(t_k^e|t_{k-1}^e)\end{array}$$

(52)

$${\tilde{\mathbf{x}}}_i^{\xi }(t_k^e|t_{k-1}^e)={\widehat{\mathit{\xi }}}_i(t_k^e|t_{k-1}^e)-\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)$$

(53)

$$\mathbf{P}(t_k^e|t_{k-1}^e)\approx {\displaystyle \sum _{i=0}^{2n}}{w}_i^c{\tilde{\mathbf{x}}}_i^{\xi }(t_k^e|t_{k-1}^e){({\tilde{\mathbf{x}}}_i^{\xi }(t_k^e|t_{k-1}^e))}^{\prime }+\mathbf{Q}(t_k^e|t_{k-1}^e).$$

(54)

where $\mathbf{Q}(t_k^e|t_{k-1}^e)$ is correlates with the state, but this dependence is neglectable during the calculation process.

3. Measurement update

For the prediction state $\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)$ and covariance $\mathbf{P}(t_k^e|t_{k-1}^e)$ in step 2, re-propagate the sigma points according to

$$\mathbf{\zeta }(t_k^e|t_{k-1}^e)=[\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)+\sqrt{(n+\lambda )\mathbf{P}(t_k^e|t_{k-1}^e)}\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)-\sqrt{(n+\lambda )\mathbf{P}(t_k^e|t_{k-1}^e)}],$$

(55)

The one-step measurement prediction is propagated by the following measurement equations

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{z}}}_i(t_k^S|t_{k-1}^S)=h({\mathbf{\zeta }}_i),i=0,1,\cdots ,2n\hfill \end{array}$$

(56)

$$\begin{array}{cc}\hfill & \widehat{\mathbf{z}}(t_k^S|t_{k-1}^S)={\displaystyle \sum _{i=0}^{2n}}{w}_i^m{\widehat{\mathbf{z}}}_i(t_k^S|t_{k-1}^S).\hfill \end{array}$$

(57)

where ${\mathbf{\zeta }}_i$ represent the $(i+1)th$ column of the matrix $\mathbf{\zeta }$.

The state and covariance for the first stage of GH-IUKF are given by

$$\begin{array}{cc}\hfill & {\tilde{\mathbf{x}}}_i^{\zeta }(t_k^e|t_{k-1}^e)={\widehat{\mathbf{\zeta }}}_i(t_k^e|t_{k-1}^e)-\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)\hfill \end{array}$$

(58)

$$\begin{array}{cc}\hfill & {\tilde{\mathbf{z}}}_i(t_k^S|t_{k-1}^S)={\widehat{\mathbf{z}}}_i(t_k^S|t_{k-1}^S)-\widehat{\mathbf{z}}(t_k^S|t_{k-1}^S)\hfill \end{array}$$

(59)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{\tilde{x}\tilde{z}}={\displaystyle \sum _{i=0}^{2n}}{w}_i^c{\tilde{\mathbf{x}}}_i^{\zeta }(t_k^e|t_{k-1}^e){({\tilde{\mathbf{z}}}_i(t_k^e|t_{k-1}^e))}^{\prime }\hfill \end{array}$$

(60)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{\tilde{z}}={\displaystyle \sum _{i=0}^{2n}}{w}_i^c{\tilde{\mathbf{z}}}_i(t_k^S|t_{k-1}^S){({\tilde{\mathbf{z}}}_i(t_k^S|t_{k-1}^S))}^{\prime }+R(t_k^S)\hfill \end{array}$$

(61)

$$\begin{array}{cc}\hfill & {\mathbf{K}}_k={\mathbf{P}}_{\tilde{x}\tilde{z}}{\mathbf{P}}_{\tilde{z}}^{-1}\hfill \end{array}$$

(62)

$$\begin{array}{cc}\hfill & \widehat{\mathbf{x}}(t_k^e)=\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)+{\mathbf{K}}_k(\mathbf{z}(t_k^S)-\widehat{\mathbf{z}}(t_k^S|t_{k-1}^S))\hfill \end{array}$$

(63)

$$\begin{array}{cc}\hfill & \mathbf{P}(t_k^e)=\mathbf{P}(t_k^e|t_{k-1}^e)-{\mathbf{K}}_k{\mathbf{P}}_{\tilde{z}}{({\mathbf{K}}_k)}^{\prime }.\hfill \end{array}$$

(64)

where $R(t_k^S)$ denotes the variance of measurement noise at the sampling instant $t_k^S$.

4. 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.

(i)

Set the initial value $[\widehat{\mathbf{x}}(t_k^e){]}^0=\widehat{\mathbf{x}}(t_k^e|t_{k-1}^e)$, ${[\mathbf{P}(t_k^e)]}^0=\mathbf{P}(t_k^e|t_{k-1}^e)$ and $[\widehat{\mathbf{x}}(t_k^e){]}^1=\widehat{\mathbf{x}}(t_k^e)$,

${[\mathbf{P}(t_k^e)]}^1=\mathbf{P}(t_k^e)$. Let $g=1,j=2$ (j represents the jth iterate)

(ii)

Generate a new sigma point for iterated state

$${[\mathit{\xi }(t_k^e)]}^j=\left[{[\widehat{\mathbf{x}}(t_k^e)]}^{j-1}{[\widehat{\mathbf{x}}(t_k^e)]}^{j-1}+\sqrt{(n+\lambda ){[\mathbf{P}(t_k^e)]}^{j-1}}{[\widehat{\mathbf{x}}(t_k^e)]}^{j-1}-\sqrt{(n+\lambda ){[\mathbf{P}(t_k^e)]}^{j-1}}\right],$$

(65)

(iii)

Repeat the time update and measurement update procedure (47)–(64) as follows

$$\begin{array}{cc}\hfill {[{\mathit{\xi }}_i^{-}(t_k^e)]}^{j,l}& =\left[{[{\mathit{\xi }}_i^{-}(t_k^e)]}^{j,l-1}-{({\mathbf{J}}_s^T{\mathbf{J}}_s)}^{-1}{\mathbf{J}}_s^{T}s\left({[{\mathit{\xi }}_i^{-}(t_k^e)]}^{j,l-1},{[{\mathit{\xi }}_i(t_k^e)]}^j\right)\right]\hfill \end{array}$$

(66)

$$\begin{array}{cc}\hfill & [{\widehat{\mathbf{x}}}^{-}(t_k^e){]}^j={\displaystyle \sum _{i=0}^{2n}}{w}_i^m[{\widehat{\mathit{\xi }}}_i^{-}(t_k^e){]}^j\hfill \end{array}$$

(67)

$$\begin{array}{cc}\hfill & {[{\widehat{\mathbf{z}}}_i^{-}(t_k^S)]}^j=h({[{\mathit{\xi }}_i(t_k^e)]}^j)\hfill \end{array}$$

(68)

$$\begin{array}{cc}\hfill & {[{\widehat{\mathbf{z}}}^{-}(t_k^S)]}^j={\displaystyle \sum _{i=0}^{2n}}{w}_i^m{[{\widehat{\mathbf{z}}}_i(t_k^S)]}^j\hfill \end{array}$$

(69)

$$\begin{array}{cc}\hfill & {[{\tilde{\mathbf{x}}}_i^{\zeta }(t_k^e)]}^j=[{\widehat{\mathbf{\zeta }}}_i^{-}(t_k^e){{]}^j-[{\widehat{\mathbf{x}}}^{-}(t_k^e)]}^j\hfill \end{array}$$

(70)

$$\begin{array}{cc}\hfill & {[{\tilde{\mathbf{z}}}_i(t_k^S)]}^j=[{\widehat{\mathbf{z}}}_i^{-}(t_k^S){{]}^j-[{\widehat{\mathbf{z}}}^{-}(t_k^S)]}^j\hfill \end{array}$$

(71)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{\tilde{x}\tilde{z}}^j={\displaystyle \sum _{i=0}^{2n}}{w}_i^c{[{\tilde{\mathbf{x}}}_i^{\zeta }(t_k^e)]}^j{({[{\tilde{\mathbf{z}}}_i(t_k^e)]}^j)}^{\prime }\hfill \end{array}$$

(72)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{\tilde{z}}^j={\displaystyle \sum _{i=0}^{2n}}{w}_i^c{[{\tilde{\mathbf{z}}}_i(t_k^S)]}^j{({[{\tilde{\mathbf{z}}}_i(t_k^S)]}^j)}^{\prime }+R(t_k^S)\hfill \end{array}$$

(73)

$$\begin{array}{cc}\hfill & {\mathbf{K}}_k^j={\mathbf{P}}_{\tilde{x}\tilde{z}}^j{({\mathbf{P}}_{\tilde{z}}^j)}^{-1}\hfill \end{array}$$

(74)

$$\begin{array}{cc}\hfill & [\widehat{\mathbf{x}}(t_k^e){]}^j={[{\widehat{\mathbf{x}}}^{-}(t_k^e)]}^j+{\mathbf{K}}_k^j\left(\mathbf{z}(t_k^S)-{[{\widehat{\mathbf{z}}}^{-}(t_k^S)]}^j\right)\hfill \end{array}$$

(75)

$$\begin{array}{cc}\hfill & {[\mathbf{P}(t_k^e)]}^j={[\mathbf{P}(t_k^e)]}^{j-1}-{\mathbf{K}}_k^j{\mathbf{P}}_{\tilde{z}}^j{({\mathbf{K}}_k^j)}^{\prime }.\hfill \end{array}$$

(76)

(iv)

Define the judgment criteria equations

$$\begin{array}{cc}\hfill & {[\widehat{\mathbf{z}}(t_k^S)]}^j=h{([\widehat{\mathbf{x}}(t_k^e)]}^j)\hfill \end{array}$$

(77)

$$\begin{array}{cc}\hfill & {[\tilde{\mathbf{x}}(t_k^e)]}^j=[\widehat{\mathbf{x}}(t_k^e){{]}^j-[\widehat{\mathbf{x}}(t_k^e)]}^{j-1}\hfill \end{array}$$

(78)

$$\begin{array}{cc}\hfill & {[\tilde{\mathbf{z}}(t_k^S)]}^j=\mathbf{z}(t_k^S)-{[\widehat{\mathbf{z}}(t_k^S)]}^j.\hfill \end{array}$$

(79)

(v)

The termination criteria for iterative process:

If the following inequality satisfies

$${({[\tilde{\mathbf{x}}(t_k^e)]}^j)}^{\prime }{[{\mathbf{P}}^{-1}(t_k^e)]}^{j-1}{[\tilde{\mathbf{x}}(t_k^e)]}^j+{({[\tilde{\mathbf{z}}(t_k^S)]}^j)}^{\prime }{R}^{-1}(t_k^S){[\tilde{\mathbf{z}}(t_k^S)]}^j\ge {({[\tilde{\mathbf{z}}(t_k^S)]}^{j-1})}^{\prime }{R}^{-1}(t_k^S){[\tilde{\mathbf{z}}(t_k^S)]}^{j-1}.$$

(80)

or if j reaches the $N_{max}$, stop the iterative and output $\widehat{\mathbf{x}}(t_k^e{)=[\widehat{\mathbf{x}}(t_k^e)]}^j,\mathbf{P}(t_k^e)={[\mathbf{P}(t_k^e)]}^j$. Otherwise, let $g=\eta ·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.

Algorithm 1 The GH-IUKF algorithm.

First stage: (1) Calculate $2n+1$ sigma points ${\mathit{\xi }}^i(t_{k-1}^e)$ for given $\left(\mathbf{x}(t_{k-1}^e),\mathbf{P}(t_{k-1}^e)\right)$ by (43). (2) Calculate the Jacobian matrix ${\mathbf{J}}_s$ by Equation (48) and predicted sigma point ${\mathit{\xi }}^i(t_k^e|t_{k-1}^e)$ by the implicit stae transmission function $s(·)$. l = 1, I = 1; while I = 1 do  ${[{\mathit{\xi }}^i(t_k^e|t_{k-1}^e)]}^l={[{\mathit{\xi }}^i(t_k^e|t_{k-1}^e)]}^{l-1}-{({\mathbf{J}}_s^T{\mathbf{J}}_s)}^{-1}{\mathbf{J}}_s^{T}s\left[{[{\mathit{\xi }}^i(t_k^e|t_{k-1}^e)]}^{l-1},{\mathit{\xi }}^i(t_{k-1}^e)\right],l=l+1$  if$\left|\frac{SS{R}^l-SS{R}^{l-1}}{SS{R}^l}\right|<\sigma$ or $l>L_{max}$ then   ${\mathit{\xi }}^i(t_k^e\left|t_{k-1}^e\right.)={[{\mathit{\xi }}^i(t_k^e\left|t_{k-1}^e\right.)]}^l$, $I=0$  end if end while (3) Update the first stage state $\widehat{\mathbf{x}}(t_k^e)$ and error covariance $\mathbf{P}(t_k^e)$ by Equations (52) to (64). Iterated stage: (1) Let the initial value $[\widehat{\mathbf{x}}(t_k^e){]}^1=\widehat{\mathbf{x}}(t_k^e)$, ${[\mathbf{P}(t_k^e)]}^1=\mathbf{P}(t_k^e)$. (2) Iterate the state and error covariance until they are satisfied with the termination criteria j = 2, g = 1; for j:$N_{max}$ do  Execute the procedure (65)–(76)  if ${({[\tilde{\mathbf{x}}(t_k^e)]}^j)}^{\prime }{[{\mathbf{P}}^{-1}(t_k^e)]}^{j-1}{[\tilde{\mathbf{x}}(t_k^e)]}^j+{({[\tilde{\mathbf{z}}(t_k^S)]}^j)}^{\prime }{R}^{-1}(t_k^S){[\tilde{\mathbf{z}}(t_k^S)]}^j\ge {({[\tilde{\mathbf{z}}(t_k^S)]}^{j-1})}^{\prime }{R}^{-1}(t_k^S){[\tilde{\mathbf{z}}(t_k^S)]}^{j-1}$ then   Break  else   $g=\eta g,j=j+1$  end if end for $\widehat{\mathbf{x}}(t_k^e{)=[\widehat{\mathbf{x}}(t_k^e)]}^j,\mathbf{P}(t_k^e)={[\mathbf{P}(t_k^e)]}^j$

5. 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_0^S,y_0^S,z_0^S)$ is $(-500,0,0)$ $\mathrm{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 ${\dot{x}}^S(t_k^S)$ = 12 m/s. Then the relationships of observer motion parameters are defined as the following equations

$$x^S=x_0^S+{\dot{x}}^S{t}_k^S$$

(81)

$$y^S=y_0^S+200sin((-2\pi /1000)x^S)$$

(82)

$${\dot{y}}^S=-\frac{2\pi }{5}cos((-2\pi /1000)x^S){\dot{x}}^S$$

(83)

$$z^S=z_0^S,{\dot{z}}^S=0.$$

(84)

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_0^e$ is calculated according to (19). Both of the bearing angle and elevation angle measurement noise standard deviation are ${\sigma }_{\theta }^v={\sigma }_{\phi }^v$ = 3°, Doppler frequency measurement error standard deviation is ${\sigma }_f^v$ = 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

$$\mathbf{q}=diag[{(0.1\mathrm{m}/{\mathrm{s}}^2)}^2{(0.1\mathrm{m}/{\mathrm{s}}^2)}^2{(0.1\mathrm{m}/{\mathrm{s}}^2)}^2{(0.01\mathrm{Hz})}^2{(0.01\mathrm{s})}^2],$$

(85)

The initial state ${\mathbf{x}}_0^{*}$ is randomly generated by the true value and the initial error covariance according to [30]

$$\mathbf{P}(t_0^e)=diag[{(100\mathrm{m})}^2{(100\mathrm{m})}^2{(100\mathrm{m})}^2{(3\mathrm{m}/\mathrm{s})}^2{(3\mathrm{m}/\mathrm{s})}^2{(3\mathrm{m}/\mathrm{s})}^2{(1\mathrm{Hz})}^2{(0.1\mathrm{s})}^2].$$

(86)

The position root mean square error (RMSE) is given by

$$RMSE({\widehat{t}}_k^e)=\sqrt{\frac{1}{N}{\displaystyle \sum _{i=1}^N}\left[{\left(\widehat{x}({\widehat{t}}_k^e)-x({\widehat{t}}_k^e)\right)}^2+{\left(\widehat{y}({\widehat{t}}_k^e)-y({\widehat{t}}_k^e)\right)}^2+{\left(\widehat{z}({\widehat{t}}_k^e)-z({\widehat{t}}_k^e)\right)}^2\right]}.$$

(87)

where N is the numbers of Monte-Carlo simulation, $\widehat{x}({\widehat{t}}_k^e)$, $\widehat{y}({\widehat{t}}_k^e)$ and $\widehat{z}({\widehat{t}}_k^e)$ are the estimation of position, $x({\widehat{t}}_k^e)$, $y({\widehat{t}}_k^e)$ and $z({\widehat{t}}_k^e)$ 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]

$$\epsilon ({\widehat{t}}_k^e)=\frac{1}{nN}{\displaystyle \sum _{i=1}^N}({\mathbf{x}}_i({\widehat{t}}_k^e)-{\widehat{\mathbf{x}}}_i({\widehat{t}}_k^e){)}^T{[\mathit{P}({\widehat{t}}_k^e)]}^{-1}({\mathbf{x}}_i({\widehat{t}}_k^e)-{\widehat{\mathbf{x}}}_i({\widehat{t}}_k^e))$$

(88)

where n is the dimension of the state vector, $({\mathbf{x}}_i({\widehat{t}}_k^e)-{\widehat{\mathbf{x}}}_i({\widehat{t}}_k^e))$ and $\mathit{P}({\widehat{t}}_k^e)$ are the filter estimation error and covariance for the ith Monte-Carlo simulation at the instant ${\widehat{t}}_k^e$. The acceptance interval threshold ${\sigma }_1$ and ${\sigma }_2$ at level $\alpha =0.05$ are calculated by [40]

$${\sigma }_{1,2}\approx \frac{1}{2nN}{\left(\pm 1.96+\sqrt{2nN-1}\right)}^2$$

(89)

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_k^e$ 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_k^e$ as a variable into the state.
• IUKF-D: This method only takes $t_k^d$ 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_k^d$ 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.

Figure 6 depicts the result of ANEES for the proposed method in this paper. The acceptance threshold ${\sigma }_1$ = 1.0360 and ${\sigma }_2$ = 0.9645. The ANEES in Figure 6 is close to 1, and the results for the majority of the time are within the confidence interval, which means the estimation error and the covariance are compatible with each other, and the estimation of the filter is reliable and credible.

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. Performance comparison.

	GH-IUKF	GH-UKF	IUKF-D	UKF-D
Time per Run (ms)	134	83	50	24
1° Average RMSE (m)	77.04	78.96	79.71	82.19
2° Average RMSE (m)	97.15	100.38	101.43	105.61
3° Average RMSE (m)	111.50	117.22	118.09	120.30
5° Average RMSE (m)	133.15	139.96	140.44	151.75

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.

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.

6. 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.