## 3 Acoustic Localization  
Taking advantage of the finite propagation speed of sound in water and the availability of GPS or global navigation satellite system (GNSS) signals [26] over the ocean surface, we deploy three surface buoys with known positions \([x_{n},y_{n},z_{n}]^{\rm T}\), \(n=1,2,3\), as shown in Figure 2. However, the underwater velocity of sound is generally not constant but varies with the water pressure, temperature, and conductivity. Therefore, the propagation path of the sound is actually curved, which leads to non-random deviations of signal arrival times. To overcome this difficulty, the ray tracing technique [27] with isogradient sound speed profiles (SSP) is utilized in the AUV acoustic localization problem. Consider the scenario that each of the three buoys carries a pinger and emits acoustic signals every \(T_{s}^{\rm T}\) seconds. Denote the position of AUV as \([x(t),y(t),z(t)]^{\rm T}\) at time \(t\). Since the depth of underwater vehicles can be accurately measured with a depth sensor [28], we consider the AUV to move at a known constant depth of \(z\) meters. The clocks of the three buoys and the AUV can be synchronized with either GPS or lower power atomic clocks. For the ray tracing problem between the surface buoys and the AUV, the SSP is dependent on the water depth as:  
\[c=az+b \tag{7}\]  
where \(c\) is the sound speed, \(a\) is a constant depending on the underwater environment, and \(b\) is the sound speed at the surface. Then, the TOAs of the signals from the three buoys at the AUV, denoted as \(\Delta t_{n}\), \(n=1,2,3\), can be calculated in simulations as [27]:\[d_{n}(t) =\sqrt{(x(t)-x_{n})^{2}+(y(t)-y_{n})^{2}} \tag{8}\] \[L_{n} =\frac{0.5 az}{b+0.5 az}\] (9) \[X_{n}(t) =z/d_{n}(t)\] (10) \[Y_{n}(t) =L_{n}(t)/X_{n}(t)\] (11) \[\alpha_{n}(t) =\arctan(Y_{n}(t))\] (12) \[\beta_{n}(t) =\arctan(X_{n}(t))\] (13) \[\theta_{n}^{B}(t) =\beta_{n}(t)+\alpha_{n}(t)\] (14) \[\theta_{n}^{A}(t) =\beta_{n}(t)-\alpha_{n}(t)\] (15) \[c=az+b \tag{7}\]0  
where \(d_{n}\) stands for the actual horizontal distance between buoy \(n\) and the AUV; \(L_{n}\), \(X_{n}\) and \(Y_{n}\) are the intermediate variables; \(\alpha_{n}\) is the angle between the actual sound ray and the straight line path for buoy \(n\); \(\beta_{n}\) is the angle between the horizontal direction and the straight line path for buoy \(n\). Lastly, \(\theta_{n}^{B}\) and \(\theta_{n}^{A}\) are the grazing ray angles at buoy \(n\) and the AUV, respectively. In simulations, TOAs \(\Delta t_{n}(t)\), \(n=1,2,3\) can be calculated as Equation (16), and in practice, they can be measured with synchronized clocks. Then, with the known TOAs, AUV depth \(z\), and the fixed buoy locations, the horizontal distance \(\hat{d}_{n}\) could be estimated reversely using algorithms such as the binary search [27]. The main idea is to compare the arrival time of the median point with the actual propagation time repeatedly so that the search area could be narrowed down until a precise enough horizontal distance estimate \(\hat{d}_{n}\) is found. Next, we are able to obtain the position of the AUV with \(\hat{d}_{n}\). If in one signal emit-receive cycle, the AUV is moving slowly and the position is assumed to be the same when the three signals from buoys 1, 2, and 3 are received, then we can get a set of nonlinear equations as:  
\[c=az+b \tag{7}\]1  
by solving which we get the acoustically estimated position \((\hat{x}(t),\hat{y}(t))\) of the AUV. Note that in practice, only two equations are necessary to be solved for \((\hat{x}(t),\hat{y}(t))\). Available functions such as _fsolve_ using trust-region algorithm in Matlab [29] can be applied to solve Equation (17). \[c=az+b \tag{7}\]2 AUV localization with three surface buoys. If the AUV horizontal heading is measured by on board sensors such as the inertial measurement unit as \(\hat{\psi}(t)\), the AUV horizontal pose can be obtained as:  
\[c=az+b \tag{7}\]3  
which can be seen as the \"measured\" system output.