Accurate Closed-Form Solution for Moving Underwater Vehicle Localization Using Two-Way Travel Time

Abstract

Position information is essential for an underwater vehicle that can affect the performance of many other applications. Vehicle motion during the two-way travel time (TWTT) using an acoustic positioning system can affect the localization accuracy of the estimators based on the traditional static model. A new time measurement model for moving vehicle localization is presented, which compensates for the vehicle motion. The Cramér–Rao lower bounds (CRLBs) of the proposed model are derived for two different cases, where the depth of the underwater vehicle is unknown or known. Then, closed-form solutions for the two cases using the proposed model are derived and the solutions are shown analytically to reach the CRLBs. Simulations collaborate the theoretical performance of the proposed estimators and the moving model significantly improves the localization accuracy in comparison with the static model.

1. Introduction

Accurate localization and navigation of autonomous underwater vehicle (AUV) is a critical focus area to a number of applications, such as underwater search and rescue, seafloor mapping, shallow water mine countermeasures, and oceanographic surveys [1,2,3,4,5,6,7,8,9,10]. Accurate localization is vital to ensure the accuracy of navigation and other applications.

The techniques of AUV localization usually can be classified to two categories, i.e., inertial/dead reckoning system and the acoustic transponders and modem system. The inertial/dead reckoning system uses inertial measurement unit (IMU) or Doppler Velocity Logs (DVL) to estimate the vehicle position [11]. However, all of the methods in this category have position error growth that is unbounded. Since they have accumulated error, their localization error will continue to increase if it is not corrected by other techniques for a long time. Therefore, location resets are necessary during some long-duration mission scenarios and these position resets of AUV are typically obtained by Global Positioning System (GPS) updates [12]. However, due to the strong attenuation of electromagnetic waves by water, GPS signal is not practically feasible except for distances of a few meters. AUV has to transit to the surface to access GPS satellites. This operation is always time consuming especially for deep-ocean scenarios. Another technique is the use of the acoustic transponders and modem system such as long baseline (LBL) system [1] and moving LBL system [13]. In this system, AUV is designed to measure the time of flight (TOF) of signals from acoustic beacons or transponders at known positions to perform localization. These methods based on the acoustic positioning system can provide bounded, long-term localization precision.

Many methods have been proposed in this area to find the position of an AUV using the acoustic positioning system [14,15,16,17,18,19,20,21,22]. The simultaneous localization and mapping (SLAM) methods proposed in [15,16] simultaneously estimate the position of an AUV and the positions of stationary beacons using range-only data. In their experiments, an AUV would send out an interrogation signal, and the beacons would reply in a predefined sequence. The two-way travel time (TWTT) of the acoustic signals is usually used to determine the ranges. When enough measurements and samples are obtained during the trajectory of the moving AUV, their proposed navigation filters will estimate all the unknown positions. However, they assume the value of AUV speed is small and the vehicle motion during interrogation and reception is ignored to simplify their algorithm. Actually, the travel times from AUV to beacons and back are not the same. Some different AUV localization systems using one-way travel time (OWTT) were proposed in [17,18,19] to avoid the problem of vehicle motion in TWTT. Refenence [17] used a ship (precisely localized via GPS) and transponders to localize the AUV. In this system, the AUV only sends out an interrogation signal and the ship receives all the signals: the direct propagation from AUV and the reflections by all the transponders. The time difference of arrivals between the reflection paths and the direct path along with depth measurements are used to locate the AUV. In [18,19], the acoustic data packets containing the state information are broadcast from a single, moving reference beacon and can be received by multiple AUVs within an acoustic range. The synchronous clocks enable range measurements based on OWTT measurements between the beacon and the AUVs. Every AUV can estimate its position when enough range measurements are obtained from the moving beacon.

The aforementioned localization methods mainly focus on Kalman filter (KF), which is a widely used recursive, self-correcting model-based estimator. The KF requires range measurements at different times, and an initial transient period is needed before it produces a good source location estimate [23]. In addition, an initial guess closed to the true value is also required to initialize the filter. Another kind of localization methods are nonlinear estimation, such as maximum likelihood estimation (MLE) [24] and the closed-form solutions [20,25,26] based on least squares (LS) or weighted LS. However, often only the measurement error is considered in causing the errors on the location estimate [27,28]. The extension of localization algorithms to account for inaccurate sensor positions [20,29,30] or spatiotemporal variation of sound speed [31,32] has also been investigated. Nevertheless, the AUV motion effect is ignored in most existing research to simplify their methods. [20] proposed a closed-form solution using the LS method for the TWTT measurements by considering the calibration error of deployed beacons, where the vehicle motion is still ignored. The vehicle motion is compensated in [21] to obtain a better solution by developing a Bayesian inference algorithm. [33] determines the vehicle position using TWTT together with the information of the angle measurements and orientation (pitch angle and yaw angle) of AUV. Feng [25] accounted for the AUV motion in the method and proposed an approximate solution by solving the circular equations constructed from the nonlinear equations of the time measurements. Although [21,25,33] improve the localization performance by including the AUV motion effect in their methods, their works lack analysis of the optimality property.

In AUV localization, the depth information can be normally measured by the pressure sensor mounted on AUV [5]. Only two position coordinate values need to be estimated by the localization system in a 3D scenario if the depth measurement is accurately obtained. However, when the pressure sensor is broken or there is a large error with the depth measurement, the known depth localization will be invalid or inaccurate. Therefore, the unknown depth localization shall be a complement of the known depth localization, and will also increase the robustness of the localization system. Thus, we will focus on these two localization cases.

This paper will propose a time measurement moving model for range-only AUV localization which compensates for vehicle motion during the TWTT interval. The Cramér–Rao lower bound (CRLB) is derived for the proposed moving model in two different cases: unknown depth localization and known depth localization. We also prove that the AUV localization accuracy can be improved by measuring the AUV depth. Then, an accurate closed-form solution is derived for the proposed model in these two cases. The closed-form solution does not require initial solution guess, and so it is computationally attractive. Finally, results from the Monte Carlo (MC) simulation show that applying the proposed moving model significantly improves the localization accuracy in comparison with the static model. More importantly, the proposed closed-form solution can reach the accuracy set by the CRLB. The main contributions of this work are highlighted as follows:

• a new time delay measurement model that accounts for the AUV motion during the TWTT is proposed in the paper. it is unbiased and accurate for the moving AUV localization.
• the CRLBs are derived using the proposed model for the two cases in range-only AUV localization. it is proved that the AUV localization accuracy can be improved by measuring the AUV depth.
• a set of closed-form solutions are constructed using time delay measurement for the two cases in range-only AUV localization. The proposed solutions are computationally efficient and can achieve the CRLB analytically. Simulations validate the performance of the proposed solutions.

The paper is organized as follows: Section 2 provides a new time measurement moving model as well as the traditional static model for a moving AUV localization scenario. Section 3 derives the CRLB for the proposed moving model in two different cases and compares the CRLBs theoretically. Section 4 presents the proposed closed-form solution and compares analytically the proposed estimator with the CRLB. Section 5 shows the simulation results, and Section 6 provides the conclusions.

The following notation will be adopted throughout the paper. Bold face uppercase letters represent matrices and lowercase bold letters represent vectors. ${(·)}^o$ is the true value and $(·)$ is its noisy version with additive noise $\Delta (·)$. The symbols $\mathbf{I}$ and $\mathbf{O}$ represent the identity matrix and zero matrix with appropriate dimension. For a given matrix $\mathbf{A}$, ${\lambda }_p\left(\mathbf{A}\right)$, $\mathrm{tr}\left(\mathbf{A}\right)$ and $\mathrm{rank}\left(\mathbf{A}\right)$ denote the positive eigenvalues, the trace, and the rank of $\mathbf{A}$. For a given vector $\mathbf{u}$, ${\parallel \mathbf{u}\parallel }_2$ or $\parallel \mathbf{u}\parallel$ is the $l_2$ norm. $E[·]$ is the mathematical expectation.

2. Models for AUV Localization

2.1. Moving Model

We first consider a moving AUV localization system with N sensors (transponders and/or beacons) fixed at ${\mathbf{s}}_i={[x_i,y_i,z_i]}^T,i=1,2,\cdots ,N$, as shown in Figure 1. The moving AUV with known velocity $\mathbf{v}={[v_x,v_y,v_z]}^T$ sends out an interrogation signal at position ${\mathbf{u}}^o={[u_x^o,u_y^o,u_z^o]}^T$ and will receive all the replied signals from the sensors. We suppose the signals received by AUV do not interfere with each other, and the processing delay of every sensor is assumed to be zero. The true signal travel distance from the moving AUV to the ith sensor and finally back to the moving AUV can be obtained as

$$\begin{array}{ccc}\hfill r_i^o& =& \parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel +\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel \hfill \\ & =& \parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel +\parallel {\mathbf{u}}^o+\frac{r_i^o}{c}\mathbf{v}-{\mathbf{s}}_i\parallel \hfill \end{array}$$

(1)

where c is acoustic signal propagation speed. The model describes the relationship between the true signal travel distance and the true AUV position value. However, when the time delay is estimated from the received signals, there will be noise in the time delay measurement [34]. Therefore, the two-way travel time (TWTT) measurement is modeled as

$${\tau }_i={\tau }_i^o+\Delta {\tau }_i=\frac{r_i^o}{c}+\Delta {\tau }_i$$

(2)

where $\Delta {\tau }_i$ is the time measurement noise that is assumed to be zero-mean Gaussian. Finally, the moving measurement model in vector form is expressed as

$$\mathit{\tau }={\mathit{\tau }}^{\mathbf{o}}+\mathbf{n}$$

(3)

where ${\mathit{\tau }}^o={[{\tau }_1^o,{\tau }_2^o,\cdots ,{\tau }_N^o]}^T$ and $\mathbf{n}={[\Delta {\tau }_1,\Delta {\tau }_2,\cdots ,\Delta {\tau }_N]}^T$. The noise vector is modeled as a zero-mean Gaussian with covariance matrix $\mathbf{Q}=E\left[\mathbf{n}{\mathbf{n}}^T\right]$.

This paper is to find the AUV position ${\mathbf{u}}^o$ from the TWTT measurement $\mathit{\tau }$, the known sensor positions ${\mathbf{s}}_i$, the signal propagation speed c, and the noise covariance matrix $\mathbf{Q}$. According to whether AUV depth is known or not, we will focus on two different cases: AUV localization with unknown depth and AUV localization with known depth.

2.2. AUV Motion Effect and Static Model

The original new model in Equation (1) is recursive and we would like to gain some insights about the effect of AUV motion after transformation of the recursive model. In Equation (1), after moving $\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel$ to the left, squaring both sides and expanding the Euclidean norms give

$$r_i^{o2}-2r_i^o\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel +\parallel {\mathbf{u}}^o-{\mathbf{s}}_i{\parallel }^2={\parallel {\mathbf{u}}^o+\frac{r_i^o}{c}\mathbf{v}-{\mathbf{s}}_i\parallel }^2.$$

(4)

where $\parallel {\mathbf{u}}^o+\frac{r_i^o}{c}\mathbf{v}-{\mathbf{s}}_i{\parallel }^2=\parallel {\mathbf{u}}^o-{\mathbf{s}}_i{\parallel }^2+2r_i^o{({\mathbf{u}}^o-{\mathbf{s}}_i)}^T\mathbf{v}/c+r_i^{o2}{\parallel \mathbf{v}\parallel }^2/c^2$. Substituting it in (4) gives

$$r_i^{o2}-2r_i^o\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel =2r_i^o{({\mathbf{u}}^o-{\mathbf{s}}_i)}^T\mathbf{v}/c+r_i^{o2}{\parallel \mathbf{v}\parallel }^2/c^2.$$

(5)

Because $r_i^o$ is positive and cannot be zero, cancelling $r_i^o$ on both sides and solving for $r_i^o$ yield

$$r_i^o=\frac{2}{1-{\parallel \mathbf{v}\parallel }^2/c^2}\left(\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel +{({\mathbf{u}}^o-{\mathbf{s}}_i)}^T\frac{\mathbf{v}}{c}\right).$$

(6)

Defining ${\theta }_i$ as the angle between ${\mathbf{u}}^o-{\mathbf{s}}_i$ and $\mathbf{v}$, we have

$${({\mathbf{u}}^o-{\mathbf{s}}_i)}^T\mathbf{v}=\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel \parallel \mathbf{v}\parallel cos\left({\theta }_i\right).$$

(7)

Putting (7) in (6) and rearranging gives an expression related to ${\theta }_i$

$$r_i^o=2\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel \frac{1}{1-{\parallel \mathbf{v}\parallel }^2/c^2}\left(1+cos\left({\theta }_i\right)\frac{\parallel \mathbf{v}\parallel }{c}\right).$$

(8)

We can see from (8) that, if the AUV motion effect during the interrogation is ignored and $\parallel \mathbf{v}\parallel /c$ can be set to zero, $r_i^o$ will be approximated by

$$r_i^s=2\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel ,$$

(9)

which is also known as the two-way time of arrival (TW-TOA) measurement [35,36,37]. It is simple and accurate for static AUV localization. Actually, the moving model (1) is a natural generalization of the static model (9) for moving AUV localization, which compensates the AUV motion effect. When the static model is used for moving AUV localization, the error in the two-way distance is

$$\begin{array}{cc}\hfill {\delta }_{ri}^o=r_i^o-r_i^s& =2\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel \frac{1}{1-{\parallel \mathbf{v}\parallel }^2/c^2}\left(cos\left({\theta }_i\right)\frac{\parallel \mathbf{v}\parallel }{c}+\frac{{\parallel \mathbf{v}\parallel }^2}{c^2}\right)\hfill \\ \hfill & =r_i^s\frac{1}{1-{\parallel \mathbf{v}\parallel }^2/c^2}\frac{\parallel \mathbf{v}\parallel }{c}\left(cos\left({\theta }_i\right)+\frac{\parallel \mathbf{v}\parallel }{c}\right).\hfill \end{array}$$

(10)

We can see that the error ${\delta }_{ri}^o$ caused by the AUV motion is nonlinear with respect to the speed normalized by the propagation speed, with leading order one. It has a sinusoidal relationship with the angle between the AUV-movement and AUV-sensor directions. Figure 2 gives an example of the model error related to $\parallel \mathbf{v}\parallel$ and ${\theta }_i$ where ${\mathbf{u}}^o={[50,50,0]}^T$ m, ${\mathbf{s}}_i={[500,500,0]}^T$ m and $c=1456$ m/s. The model error of the two-way distance becomes significant when $\parallel \mathbf{v}\parallel$ increases.

Therefore, for moving AUV localization, the static model ignores the AUV motion effect and the position estimate based on (9) shall be inaccurate. In other words, the static model will introduce estimation error in localization.

We illustrate this aspect visually in Figure 3 where the blue square symbol represents the true position of the moving AUV in the instant of interrogation. The use of static model to locate a moving AUV will lead to a biased estimate, and the optimal position of the likelihood function is away from the true value. In contrast, the true position value is on the peak of the likelihood surface using moving model. Thus, the corresponding ML estimator is unbiased. We would like to show the loss of localization performance when using the static model compared with the moving model.

3. CRLB

This section will evaluate the CRLB for moving AUV localization problem. The CRLB is the lowest bound of the covariance matrix of an unbiased estimate. Roughly speaking, the CRLB defines the best estimation accuracy that an unbiased estimator can achieve in terms of mean square error. The CRLB is always used as a benchmark for comparison of the localization methods although some of them may not be unbiased. Because the AUV may have the information of its depth, the CRLB focuses on the two cases: AUV localization with unknown depth and AUV localization with known depth.

3.1. AUV Localization with Unknown Depth

Assume that the time measurement vector $\mathit{\tau }$ satisfies the measurement model in (3) and define the CRLB matrix of an unbiased estimate as ${\mathbf{C}}_{{\mathbf{u}}^o}$. Based on the equations in [23,38], we have

$${\mathbf{C}}_{{\mathbf{u}}^o}={\mathbf{F}}_{{\mathbf{u}}^o}^{-1}$$

(11)

where ${\mathbf{F}}_{{\mathbf{u}}^o}$ is the Fisher information matrix and it is

$${\mathbf{F}}_{{\mathbf{u}}^o}={\mathbf{J}}_{{\mathbf{u}}^o}{\mathbf{Q}}^{-1}{\mathbf{J}}_{{\mathbf{u}}^o}^T.$$

(12)

${\mathbf{J}}_{{\mathbf{u}}^o}$ is concretely expressed as

$${\mathbf{J}}_{{\mathbf{u}}^o}=\frac{\partial {\mathit{\tau }}^{oT}}{\partial {\mathbf{u}}^o}=\left[\frac{\partial {\tau }_1^o}{\partial {\mathbf{u}}^o},\frac{\partial {\tau }_2^o}{\partial {\mathbf{u}}^o},\cdots ,\frac{\partial {\tau }_N^o}{\partial {\mathbf{u}}^o}\right]$$

(13)

where $\partial {\tau }_i^o/\partial {\mathbf{u}}^o$ is the partial derivative of ${\tau }_i^o$ with respect to the unknown vector ${\mathbf{u}}^o$. This term is given by

$$\frac{\partial {\tau }_i^o}{\partial {\mathbf{u}}^o}=\frac{1}{c}\left[\frac{{\mathbf{u}}^o-{\mathbf{s}}_i}{\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel }+\frac{1}{\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel }\frac{\partial {\mathbf{u}}_i^{oT}}{\partial {\mathbf{u}}^o}({\mathbf{u}}_i^o-{\mathbf{s}}_i)\right],$$

(14)

where

$$\frac{\partial {\mathbf{u}}_i^{oT}}{\partial {\mathbf{u}}^o}=\mathbf{I}+\frac{\partial {\tau }_i^o}{\partial {\mathbf{u}}^o}{\mathbf{v}}^T.$$

(15)

Finally, $\partial {\tau }_i^o/\partial {\mathbf{u}}^o$ is obtained by substituting (15) into (14) and solving (14)

$$\frac{\partial {\tau }_i^o}{\partial {\mathbf{u}}^o}=\frac{1}{c-{\mathbf{\rho }}_{{\mathbf{u}}_i^o-{\mathbf{s}}_i}^T\mathbf{v}}({\mathbf{\rho }}_{{\mathbf{u}}^o-{\mathbf{s}}_i}+{\mathbf{\rho }}_{{\mathbf{u}}_i^o-{\mathbf{s}}_i})$$

(16)

where ${\mathbf{\rho }}_{\mathit{a}}=\frac{\mathit{a}}{\parallel \mathit{a}\parallel }$ for a given vector $\mathit{a}$.

3.2. AUV Localization with Known Depth

If the depth of AUV is known, the z-axis coordinate value $u_z^o$ is a known parameter. Supposing that ${\mathbf{\mu }}^o={[u_x^o,u_y^o]}^T$, the CRLB of ${\mathbf{\mu }}^o$ using model (3) has the similar expression as (11) by replacing ${\mathbf{u}}^o$ with ${\mathbf{\mu }}^o$. Hence, the related vector $\partial {\tau }_i^o/\partial {\mathbf{\mu }}^o$ that represents the partial derivative of ${\tau }_i^o$ with respect to the unknown vector ${\mathbf{\mu }}^o$ is

$$\frac{\partial {\tau }_i^o}{\partial {\mathbf{\mu }}^o}=\frac{1}{c}\left[\frac{{\mathbf{\mu }}^o-{\mathbf{\omega }}_i}{\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel }+\frac{\mathbf{I}+\frac{\partial {\tau }_i^o}{\partial {\mathbf{\mu }}^o}{\mathbf{\nu }}^T}{\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel }({\mathbf{\mu }}_i^o-{\mathbf{\omega }}_i)\right],$$

(17)

where ${\mathbf{\omega }}_i={[x_i,y_i]}^T$, $\mathbf{\nu }={[v_x,v_y]}^T$ and ${\mathbf{\mu }}_i^o={\mathbf{\mu }}^o+{\tau }_i^o\mathbf{\nu }$. We can obtain $\partial {\tau }_i^o/\partial {\mathbf{\mu }}^o$ by solving (17), which is

$$\frac{\partial {\tau }_i^o}{\partial {\mathbf{\mu }}^o}=\frac{1}{c-{\mathbf{\nu }}^T\frac{{\mathbf{\mu }}_i^o-{\mathbf{\omega }}_i}{\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel }}\left[\frac{{\mathbf{\mu }}^o-{\mathbf{\omega }}_i}{\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel }+\frac{{\mathbf{\mu }}_i^o-{\mathbf{\omega }}_i}{\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel }\right].$$

(18)

3.3. Comparisons of the CRLBs of Proposed Cases

We claim that

$$\mathrm{tr}\left({\mathbf{C}}_{{\mathbf{u}}^o}\right)>\mathrm{tr}\left({\mathbf{C}}_{{\mathbf{\mu }}^o}\right)$$

(19)

when the covariance matrix of the measurement noise in the two cases is the same. The detailed derivation is shown in Appendix A. This implies the CRLB of AUV localization with known depth is less than that of unknown depth localization. In other words, the AUV localization accuracy can be improved by measuring the AUV depth.

The following simulation compares the CRLBs of the proposed two cases using the moving AUV localization scenario in [21], with one additional sensor. The positions of sensors are shown in

Table 1. The positions of sensors.

No. i	${\mathit{x}}_{\mathit{i}}$ m	${\mathit{y}}_{\mathit{i}}$ m	${\mathit{z}}_{\mathit{i}}$ m
1	100	100	30
2	100	−100	30
3	−100	−100	30
4	−100	100	30
5	0	0	0

in Section 5. The AUV moving tracks are shown as blue lines in Figure 4a. The AUV starts at $[-150,-150,3.9]$ m, moves from left to right with $1.5$ m/s, and then turns back on a opposite direction with the same speed. The AUV performs this operation until it covers the preset region. The localization cycle period is eight seconds. In each cycle, the AUV first interrogates the sensors and sends out a signal, and the sensors would reply a predefined sequence once they receive the signal. The AUV estimates its position every cycle. The acoustic signal speed $c=1456$ m/s and the covariance matrix of time measurement noise is $\mathbf{Q}={\sigma }_t^2\mathbf{I}$. The standard deviation ${\sigma }_t=30$ $\mathsf{\mu }$s.

The CRLB establishes the theoretical lower bound for the variance of the position error. Figure 4b–c show the comparisons of CRLBs in different cases. The simulation settings in Figure 4b,c are the same except if the AUV depth is unknown. Figure 4b is the CRLB for AUV localization with unknown depth. The CRLB of the position error is between ${10}^{-3}$ and $7\times {10}^{-3}$ and the large values occur at the beginning and end of each row in the tracks. The CRLB values for known depth localization shown in Figure 4c are generally less than the CRLB values in the corresponding positions of Figure 4b. Furthermore, the large values only occur in the corners of the region. This result also validates the previous claim that the AUV localization accuracy can be improved by measuring the AUV depth. The known depth localization in Figure 4d only uses four co-plane sensors which usually become invalid in unknown depth localization. However, the CRLB is also less than the result in Figure 4b.

4. Closed-Form Solution

The closed-form solution for localization using a static model has been derived in [26,39]. This section will propose a close-form solution using the accurate model (3) for moving AUV localization. We first find an initial solution using the static model, and then we use the moving model to refine the initial estimate. The proposed solution has three stages. The first and second stages aim to provide a position estimation from the traditional static model as accurate as possible so that the refinement in the third stage can work well. the first stage introduces a nuisance variable to transform the nonlinear static measurement model to a pseudo linear equation. The second stage is to estimate the amount of correction to refine the estimate from the first stage solution. Finally, in the third stage, the moving model is to used to refine the solution by using Taylor-series expansions [23,40]. The closed-form solutions are proposed in this section for two cases: moving AUV localization with unknown depth and moving AUV localization with known depth.

4.1. AUV Localization with Unknown Depth

In this case, we shall estimate ${\mathbf{u}}^o$ from (2) using $\mathit{\tau }$, ${\mathbf{s}}_i$, and $\mathbf{v}$. The proposed closed-form solution has three stages.

4.1.1. First Stage

In this stage, a nuisance variable is introduced to transform the nonlinear static measurement model to a pseudo linear equation that can be solved directly by weighted least squares (WLS). We start with the static model in (9). Squaring on both sides yields the nonlinear TOA equation

$$c^2{\tau }_i^{s2}=4({\alpha }^o-2{\mathbf{s}}_i^T{\mathbf{u}}^o+{\mathbf{s}}_i^T{\mathbf{s}}_i)$$

(20)

where ${\tau }_i^s=r_i^s/c$ and ${\alpha }^o={\mathbf{u}}^{oT}{\mathbf{u}}^o$. Substituting ${\tau }_i^s={\tau }_i-\Delta {\tau }_i$ and rearranging yield

$$\begin{array}{c}\hfill -2c^2{\tau }_i\Delta {\tau }_i=4{\alpha }^o-8{\mathbf{s}}_i^T{\mathbf{u}}^o+4{\mathbf{s}}_i^T{\mathbf{s}}_i-c^2{\tau }_i^2\end{array}$$

(21)

where the second-order error terms have been ignored. Define ${\mathbf{\phi }}^o={[{\mathbf{u}}^{oT},{\alpha }^o]}^T$ and Equation (21) can be written in matrix form and finally we get the linear system of equations

$${\mathbf{P}}_1\mathbf{n}={\mathbf{h}}_1-{\mathbf{G}}_1{\mathbf{\phi }}^o$$

(22)

where

$${\mathbf{h}}_1=\left[\begin{array}{c}4{\mathbf{s}}_1^T{\mathbf{s}}_1-c^2{\tau }_1^2\\ 4{\mathbf{s}}_2^T{\mathbf{s}}_2-c^2{\tau }_2^2\\ ⋮\\ 4{\mathbf{s}}_N^T{\mathbf{s}}_N-c^2{\tau }_N^2\end{array}\right],{\mathbf{G}}_1=\left[\begin{array}{cc}8{\mathbf{s}}_1^T& -4\\ 8{\mathbf{s}}_2^T& -4\\ ⋮\\ 8{\mathbf{s}}_N^T& -4\end{array}\right],{\mathbf{P}}_1=-2c^2\left[\begin{array}{cccc}{\tau }_1& 0& \cdots & 0\\ 0& {\tau }_2& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ 0& 0& \cdots & {\tau }_N\end{array}\right].$$

The WLS solution to (22) is

$$\overline{\mathbf{\phi }}={[{\overline{\mathbf{u}}}^T,\overline{\alpha }]}^T={\left({\mathbf{G}}_1^T{\mathbf{W}}_1{\mathbf{G}}_1\right)}^{-1}{\mathbf{G}}_1^T{\mathbf{W}}_1{\mathbf{h}}_1,$$

(23)

where ${\mathbf{W}}_1={\left({\mathbf{P}}_1\mathbf{Q}{\mathbf{P}}_1^T\right)}^{-1}$ is the weighting matrix.

4.1.2. Second Stage

Because the first stage neglects the relevance between ${\alpha }^o$ and ${\mathbf{u}}^o$, this stage improves the accuracy of position estimate $\overline{\mathbf{u}}$ by computing the amounts of corrections. Supposing that $\overline{\mathbf{\phi }}={\mathbf{\phi }}^o+\delta \mathbf{\phi }$, $\delta \mathbf{\phi }={[\delta {\mathbf{u}}^T,\delta \alpha ]}^T$ and, based on ${\alpha }^o={\mathbf{u}}^{oT}{\mathbf{u}}^o$, we have

$$\overline{\alpha }-\delta \alpha =({\overline{\mathbf{u}}}^T-\delta {\mathbf{u}}^T)(\overline{\mathbf{u}}-\delta \mathbf{u})\approx {\overline{\mathbf{u}}}^T\overline{\mathbf{u}}-2{\overline{\mathbf{u}}}^T\delta \mathbf{u}$$

(24)

where $\delta \mathbf{u}$ is considered as a unknown vector and it can be used to correct the solution in the first stage. We can construct a system of equations

$$\delta \mathbf{\phi }={\mathbf{h}}_2-{\mathbf{G}}_2\delta \mathbf{u}$$

(25)

where

$${\mathbf{G}}_2=\left[\begin{array}{c}-\mathbf{I}\\ -2{\overline{\mathbf{u}}}^T\end{array}\right]{\mathbf{h}}_2=\left[\begin{array}{c}\mathbf{0}\\ \overline{\alpha }-{\overline{\mathbf{u}}}^T\overline{\mathbf{u}}\end{array}\right].$$

The left-side $\delta \mathbf{\phi }$ is pretended as a random error vector and $\delta \mathbf{u}$ on the right side is the unknown. The WLS solution of (25) is

$$\delta \overline{\mathbf{u}}={\left({\mathbf{G}}_2^T{\mathbf{W}}_2{\mathbf{G}}_2\right)}^{-1}{\mathbf{G}}_2^T{\mathbf{W}}_2{\mathbf{h}}_2$$

(26)

where ${\mathbf{W}}_2=cov{\left(\delta \mathbf{\phi }\right)}^{-1}=cov{\left(\overline{\mathbf{\phi }}\right)}^{-1}\approx {\mathbf{G}}_1^T{\mathbf{W}}_1{\mathbf{G}}_1$ is the weighting matrix. Finally, the estimate of this stage is

$$\widehat{\mathbf{u}}=\overline{\mathbf{u}}-\delta \overline{\mathbf{u}}.$$

(27)

In fact, the position estimate $\widehat{\mathbf{u}}$ in this stage is the accurate solution of the static model (9). However, the static model is an approximation of the moving model (1). $\widehat{\mathbf{u}}$ is not accurate and it will be refined in the following stage.

4.1.3. Third Stage

In this stage, we will refine the estimate $\widehat{\mathbf{u}}$ using the moving model (1). Squaring (1) after putting $\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel$ to the left side,

$$\begin{array}{c}\hfill c^2{\tau }_i^{o2}-2c{\tau }_i^o\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel ={\tau }_i^{o2}{\mathbf{v}}^T\mathbf{v}+2{\tau }_i^o{\mathbf{v}}^T({\mathbf{u}}^o-{\mathbf{s}}_i).\end{array}$$

(28)

Suppose ${\mathbf{u}}^o=\widehat{\mathbf{u}}+\Delta \mathbf{u}$ and we have the first-order Taylor-series expansion

$$\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel \approx \parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel +{\mathbf{\rho }}_{\widehat{\mathbf{u}}-{\mathbf{s}}_i}^T\Delta \mathbf{u}.$$

(29)

Then, substituting ${\mathbf{u}}^o=\widehat{\mathbf{u}}+\Delta \mathbf{u}$, ${\tau }_i^o={\tau }_i-\Delta {\tau }_i$ and (29) in (28), and neglecting the high-order noise terms yield

$$\begin{array}{c}\hfill [-2c^2{\tau }_i+2c\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel +2{\tau }_i{\mathbf{v}}^T\mathbf{v}+2{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)]\Delta {\tau }_i\\ \hfill =2({\tau }_i{\mathbf{v}}^T+c{\tau }_i{\mathbf{\rho }}_{\widehat{\mathbf{u}}-{\mathbf{s}}_i}^T)\Delta \mathbf{u}+{\tau }_i^2{\mathbf{v}}^T\mathbf{v}+2{\tau }_i{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)-c^2{\tau }_i^2+2c{\tau }_i\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel \end{array}$$

(30)

where $\Delta \mathbf{u}$ is unknown. This is another set of linear equations with respect to $\Delta \mathbf{u}$ and the matrix form is

$${\mathbf{P}}_3\mathbf{n}={\mathbf{h}}_3-{\mathbf{G}}_3\Delta \mathbf{u}$$

(31)

where the i-th row vector of ${\mathbf{G}}_3$ is $-2({\tau }_i{\mathbf{v}}^T+c{\tau }_i{\mathbf{\rho }}_{\widehat{\mathbf{u}}-{\mathbf{s}}_i}^T)$, the i-th element of ${\mathbf{h}}_3$ is ${\mathbf{h}}_3\left(i\right)={\tau }_i^2{\mathbf{v}}^T\mathbf{v}+2{\tau }_i{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)-c^2{\tau }_i^2+2c{\tau }_i\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel$, and ${\mathbf{P}}_3$ is a diagonal matrix. The i-th diagonal element is ${\mathbf{P}}_3(i,i)=-2c^2{\tau }_i+2c\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel +2{\tau }_i{\mathbf{v}}^T\mathbf{v}+2{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)$.

The WLS solution of (31) is

$$\Delta \widehat{\mathbf{u}}={\left({\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{G}}_3\right)}^{-1}{\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{h}}_3$$

(32)

where the weighting matrix is ${\mathbf{W}}_3={\left({\mathbf{P}}_3\mathbf{Q}{\mathbf{P}}_3^T\right)}^{-1}$. By refining the estimate in the second stage, the solution in this stage is finally updated as

$$\mathbf{u}=\widehat{\mathbf{u}}+\Delta \widehat{\mathbf{u}}.$$

(33)

It is obvious that (33) can be iterated several times to obtain an even better solution. The simulations show that applying (33) twice is sufficient to produce an accurate result.

4.2. AUV Localization with Known Depth

In this case, we shall estimate ${\mathbf{\mu }}^o={[u_x^o,u_y^o]}^T$ from the moving measurement model (1) using $\mathit{\tau }$, ${\mathbf{s}}_i$, $\mathbf{v}$ and $u_z^o$.

4.2.1. First Stage

When the depth is known ($u_z^o$ is known), (20) can be transformed as

$$c^2{\tau }_i^{s2}=4({\beta }^o+u_z^o{u}_z^o-2{\mathbf{\omega }}_i^T{\mathbf{\mu }}^o-2z_i{u}_z^o+{\mathbf{s}}_i^T{\mathbf{s}}_i)$$

(34)

where ${\beta }^o={\mathbf{\mu }}^{oT}{\mathbf{\mu }}^o$ is a nonlinear term, and it is supposed to be independent unknown variable with ${\mathbf{\mu }}^o$ in the first step. Substituting ${\tau }_i^s={\tau }_i-\Delta {\tau }_i$ into (34) and rearranging yield

$$\begin{array}{c}\hfill -2c^2{\tau }_i\Delta {\tau }_i=4{\beta }^o-8{\mathbf{\omega }}_i^T{\mathbf{\mu }}^o+{\overline{h}}_1\left(i\right)\end{array}$$

(35)

where ${\overline{h}}_1\left(i\right)=4u_z^o(u_z^o-2z_i)+4{\mathbf{s}}_i^T{\mathbf{s}}_i-c^2{\tau }_i^2$. Define ${\mathbf{\phi }}_2^o={[{\mathbf{\mu }}^{oT},{\beta }^o]}^T$, ${\overline{\mathbf{h}}}_1={[{\overline{h}}_1\left(1\right),{\overline{h}}_1\left(2\right),\cdots ,{\overline{h}}_1\left(N\right)]}^T$ and Equation (35) can be written in a matrix form as

$${\overline{\mathbf{P}}}_1\mathbf{n}={\overline{\mathbf{h}}}_1-{\overline{\mathbf{G}}}_1{\mathbf{\varphi }}^o$$

(36)

where ${\overline{\mathbf{P}}}_1={\mathbf{P}}_1$ and ${\overline{\mathbf{G}}}_1$ is

$${\overline{\mathbf{G}}}_1=\left[\begin{array}{cc}8{\mathbf{s}}_{21}^T& -4\\ 8{\mathbf{s}}_{22}^T& -4\\ ⋮\\ 8{\mathbf{s}}_{2N}^T& -4\end{array}\right].$$

(37)

The WLS solution is

$$\overline{\mathbf{\varphi }}={[{\overline{\mathbf{\mu }}}^T,\overline{\beta }]}^T={\left({\overline{\mathbf{G}}}_1^T{\overline{\mathbf{W}}}_1{\overline{\mathbf{G}}}_1\right)}^{-1}{\overline{\mathbf{G}}}_1^T{\overline{\mathbf{W}}}_1{\overline{\mathbf{h}}}_1$$

(38)

where ${\overline{\mathbf{W}}}_1={\left({\overline{\mathbf{P}}}_1\mathbf{Q}{\overline{\mathbf{P}}}_1^T\right)}^{-1}$ is the weighting matrix. Under small noise conditions, we have $cov\left(\overline{\mathbf{\varphi }}\right)\approx {\left({\overline{\mathbf{G}}}_1{\overline{\mathbf{W}}}_1{\overline{\mathbf{G}}}_1^T\right)}^{-1}$.

4.2.2. Second Stage

The second stage will estimate the amounts of corrections to the estimate from the first stage solution ${\overline{\mathbf{\phi }}}_2$. Supposing $\overline{\mathbf{\varphi }}={\mathbf{\varphi }}^o+\delta \mathbf{\varphi }$ and $\delta \mathbf{\varphi }={[\delta {\mathbf{\mu }}^T,\delta \beta ]}^T$, we have

$$\overline{\beta }-\delta \beta \approx {\overline{\mathbf{\mu }}}^T\overline{\mathbf{\mu }}-2{\overline{\mathbf{\mu }}}^T\delta \mathbf{\mu }$$

(39)

where $\delta \mathbf{u}$ is the unknown to be estimated. (39) can be written in a matrix form as

$$\delta \mathbf{\varphi }={\overline{\mathbf{h}}}_2-{\overline{\mathbf{G}}}_2\delta \mathbf{\mu }$$

(40)

where

$${\mathbf{G}}_2=\left[\begin{array}{c}-\mathbf{I}\\ -2{\overline{\mathbf{\mu }}}^T\end{array}\right]{\mathbf{h}}_2=\left[\begin{array}{c}\mathbf{0}\\ \overline{\beta }-{\overline{\mathbf{\mu }}}^T\overline{\mathbf{\mu }}\end{array}\right].$$

$\delta \mathbf{\varphi }$ is pretended as a noise vector and $cov\left(\delta \mathbf{\varphi }\right)\approx {\left({\overline{\mathbf{G}}}_1^T{\overline{\mathbf{W}}}_1{\overline{\mathbf{G}}}_1\right)}^{-1}$. The unknown vector $\delta \mathbf{\mu }$ can be solved using an WLS method

$$\delta \overline{\mathbf{\mu }}={\left({\overline{\mathbf{G}}}_2^T{\overline{\mathbf{W}}}_2{\overline{\mathbf{G}}}_2\right)}^{-1}{\overline{\mathbf{G}}}_2^T{\overline{\mathbf{W}}}_2{\overline{\mathbf{h}}}_2$$

(41)

where ${\overline{\mathbf{W}}}_2=cov{\left(\delta \overline{\mathbf{\varphi }}\right)}^{-1}$ is the weighting matrix. Finally, the estimate of this stage is

$$\widehat{\mathbf{\mu }}=\overline{\mathbf{\mu }}-\delta \overline{\mathbf{\mu }}.$$

(42)

4.2.3. Third Stage

Suppose ${\mathbf{\mu }}^o=\widehat{\mathbf{\mu }}+\Delta \mathbf{\mu }$, $\widehat{\mathbf{u}}={[{\widehat{\mathbf{\mu }}}^T,u_z^o]}^T$ and we have the first-order Taylor-series expansion

$$\parallel {\mathbf{u}}^o-{\mathbf{s}}_i\parallel \approx \parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel +\frac{{(\widehat{\mathbf{\mu }}-{\mathbf{\omega }}_i)}^T}{\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel }\Delta \mathbf{\mu }.$$

(43)

Then, substituting ${\mathbf{\mu }}^o=\widehat{\mathbf{\mu }}+\Delta \mathbf{\mu }$, ${\tau }_i^o={\tau }_i-\Delta {\tau }_i$ and (43) in (28) and neglecting the high-order noise terms yield:

$$\begin{array}{c}[-2c^2{\tau }_i+2c\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel +2{\tau }_i{\mathbf{v}}^T\mathbf{v}+2{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)]\Delta {\tau }_i\hfill \\ =2({\tau }_i{\mathbf{\nu }}^T+c{\tau }_i\frac{{(\widehat{\mathbf{\mu }}-{\mathbf{\omega }}_i)}^T}{\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel })\Delta \mathbf{\mu }+{\tau }_i^2{\mathbf{v}}^T\mathbf{v}+2{\tau }_i{\mathbf{v}}^T(\widehat{\mathbf{u}}-{\mathbf{s}}_i)-c^2{\tau }_i^2+2c{\tau }_i\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel \hfill \end{array}$$

(44)

where $\Delta \mathbf{\mu }$ is unknown and the corresponding matrix form is expressed as

$${\overline{\mathbf{P}}}_3\mathbf{n}={\overline{\mathbf{h}}}_3-{\overline{\mathbf{G}}}_3\Delta \mathbf{\mu }$$

(45)

where the i-th row vector of ${\overline{\mathbf{G}}}_3$ is

$${\overline{\mathbf{G}}}_3\left(i\right)=-2\left({\tau }_i{\mathbf{\nu }}^T+c{\tau }_i\frac{{(\widehat{\mathbf{\mu }}-{\mathbf{\omega }}_i)}^T}{\parallel \widehat{\mathbf{u}}-{\mathbf{s}}_i\parallel }\right).$$

(46)

The expressions of ${\overline{\mathbf{h}}}_3$ and ${\overline{\mathbf{P}}}_3$ are the same as ${\mathbf{h}}_3$ and ${\mathbf{P}}_3$ in (31), respectively. The WLS solution of (45) is

$$\Delta \widehat{\mathbf{\mu }}={\left({\overline{\mathbf{G}}}_3^T{\overline{\mathbf{W}}}_3{\overline{\mathbf{G}}}_3\right)}^{-1}{\overline{\mathbf{G}}}_3^T{\overline{\mathbf{W}}}_3{\overline{\mathbf{h}}}_3$$

(47)

where the weighting matrix is ${\overline{\mathbf{W}}}_3={\left({\overline{\mathbf{P}}}_3\mathbf{Q}{\overline{\mathbf{P}}}_3^T\right)}^{-1}$. By refining the estimate in the second stage, the solution in this stage is finally refined as

$$\mathbf{\mu }=\widehat{\mathbf{\mu }}+\Delta \widehat{\mathbf{\mu }}.$$

(48)

4.3. Performance Analysis

We shall estimate the theoretical covariance matrix of the proposed solution, and compare it with the CRLB. We first consider the known depth localization. When the noise is small, both the bias of the final estimate $\mathbf{u}$ and the bias of the third step estimate $\Delta \widehat{\mathbf{u}}$ are negligible, and so the covariance of $\mathbf{u}$ is

$$\begin{array}{ccc}cov\left(\mathbf{u}\right)\hfill & =\hfill & E\left[(\mathbf{u}-{\mathbf{u}}^o){(\mathbf{u}-{\mathbf{u}}^o)}^T\right]\hfill \\ \hfill & =\hfill & E\left[(\widehat{\mathbf{u}}+\Delta \widehat{\mathbf{u}}-{\mathbf{u}}^o){(\widehat{\mathbf{u}}+\Delta \widehat{\mathbf{u}}-{\mathbf{u}}^o)}^T\right]\hfill \\ \hfill & =\hfill & E\left[(\Delta \widehat{\mathbf{u}}-\Delta \mathbf{u}){(\Delta \widehat{\mathbf{u}}-\Delta \mathbf{u})}^T\right]\hfill \\ \hfill & =\hfill & cov(\Delta \widehat{\mathbf{u}}).\hfill \end{array}$$

(49)

This implies that the covariance of the final estimate $\mathbf{u}$ is equal to the covariance matrix of the third step estimate $\Delta \widehat{\mathbf{u}}$. After subtracting both sides of (32) by the value $\Delta \mathbf{u}$ and applying (31) yield the estimation error

$$\Delta \widehat{\mathbf{u}}-\Delta \mathbf{u}={\left({\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{G}}_3\right)}^{-1}{\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{P}}_3\mathbf{n}.$$

(50)

When the noise is small so that the noise in the related matrices can be ignored, it can be shown that

$$\begin{array}{cc}\hfill E[\Delta \widehat{\mathbf{u}}-\Delta \mathbf{u}]& \approx {\left({\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{G}}_3\right)}^{-1}{\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{P}}_{3}E\left[\mathbf{n}\right]=\mathbf{0}\hfill \\ \hfill cov(\Delta \widehat{\mathbf{u}})& \approx {\left({\mathbf{G}}_3^T{\mathbf{W}}_3{\mathbf{G}}_3\right)}^{-1}.\hfill \end{array}$$

(51)

Substitute ${\mathbf{W}}_3$ below (32), and $cov(\Delta \widehat{\mathbf{u}})$ can be expressed as

$$\begin{array}{c}cov(\Delta \widehat{\mathbf{u}})\approx {\left({\mathbf{G}}_3^T{\mathbf{P}}_3^{-1}{\mathbf{Q}}^{-1}{\mathbf{P}}_3^{-1}{\mathbf{G}}_3\right)}^{-1}.\hfill \end{array}$$

(52)

Note that (52) has the same form as the CRLB of (11). If we substitute the true values ${\mathbf{u}}^o$ and ${\tau }_i^o$ for the estimated ones $\widehat{\mathbf{u}}$ and ${\tau }_i$ in ${\mathbf{G}}_3$ and ${\mathbf{P}}_3$, we have

$$\begin{array}{ccc}{\mathbf{P}}_3^{-1}(i,i)\hfill & =\hfill & {\left({\mathbf{P}}_3(i,i)\right)}^{-1}\hfill \\ \hfill & =\hfill & -{\left(2\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel \right)}^{-1}{(c-{\mathbf{\rho }}_{{\mathbf{u}}_i^o-{\mathbf{s}}_i}^T\mathbf{v})}^{-1}\hfill \end{array}$$

(53)

and the ith column of ${\mathbf{G}}_3^T$ is

$$\begin{array}{ccc}{\mathbf{G}}_3^T\left(i\right)\hfill & =\hfill & -2({\tau }_i\mathbf{v}+c{\tau }_i{\mathbf{\rho }}_{\widehat{\mathbf{u}}-{\mathbf{s}}_i})\hfill \\ \hfill & =\hfill & -2\left({\mathbf{u}}_i^o-{\mathbf{s}}_i+\parallel {\mathbf{u}}_i^o-{\mathbf{s}}_i\parallel {\mathbf{\rho }}_{{\mathbf{u}}^o-{\mathbf{s}}_i}\right).\hfill \end{array}$$

(54)

After some straightforward matrix manipulation, we have

$${\mathbf{G}}_3^T{\mathbf{P}}_3^{-1}={\mathbf{J}}_{{\mathbf{u}}^o}.$$

(55)

As a result, based on (11) and (52) and for sufficient small noise, we have

$$cov\left(\mathbf{u}\right)\approx {\mathbf{C}}_{{\mathbf{u}}^o}$$

(56)

and the proposed closed-form solution can reach the CRLB.

The same analysis can also obtained for the AUV localization case with known depth. The estimated theoretical covariance matrix is

$$cov\left({\mathbf{u}}_2\right)={\left({\overline{\mathbf{G}}}_3^T{\overline{\mathbf{W}}}_3{\overline{\mathbf{G}}}_3\right)}^{-1}\approx {\mathbf{C}}_{{\mathbf{u}}_2^o}$$

(57)

if the noise effect in ${\overline{\mathbf{G}}}_3$ or ${\overline{\mathbf{W}}}_3$ is insignificant. Therefore, the proposed solution can attain the corresponding CRLB for sufficient small noise.

5. Simulation

In this section, the numerical simulations are carried out to examine the performance of the proposed closed-form solution for the moving model. the existing Feng’s method in [25] for moving AUV localization and the two-step WLS method in [26] for solving the static model are included for comparison. In addition, the first stage solution and second stage solution are evaluated to show the performance improvement in different stages. the localization accuracy is examined in terms of mean square error (MSE) and bias,

$$\begin{array}{c}\hfill \mathrm{bias}=\parallel \frac{1}{L}\sum _{l=1}^L{\mathbf{u}}_l-{\mathbf{u}}^o\parallel \\ \hfill \mathrm{MSE}=\frac{1}{L}\sum _{l=1}^L{\parallel {\mathbf{u}}_l-{\mathbf{u}}^o\parallel }^2\end{array}$$

where ${\mathbf{u}}_l$ denotes a position estimate of a given method at ensemble l and $L=5000$ is the ensemble runs.

The sensor positions shown in Table 1 are based on the moving AUV localization scenario in [21], with one additional sensor. The acoustic signal speed $c=1456$ m/s and the covariance matrix of time measurement noise is $\mathbf{Q}={\sigma }_t^2\mathbf{I}$. ${\sigma }_t$ is the standard deviation of $\Delta {\tau }_i$. For simplicity, we define range measurement standard deviation $\sigma =c{\sigma }_t$ m.

5.1. AUV Localization with Unknown Depth

Figure 5 compares the MSE of the proposed solution for a moving model and the two-step WLS solution for a static model. The localization scenario and noise setting are the same as in Figure 4b, and Figure 4b is also the corresponding CRLB of this scenario. Figure 5a shows the MSE using the static model. The MSE values of the static model are large and far away from the CRLB. In contrast, the proposed solution shown in Figure 5b can improve the estimation accuracy and reach the CRLB.

Figure 6a is the result for a moving AUV located at ${[50,50,3.9]}^T$ m with velocity $[1.5,1.5,0]$ m/s. The sensor positions correspond to those in Figure 4b and there are five sensors. The value of measurement noise $10log\left(\sigma \right)$ varies from $-20$ dB to 0 dB. The localization performance of the static model is generally worse for a moving AUV. There is a definite performance improvement in the three stages and the performance of second stage solution is almost the same as that of the two-step WLS. the bias of the proposed closed-form method for moving model is near zero, while the two-step WLS solution of the static model has a steady bias. Although the bias of Feng’s method is also close to zero, the corresponding MSE can not reach the CRLB and there is a steady gap between them that is about $3.5$ dB. As expected, the MSE values of static model are far away from CRLB, especially when the noise is small. The MSE of the proposed method can attain the CRLB and is lower than Feng’s method by $3.4$ dB. For example, when $\sigma =1$ m, the MSE of the proposed method is less than that of Feng’s method by $1.9$ m${}^2$.

Figure 6b shows the result of the proposed method for moving AUV localization with different values of speed. The same localization geometry is utilized as in Figure 6a, except that the AUV speed $\mathbf{v}$ is normalized to be a unit vector and then is multiplied by a scale value, and the standard deviation ${\sigma }_t=300$ $\mathsf{\mu }$s ($\sigma =0.44$ m). Both the bias and the MSE of the static model increase as the AUV speed increases. On the contrary, the AUV speed has less effect on the performance of the proposed solution and Feng’s method. However, the observation validates again that the proposed method is better than Feng’s method in terms of MSE. in general, the proposed method using the moving model outperforms the other two methods.

5.2. AUV Localization with Known Depth

Figure 7 shows the result for known depth localization. The localization scenario and noise setting are the same as in Figure 4d which also shows the CRLB of this case. It is evident that the proposed method in Figure 7b is able to attain the accuracy set by the CRLB. However, the MSE of the two-step WLS solution using the static model is not satisfying.

Figure 8a,b are for the known depth localization as in Figure 4d and there are four sensors. The AUV is located at ${[50,50,3.9]}^T$ m with velocity $[1.5,1.5,0]$ m/s. Figure 8a shows the effect of noise to bias and MSE. The proposed solution performs better than the two-step WLS using the static model and it can reach the CRLB when the measurement noise is small. The performance improvement in MSE is about $1.5$ dB when using the proposed method compared with Feng’s method. Figure 8b shows the performance of the mentioned methods with different values of speed. The observations are consistent with those in Figure 8b. The performance improvement of the proposed method using the moving model is significant.

5.3. AUV Localization Performance When Error Occurring in Depth

This subsection investigates the effect of depth error on AUV localization. The AUV depth is assumed to be exactly known in the known depth localization. However, there is an error with the depth measurement in the real situation besides the TWTT measurement noise. In this subsection, the available depth measurement $u_z$ is contaminated by noise and it is modeled as $u_z=u_z^o+\Delta u_z$. $\Delta u_z$ is zero mean Gaussian and ${\sigma }_d$ is the standard deviation that changes from 0.2 m to 2 m. The other settings are the same as Figure 6 except ${\sigma }_t$.

Figure 9 demonstrates the performance of the proposed methods when depth error increases where “UD” denotes “Unknown Depth” and “KD” denotes “Known Depth”. Apparently, the observation shows that the depth error cannot affect the performance of unknown depth localization. Figure 9a shows that depth error effect is not significant in the known depth localization when TWTT measurement noise is relatively large (${\sigma }_t=300$ $\mathsf{\mu }$s). The proposed known depth localization can still reach the CRLB. However, the depth error effect becomes significant in Figure 9b when TWTT measurement noise is relatively small (${\sigma }_t=30$ $\mathsf{\mu }$s). Therefore, the known depth localization methods are sensitive to depth error when the TWTT measurement noise is small.

5.4. AUV Localization Performance When Error Occurring in Velocity

Similar to the depth measurement, there is an error with the AUV velocity in the real situation besides the TWTT measurement noise. This subsection investigates the effect of AUV velocity error on the localization. The available velocity is $\mathbf{v}+\Delta \mathbf{v}$ instead of $\mathbf{v}$ and $\Delta \mathbf{v}$ is assumed to be zero-mean Gaussian. In the simulation, the covariance of $\Delta \mathbf{v}$ is set as ${\sigma }_v^2\mathbf{I}$ and ${\sigma }_v$ is the standard deviation that changes from 0.1 m/s to 1 m/s. The other settings are the same as Figure 6 except ${\sigma }_t$.

Figure 10 demonstrates the performance of the proposed methods when velocity error increases. The performance is affected by the velocity error. Figure 10a shows that velocity error effect is not significant when TWTT measurement noise is relatively large (${\sigma }_t=300$ $\mathsf{\mu }$s). The proposed methods can still reach the CRLB when ${\sigma }_v\le 0.3$ m/s. However, the velocity error effect becomes significant in Figure 10b when TWTT measurement noise is relatively small (${\sigma }_t=30$ $\mathsf{\mu }$s). Therefore, the localization methods are sensitive to velocity error when the TWTT measurement noise is small.

5.5. AUV Localization Performance with Randomly Generated Scenarios

It is widely known that the localization performance depends greatly on the AUV-sensor geometry, such as sensor positions, number of sensors, AUV positions, AUV moving direction, and so on. These factors are fixed in the above experiments. Therefore, in this subsection, the localization performance is examined using the randomly generated scenarios. The sensors and AUV are placed randomly at uniformly distributed positions inside a 3D cube ($[0,500]$ m$\times [0,500]$ m$\times [0,100]$ m). The AUV moving direction is generated randomly in each direction and AUV speed is generated randomly from the interval $[5,10]$ m/s. We randomly generate 200 scenarios each with different geometries, and the results are the average over them. The number of ensemble runs is 5000 for each given geometry realization. There is no depth error or velocity error and TWTT noise ${\sigma }_t=300$ $\mathsf{\mu }$s.

Figure 11 and Figure 12 show the performance with randomly generated scenarios using different number of sensors. The introduction of the auxiliary variable ${\alpha }^o$ (or ${\beta }^o$) in the first stage requires a minimum of four (or three) TWTT measurements for the unknown (or known) depth localization. However, the minimum required measurements are not enough to ensure the localization performance in the randomly generated scenarios because an improper geometry with less sensors can appear. This phenomenon can be alleviated dramatically if we increase the number of sensors by two (or one) for unknown (or known) depth localization. The proposed solutions can reach the CRLBs when there are enough sensors. In addition, it should be noted that extensive simulations show that the performance of the second stage solution is almost the same as that of the two-step WLS. Hence, the proposed first and second stage solution can be replaced by the two-step WLS in most cases.

6. Conclusions

In this paper, we studied the accurate closed-form solution for moving AUV localization with range-only data. First, a new time measurement model for moving AUV localization was presented, which compensates for vehicle motion. Then, the CRLB of the proposed model was derived in two different cases. We also proved that accurately measuring the AUV depth can improve the AUV localization accuracy. Next, we derived a closed-form solution for the proposed model in two cases. Moreover, the proposed solution was shown analytically to reach the CRLB. Finally simulations collaborate the theoretical performance of the proposed estimators and the moving model significantly improves the localization accuracy in comparison with the static model.

The AUV localization accuracy can be affected by the accuracy of parameters in the measurement model, including the TWTT measurement noise, the depth error, the AUV velocity error, the sensor position error, and the sound speed error. When the TWTT measurement noise is significant compared with the other errors, the proposed solutions are accurate enough to reach the CRLB. However, when the other errors are comparable with the TWTT measurement noise, the errors that are significant should also be accounted for in the method to improve the localization accuracy. Some errors, such as the sensor position error and the sound speed error, have been studied by the previous research [20,29,30,31,32], while the other errors, such as depth error and AUV velocity error, should be studied further in the future research.