Robust Underwater Vehicle Pose Estimation via Convex Optimization Using Range-Only Remote Sensing Data

Abstract

Accurate localization plays a critical role in enabling underwater vehicle autonomy. In this work, we develop a robust infrastructure-based localization framework that estimates the position and orientation of underwater vehicles using only range measurements from long baseline (LBL) acoustic beacons to multiple on-board receivers. The proposed framework integrates three key components, each formulated as a convex optimization problem. First, we introduce a robust calibration function that unifies multiple sources of measurement error—such as range-dependent degradation, variable sound speed, and latency—by modeling them through a monotonic function. This function bounds the true distance and defines a convex feasible set for each receiver location. Next, we estimate the receiver positions as the center of this feasible region, using two notions of centrality: the Chebyshev center and the maximum volume inscribed ellipsoid (MVE), both formulated as convex programs. Finally, we recover the vehicle’s full 6-DOF pose by enforcing rigid-body constraints on the estimated receiver positions. To do this, we leverage the known geometric configuration of the receivers in the vehicle and solve the Orthogonal Procrustes Problem to compute the rotation matrix that best aligns the estimated and known configurations, thereby correcting the position estimates and determining the vehicle orientation. We evaluate the proposed method through both numerical simulations and field experiments. To further enhance robustness under real-world conditions, we model beacon-location uncertainty—due to mooring slack and water currents—as bounded spherical regions around nominal beacon positions. We then mitigate the uncertainty by integrating the modified range constraints into the MVE position estimation formulation, ensuring reliable localization even under infrastructure drift.

1. Introduction

Autonomous Underwater Vehicles (AUVs) are increasingly deployed for marine exploration, environmental monitoring, infrastructure inspection, and defense applications [1,2]. Accurate localization is essential in these missions—not only for autonomous navigation and control [3] but also to preserve the spatial context of collected data, which determines its scientific or operational value. However, underwater localization remains difficult due to the complex, unstructured, and dynamic nature of the subsurface environment.

Conventional radio-frequency-based localization methods are ineffective in water due to the rapid attenuation of electromagnetic signals. Alternative approaches, such as Simultaneous Localization and Mapping (SLAM) using magnetic or gravitational anomaly maps [4,5], or optical-signal-based localization, face limitations in low-visibility or unstructured environments. As a result, infrastructure-based acoustic localization—particularly via Long Baseline (LBL) beacon arrays—has emerged as the preferred solution for mid- and deep-water deployments.

In LBL networks, multiple acoustic beacons are deployed at known locations, either moored to the sea floor or suspended from surface vessels [6]. These beacons transmit acoustic signals to receivers onboard the AUV to obtain range measurements, typically derived from the time-of-arrival (TOA) of the transmitted signals [7]. The range measurements are used to estimate the AUV’s position using trilateration. While range-free localization techniques exist, we focus on range-based methods due to their higher accuracy and broader applicability [8]. Despite the structural benefits and wide area coverage offered by LBL networks, trilateration within them is subject to several sources of uncertainty that degrade localization accuracy. These include multipath propagation of acoustic signals, variability in sound speed (due to fluctuations in temperature and salinity), beacon drift (caused by currents and mooring slack), and range-dependent degradation in measurement accuracy. Addressing these effects is critical to enable robust, real-time localization [9].

1.1. Recent Literature on LBL Localization

Recent work on long-baseline (LBL) localization has predominantly focused on modeling individual sources of uncertainty. A major area of attention is sound-speed variation, which can distort time-of-arrival (TOA) estimates. To address this, Yao et al. [10] employ constant-gradient acoustic ray tracing using a depth-dependent sound-speed profile to estimate bent propagation paths. They then compute position estimates by minimizing the sum of squared residuals between predicted and measured TOAs, a non-convex approach that relies on iterative solvers.

Other approaches target multipath distortion. Sun et al. [11] and Li et al. [12] improve direct-sound selection (DSS) to identify and suppress reflected signals. While these methods enhance TOA reliability, their performance often depends on thresholding and signal-level heuristics, which may limit generalization. An alternative paradigm is to bypass TOA extraction altogether: track-before-detect (TBD) approaches, such as those in [13,14], operate directly on raw acoustic data. These methods track multiple vehicle states (e.g., position and velocity) using particle filtering or recursive estimation. Although powerful in complex environments, they typically require access to raw waveform data and incur substantial computational overhead.

On the hardware front, efforts to develop inexpensive, rapidly deployable acoustic beacons have gained momentum [15]. However, such systems often suffer from range-dependent degradation and beacon drift—challenges confirmed by field deployments using surface buoys [16] and long-range trials [17].

1.2. Proposed Method in Context

Our method differs fundamentally from the above approaches in the following ways. Firstly, our method relies only on range measurements, making the approach simple to deploy and compatible with low-cost acoustic infrastructure such as that considered in [15]. Secondly, rather than addressing error sources individually, we introduce a unified error treatment through a data-driven calibration function, which maps noisy range measurements to upper bounds on true distances. This function captures increasing range-dependent error (as observed in [16]) and reduces the influence of long-distance beacons by inflating their associated feasible sets. In parallel, we model beacon location uncertainty using spherical drift bounds centered at nominal positions. Thirdly, to overcome the limitations of least-squares residual minimization methods (for trilateration), we propose two convex optimization formulations for position estimation: one based on the Chebyshev center, and another using the Maximum-Volume Ellipsoid (MVE), which is scale-invariant and avoids the skew caused by distant sensors. Both formulations are computationally efficient and solvable to the global optimum with commercial solvers, in contrast to particle-swarm or gradient-based heuristics. Fourthly, our method provides an explicit uncertainty radius along with position estimates. Lastly, our method leverages the relative positioning of multiple on-board receivers to correct the position estimates and determine the vehicle’s orientation, recovering the vehicle’s full 6-DOF pose.

To our knowledge, no recent work simultaneously addresses sound-speed variation, multipath bias, range degradation, and beacon drift, while also providing guaranteed globally optimal position and orientation estimates. Moreover, our method provides point-to-point estimates based solely on range measurements. This makes it easily integrable with existing tracking or filtering frameworks such as Kalman filters, particle filters, or track-before-detect pipelines. Improvements in range or TOA estimation—such as those enabled by advanced signal processing or DSS methods—can be directly incorporated into our calibration pipeline to further refine pose estimates. Thus, rather than replacing other localization strategies, our framework is designed to complement and enhance them. It serves as a modular component that provides globally consistent, geometry-aware position and orientation estimates that can be embedded within larger navigation or tracking systems. This technique also allows online re-calibration, when the vehicle maneuvers outside the calibration range, and unlike standard methods such as dead reckoning, this scheme provides bounded error estimates, and the errors do not propagate with time.

1.3. Organization

The remainder of the paper is organized as follows. Section 2 presents the problem setup and statement. Section 3 presents the mathematical formulation of the three components: sensor calibration, position estimation, and rigid-body correction for orientation. Section 4 discusses algorithms for solving the mathematical formulations. Section 5 evaluates the proposed framework through simulations and field experiments. Concluding remarks are provided in Section 6. Modeling and incorporation of beacon location uncertainty is deferred to Appendix B.

2. Problem Setup

Consider an underwater sensor network, consisting of N LBL beacons positioned at pre-determined locations, to aid the localization of vehicles. Let the coordinates of these beacons in a global reference frame be $B_i:=(x_i^b,y_i^b,z_i^b)$, $i=1,2,\dots ,N$. The vehicle, for which the position and orientation are to be determined, is equipped with L receivers on board, and is assumed to navigate within the convex hull of these beacons. Let the location estimates of these receivers in the global reference frame be $r_j:=(x_j,y_j,z_j)$, $j=1,2,\dots ,L$. Each beacon in the network communicates with the receivers on board, to estimate the ranges between them with the help of OWTT with synchronized clocks. A cut-off time is selected for the acoustic signals to reach from the beacons to the receivers, and all the range measurements obtained before the cut-off time are considered for localization. Let the measured range of jth on-board receiver as measured by the ith beacon be $D_{ij}$, while the true range between them is $d_{ij}$. If the range measurements of a receiver j are indeed equal to the true ranges from the beacons, then the receivers are supposed to lie on the intersection of spheres of radius $d_{ij}$, centered at $B_i$. Nonetheless, in reality, the range measurements need not be equal to the true ranges. If all the measured ranges are greater than the true range values, the receivers lie inside spheres of radius $D_{ij}$ centered at the beacons. The intersection of all such spheres forms the set of all possible receiver locations. However, if a few of the measured ranges are less than the true ranges, this need not be true as the spheres may no longer intersect. Hence, we compute a sensing function (calibrating function) $\varphi$ that is an increasing function of range measurements, to update the radii of the spheres to $\varphi \left(D_{ij}\right)$, such that the receivers are guaranteed to lie inside the intersection of these spheres. The intersection forms a convex set that contains all the possible receiver locations. This sensing model has been chosen so that it is amenable to experimental corroboration and makes the subsequent formulation cleaner. Additionally, it accounts for various errors arising in an underwater setting, such as variation in the speed of sound, small changes in the vehicle’s location before range measurements from various beacons are obtained, etc. The problem of determining the calibrating function is formulated as a semi-definite program in Section 3.2.

Given the aforementioned context, the problem of localization is stated as the following: Determine the estimates $(x_j,y_j,z_j)$, $j=1,2,\dots ,L$ of the location of the L on-board receivers as well as the orientation of the vehicle that is treated as a rigid body. Note that while standard pressure sensors, such as strain gauges and quartz crystals, provide accurate measurements of water depth, they need not always correspond to the geodetic altitude of the vehicle. This is due to the variation in the ocean’s surface [18]. Hence, the 3-D estimates provided by the algorithm would be useful. However, in applications where the depth measurements are sufficient, they can be combined with the 2-D estimates provided by the algorithm to locate the vehicle.

3. Mathematical Formulation

In this section we present a mathematical formulation of the following three problems: (i) optimal estimation of the location of the jth on-board receiver, (ii) determination of the function $\varphi (·)$ for the sensing model, and (iii) estimation of orientation of the vehicle from the location estimates of the on-board receivers.

3.1. Optimal Estimation of the Location of the $j^{th}$ On-Board Receiver

Since the distance measurements are available from each beacon, it is readily clear that

$$\begin{array}{c}\hfill ∥r_j-B_i∥=d_{ij}⩽\varphi \left(D_{ij}\right),i=1,\dots ,N.\end{array}$$

(1)

These N distance constraints are convex in $r_j$; note that $B_i$ are known a priori (Formulation considering the uncertainty in beacon locations is presented in Appendix B). Let ${\mathcal{F}}_j$ be the feasible values of $r_j$ for the above set of constraints. Essentially, the feasible set is the set obtained by intersecting spheres centered at the beacons and of radii determined by the range measurements gathered by the jth receiver; hence, ${\mathcal{F}}_j$ is a convex set. The feasible set indicates the set of all possible locations of the jth receiver. The center of the set ${\mathcal{F}}_j$ can be considered as the best estimate of $r_j$. We consider two notions of “center” of the set ${\mathcal{F}}_j$: the center of the largest inscribed disk (referred to as Chebyshev center) [19] and the center of the maximum volume inscribed ellipsoid [20]. The former can be computed via linear programming, while the latter can be computed via semi-definite programming.

3.1.1. Chebyshev Center of ${\mathcal{F}}_j$

If u is any unit vector and $r_{c,j}$ is the center of the Chebyshev disk, then the Chebyshev center of ${\mathcal{F}}_j$ can be computed by the following optimization problem:

$$\left({\mathcal{L}}_1\right)\underset{l,r_{c,j}}{max}l,\mathrm{subject}\mathrm{to}:$$

(2)

$$∥r_{c,j}+lu-B_i∥⩽\varphi \left(D_{ij}\right),i=1,\dots ,N,\forall u.$$

(3)

The constraints (3) are convex conic constraints [21] and describe a disk. The radius of the disk l is defined by the model for measurement and is characterized by the function $\varphi (·)$. These constraints can be relaxed to linear constraints in the same manner as approximating a disk by a regular polygon circumscribing the disk. Note that for any vector x, its norm $∥x∥={max}_{v:∥v∥=1}v·x$. Hence, the conic constraints can be recast as a semi-infinite set of linear inequalities [21] as:

$$\begin{array}{c}\hfill \underset{v:∥v∥=1}{max}\left(v·r_{c,j}+lv·u-v·B_i\right)⩽\varphi \left(D_{ij}\right),i=1,\dots ,N,\forall u.\end{array}$$

The tightest inequality corresponds to $u=v$ and hence, the semi-infinite linear program describing the location estimation problem is:

$$\begin{array}{c}\hfill \left({\mathcal{L}}_2\right)\underset{l,r_{c,j}}{max}l,\mathrm{subject}\mathrm{to}:v·r_{c,j}+l⩽v·B_i+\varphi \left(D_{ij}\right),i=1,\dots ,N,\forall v:∥v∥=1.\end{array}$$

3.1.2. Maximum Volume Inscribed Ellipsoid of ${\mathcal{F}}_j$

The volume of the feasible set ${\mathcal{F}}_j$ is a measure of the uncertainty/confidence associated with the position of the jth on-board receiver. Here, the “best” estimate is given by the center of the maximum volume ellipsoid ${\mathcal{E}}_j$ contained in ${\mathcal{F}}_j$. The rationale behind this approach is that the volume $V_j$ of this ellipsoid ${\mathcal{E}}_j$ provides a lower bound for the volume of ${\mathcal{F}}_j$ while the minimum volume of an ellipsoid containing ${\mathcal{F}}_j$ is within $\sqrt{3}{V}_j$ in a three-dimensional space [21]. Hence, the volume $V_j$ can be used as a proxy measure for the volume of ${\mathcal{F}}_j$. Moreover, the problem of determining the maximum volume ellipsoid contained in ${\mathcal{F}}_j$ is convex, while the problem of determining the minimum volume ellipsoid containing ${\mathcal{F}}_j$ is intractable given the inequality constraints [22]. Furthermore, the estimate of the center of this ellipsoid is invariant under affine transformations, unlike the Chebyshev center [21].

Any $x\in {\mathcal{E}}_j$ can be written as $x=P_{j}u+r_{c,j}$, where $P_j⪰0$ (symmetric and positive semi-definite matrix of appropriate dimension), $r_{c,j}$ is the center of the ${\mathcal{E}}_j$ and ${∥u∥}^2⩽1$. Using these notations, the problem of computing the maximum volume inscribed ellipsoid ${\mathcal{E}}_j$ of ${\mathcal{F}}_j$ can be formulated as an SDP:

$$\begin{array}{cc}\hfill \left({\mathcal{L}}_3\right)V_j:=& maxlogdet{P}_j,\hfill \\ \hfill P_j& ⪰0,\hfill \end{array}$$

(4)

$$∥P_{j}u+r_{c,j}-B_i∥⩽\varphi \left(D_{ij}\right),i=1,\dots ,N,\forall \{u:{∥u∥}^2⩽1\}.$$

(5)

The constraints (5) are convex and represent the ellipsoid ${\mathcal{E}}_j$ that is constrained to lie in the intersection of the spheres centered at the beacon locations of radii determined by the range measurements gathered by the jth receiver.

Proposition 1. 

For a fixed value of $i\in \{1,\dots ,N\}$, the set of infinite constraints (5) in formulation ${\mathcal{L}}_3$ is equivalent to the following set of constraints:

$$\begin{array}{c}\hfill \lambda \ge 0,\left[\begin{array}{ccc}\varphi \left(D_{ij}\right)-\lambda & {(r_{c,j}-B_i)}^T& 0\\ (r_{c,j}-B_i)& \varphi \left(D_{ij}\right)I_3& P_j\\ 0& P_j& \lambda I_3\end{array}\right]⪰0.\end{array}$$

(6)

Proof. 

See Appendix A. □

Hence, an equivalent formulation for computing ${\mathcal{E}}_j$ is given by:

$$\begin{array}{c}\left({\mathcal{L}}_4\right)V_j=maxlogdet{P}_j,\\ \left[\begin{array}{ccc}\varphi \left(D_{ij}\right)-\lambda & {(r_{c,j}-B_i)}^T& 0\\ (r_{c,j}-B_i)& \varphi \left(D_{ij}\right)I_3& P_j\\ 0& P_j\lambda I_3\end{array}\right]⪰0,\\ P_j⪰0,\lambda ⩾0.\end{array}$$

3.2. Determination of the Sensing Function $\varphi (·)$

Sources such as sound-speed variability, signal latency, and range-dependent degradation directly manifest as errors in range measurements. In our framework, we unify all these uncertainties into a single bounded disturbance on range measurements. Specifically, we introduce a sensing function $\varphi (·)$, that maps a given range measurement $D_{ij}$ (of an on-board receiver j from beacon i), to an upper bound on the true range value, $d_{ij}$. In other words, ($d_{ij}\le \varphi \left(D_{ij}\right)$) and the sensing function $\varphi (·)$ ensures that the receiver lies within a sphere of radius $\varphi \left(D_{ij}\right)$ centered at beacon i. Here, all systematic biases and noises (clock offsets, multipath, SNR loss, etc.) are absorbed into the non-negative gap, $\varphi \left(D_{ij}\right)-d_{ij}$.

To simulate the decreasing reliability of sensors with an increase in their distance from the receivers, $\varphi (·)$ is chosen to be an increasing function of the measured distance. The ramification of choosing an increasing function is that the range measurements originating from beacons that are sufficiently far away from the vehicle are ignored. In this section, we formulate the problem of finding an increasing function $\varphi (·)$ using the range measurements and true distance data as an SDP. This problem can either be solved offline before the beacons and the on-board receivers are deployed and can be considered as a part of the receiver calibration process or can be adaptively estimated and updated if there are other sensors in place to estimate the position of the vehicle via sensor fusion. We consider a set of true distance data indexed by the set K. For each data point $d_k$, $k\in K$, multiple range measurements are obtained while placing an arbitrary beacon and an on-board receiver $d_k$ units apart. Let $[D_k^l,D_k^u]$ denote the lower and upper bounds of the measurements corresponding to the true distance $d_k$, $k\in K$. Define $D^l:={min}_{k\in K}{D}_k^l$ and $D^u:={max}_{k\in K}{D}_k^u$. The problem now is to determine a univariate increasing function $\varphi :[D^l,D^u]\to {\mathbb{R}}^{+}$ that solves the following optimization problem:

$$min\sum _{k\in K}(\varphi \left(D_k^u\right)-d_k),$$

(7)

$${\varphi }^{\prime }\left(d\right)⩾0,\forall d\in [D^l,D^u],$$

(8)

$$\varphi \left(D_k^l\right)⩾d_k,\forall k\in K.$$

(9)

The constraints (8) enforce the function $\varphi (·)$ to be an increasing function in its domain, and the constraints (9) ensure that for any range measurement $D_{ij}$ between a beacon i and an on-board receiver j, $\varphi \left(D_{ij}\right)$ provides an upper bound on the true distance $d_{ij}$. The objective ensures that this bound obtained is the least upper bound while satisfying the other constraints (this prevents overestimation of the radius of the sphere in which the receiver is guaranteed to lie). To solve the above problem, we approximate $\varphi \left(x\right)$ using a univariate polynomial. The condition (8) is equivalent to the polynomial ${\varphi }^{\prime }(·)$ being non-negative in the interval $[D^l,D^u]$. We now state two known results, using which we recast the non-negativity restrictions on the univariate polynomial to an SDP.

Theorem 1 

(Markov-Lukàcs theorem). Let $a<b$. Then, a univariate polynomial $p\left(x\right)$ is non-negative on $[a,b]$, if and only if it can be written as

$$\begin{array}{cc}& p\left(x\right)=\left\{\begin{array}{cc}s\left(x\right)+(x-a)(b-x)t\left(x\right),\hfill & \mathit{if}degree\left(p\right)\mathit{is}\mathit{even};\hfill \\ (x-a)s\left(x\right)+(b-x)t\left(x\right),\hfill & \mathit{if}degree\left(p\right)\mathit{is}\mathit{odd};\hfill \end{array}\right.\hfill \end{array}$$

where $s\left(x\right)$ and $t\left(x\right)$ are ‘Sum of Squares’ (SOS). In the first case, we have $degree\left(p\right)=2k$, and $degree\left(s\right)\le 2k$, $degree\left(t\right)\le 2k-2$. In the second, $degree\left(p\right)=2k+1$, and $degree\left(s\right)\le 2k$, $degree\left(t\right)\le 2k$.

Proof. 

See [23]. □

Theorem 2. 

Let $p\left(x\right)$ be a univariate polynomial of degree $2k$. Then, $p\left(x\right)$ is SOS if and only if there exists a $(k+1)\times (k+1)$ positive semi-definite matrix P that satisfies

$$p\left(x\right)={\left[x\right]}_k^{T}P{\left[x\right]}_k$$

where, ${\left[x\right]}_k={[1x{x}^2\dots x^k]}^T$.

Proof. 

See [24]. □

Let $\varphi \left(x\right)=a_0+a_{1}x+\cdots +a_n{x}^n$ be an n^th degree polynomial whose coefficients are to be determined. Then, we have

$$\begin{array}{c}\hfill {\varphi }^{\prime }\left(x\right)=a_1+2a_{2}x+\dots n{a}_n{x}^{n-1}=\sum _{i=1}^{n}i{a}_i{x}^{i-1}.\end{array}$$

(10)

The constraint (8) requires ${\varphi }^{\prime }\left(x\right)$ to be non-negative on $[D^l,D^u]$. Suppose that the degree of $\varphi$ is even (the case when $\varphi$ has an odd degree has a similar reduction and hence, is not presented). Then, by Theorem 1, we have

$$\begin{array}{c}\hfill {\varphi }^{\prime }\left(x\right)=(x-D^l)s\left(x\right)+(D^u-x)t\left(x\right)\end{array}$$

(11)

where, $s\left(x\right)$ and $t\left(x\right)$ are SOS. Let $degree\left({\varphi }^{\prime }\right)=2k+1$, then degree of $s\left(x\right)$ and $t\left(x\right)$ is at most $2k$. Then, by Theorem 2, we have

$$\begin{array}{c}\hfill s\left(x\right)={\left[x\right]}_k^{T}S{\left[x\right]}_k,t\left(x\right)={\left[x\right]}_k^{T}T{\left[x\right]}_k\end{array}$$

(12)

where, S and T are $(k+1)\times (k+1)$ positive semi-definite matrices. Combining (10)–(12), we obtain:

$$\begin{array}{c}\hfill \sum _{i=1}^{2k+1}i{a}_i{x}^{i-1}=(x-D^l){\left[x\right]}_k^{T}S{\left[x\right]}_k+(D^u-x){\left[x\right]}_k^{T}T{\left[x\right]}_k.\end{array}$$

Indexing the rows and columns of S and T by $\{0,1,\dots ,k\}$ and equating the coefficients of the Mth power of x on both sides of the above equation, we obtain a set of $(2k+1)$ linear equations relating the coefficients of the polynomial $\varphi$ and then entries of the positive semi-definite matrices S and T as follows:

$$\begin{array}{c}\hfill (M+1)a_{M+1}=\sum _{0⩽i,j⩽k}^{i+j=M-1}\left(Q_{ij}-T_{ij}\right)+\sum _{0⩽i,j⩽k}^{i+j=M}\left(D^u{T}_{ij}-D^l{S}_{ij}\right).\end{array}$$

(13)

Hence, an equivalent SDP for computing the function $\varphi$, which is approximated by a degree n polynomial is given by:

$$\begin{array}{cc}\hfill \left({\mathcal{L}}_5\right)min& \sum _{k\in K}(\varphi \left(D_k^u\right)-d_k),\mathrm{subject}\mathrm{to}:\left(13\right),\left(9\right),\mathrm{and}S,T⪰0.\hfill \end{array}$$

Constraints (13) and the positive semi-definiteness of $S,T$ together are equivalent to enforcing the polynomial $\varphi$ to be increasing; the constraints (13) and (9) are linear constraints.

3.3. Estimation of the Orientation of the Vehicle from the Location Estimates of the On-Board Receivers

Suppose $\mathcal{F}$ is a frame of reference attached to the rigid body with its origin at O and unit vectors $\widehat{i},\widehat{j},\widehat{k}$, respectively. Let the coordinates of the vehicle’s on-board receivers in $\mathcal{F}$ be $w_j=(a_j,b_j,c_j),$$j=1,2,\dots ,L$, respectively, and its estimated location in the ground frame be $r_{c,j}=(x_{c,j},y_{c,j},z_{c,j})$. Let R be the rotation matrix associated with the body describing its orientation. Let $r_0=(x_0,y_0,z_0)$ denote the estimate of the location of the origin O of the body frame $\mathcal{F}$. Then, it is clear that the following rigid body motion constraints must hold when there is no estimation error in the location of the on-board receivers:

$$\begin{array}{c}\hfill r_{c,j}=r_0+R{w}_j,\forall j=1,2,\dots ,L.\end{array}$$

(14)

Essentially, these constraints guarantee that the angles between line segments joining the receivers, as inferred from the location estimates, will remain the same as their true values, and the distance between the receivers as inferred from their locations will remain the same as their true values. Compactly, one can rewrite the above equation as:

$$\begin{array}{c}\hfill \left[\begin{array}{c}{r}_{c,1}\cdots r_{c,L}\end{array}\right]=\left[\begin{array}{c}{r}_0\cdots r_0\end{array}\right]+R\left[\begin{array}{c}{w}_1\cdots w_L\end{array}\right].\end{array}$$

(15)

However, the estimates may not satisfy the above relationship due to errors in measurements and subsequent location estimation of onboard sensors. In particular, the estimate of the distance between the on-board receivers need not equal the actual distance between them. As a consequence, the relative configuration of the on-board receivers indicated by their location estimates will not be the same as the true relative configuration of the receivers. One then needs to correct these location estimates to ensure that the distance between the on-board receivers is its true value. Since the errors in the location estimates will be non-zero, let us define an error matrix, E, as:

$$\begin{array}{c}\hfill E:=\left[\begin{array}{c}{r}_{c,1}\cdots r_{c,L}\end{array}\right]-\left[\begin{array}{c}{r}_0\cdots r_0\end{array}\right]-R\left[\begin{array}{c}{w}_1\cdots w_L\end{array}\right].\end{array}$$

(16)

The problem of localization can now be posed as

$$\begin{array}{c}\hfill \left({\mathcal{L}}_6\right)J=\underset{r_0,R\in SO\left(3\right)}{min}trace\left(E^{T}E\right)\mathrm{subject}\mathrm{to}\left(16\right).\end{array}$$

In the above formulation ${\mathcal{L}}_6$, $trace\left(E^{T}E\right)$ is the square of the Frobenius norm of the error matrix E, and $SO\left(3\right)=\{R:det\left(R\right)=1,R^{-1}=R^T\}$ is referred to as the ‘Special Orthogonal Group’.

4. Algorithms

In this section, we focus on solving the relevant optimization problems from the previous section. In Section 4.1, we will outline a cutting plane algorithm to solve the formulations ${\mathcal{L}}_2$. In Section 4.2, we will provide a solution procedure to solve the problem of localization given by the formulation ${\mathcal{L}}_6$ and thereby determine the optimal orientation that minimizes the square of the Frobenius norm of the error matrix, E. As for the formulation ${\mathcal{L}}_4$ and ${\mathcal{L}}_5$, they can be solved to optimality using off-the-shelf semi-definite solvers like SCS [25].

4.1. Location Estimation Procedure

4.1.1. Algorithm to Estimate the Chebyshev Center

The procedure involves a relaxation of the semi-infinite LP in the formulation ${\mathcal{L}}_2$ to a finite LP by ignoring all but finite constraints and providing an iterative way of adding the required constraints from the dropped set of constraints. This generic procedure is referred to as a cutting plane method (see [21]).

To that end, let $v_1,\dots ,v_M$ be unit vectors representing the M sides of a circumscribing polygon of the feasible region, then a relaxation of ${\mathcal{L}}_2$ is given by

$$\begin{array}{c}\hfill {\overline{l}}_{max}=\underset{l,r_{c,j}}{max}l,\mathrm{subject}\mathrm{to}:v_k·r_{c,j}+l∥v_k∥\le v_k·B_i+\varphi \left(D_{ij}\right),i=1,\dots ,N,k=1,\dots ,M.\end{array}$$

Clearly, the feasible set of this LP, ${\overline{\mathcal{F}}}_j$ contains the feasible set ${\mathcal{F}}_j$ of the original problem as all by finite constraints of the original semi-infinite LP have been dropped. Suppose the optimal solution, $({\overline{l}}_{max},{\overline{r}}_{c,j})$, of the relaxed finite LP satisfies semi-infinite constraints; then, it is clear that $({\overline{l}}_{max},{\overline{r}}_{c,j})$ is optimal for ${\mathcal{L}}_2$. Otherwise, for some unit vector $v_{M+1}$ distinct from those considered before, and for some i, the following inequality holds:

$$v_{M+1}·{\overline{r}}_{c,j}+{\overline{l}}_{max}>v_{M+1}·B_i+\varphi \left(D_{ij}\right).$$

Such a unit vector $v_{M+1}$ can be easily computed by maximizing the function $\left[v·({\overline{r}}_{c,j}-B_i)\right]$ over the unit ball $∥v∥=1$ and checking if the optimum objective is strictly greater than $\varphi \left(D_{ij}\right)-{\overline{l}}_{max}$. The maximization problem can trivially be solved using the Cauchy–Schwarz inequality. By adding the “cut”

$$v_{M+1}·r_{c,j}+l\le v_{M+1}·B_i+\varphi \left(D_{ij}\right),$$

which must be satisfied by the optimal solution for the semi-infinite LP and violated by the previously obtained optimal solution for the finite LP, we improve the solution. This is akin to finding another face of the polygon circumscribing the disk that cuts off a vertex of the previously obtained polygon. This cutting plane method can be used along with an off-the-shelf LP solver to solve the semi-infinite LP to arbitrary accuracy.

4.2. Procedure for Orientation Estimation and Correcting the Location Estimates Taking Rigid Body Constraints into Account

The location estimates of the on-board receivers have been obtained without regard to the rigid body motion constraints between them. Since receivers are attached to the rigid body, the distance between any pair of them is pre-specified. The estimates may not satisfy the distance constraints, and even the angle between line segments joining the receivers computed from their location estimates may not correspond to their true values. For this reason, a correction procedure for the location estimates is required. Fortunately, this pursuit involves the estimation of the orientation of the body.

Let,

$$e_j:=r_{c,j}-r_0-R{w}_j,j=1,\dots ,L.$$

The term $e_j$ describes the error in the estimate of the location to maintain a rigid body constraint for the jth receiver and is a function of the location $r_0$ of the origin of the body frame and the rotation matrix R that describes the orientation of the rigid body. One can observe that $trace\left(E^{T}E\right)={\sum }_{j=1}^L{e}_j^T{e}_j$ and $e_j$ is a linear function of $r_0=(x_0,y_0,z_0)$ and R. Hence, minimization over $r_0,R$ can be performed sequentially since there are no constraints if we explicitly express trace $\left(E^{T}E\right)$ as a function of $r_0,R$. Let

$$\overline{w}={\displaystyle \frac{1}{L}}\sum _{j=1}^L{w}_j,\overline{r}={\displaystyle \frac{1}{L}}\sum _{j=1}^L{r}_{c,j}.$$

Minimization of trace $\left(E^{T}E\right)$ with respect to $r_0$ yields

$$r_0={\displaystyle \frac{1}{L}}\sum _{j=1}^L\left[r_{c,j}-R{w}_j\right]=\overline{r}-R\overline{w}.$$

Define for $j=1,\dots ,L$

$${\tilde{r}}_{c,j}:=r_{c,j}-\overline{r},{\tilde{w}}_j:=w_j-\overline{w}.$$

With these definitions and the optimizing value of $r_0$,

$$e_j={\tilde{r}}_{c,j}-R{\tilde{w}}_j,j=1,\dots ,L.$$

Correspondingly,

$$\begin{array}{c}\hfill trace\left(E^{T}E\right)=\sum _{j=1}^L{({\tilde{r}}_{c,j}-R{\tilde{w}}_j)}^T({\tilde{r}}_{c,j}-R{\tilde{w}}_j)\\ \hfill =\sum _{j=1}^L\left({\tilde{r}}_{c,j}^T{\tilde{r}}_{c,j}+{\tilde{w}}_j^T{\tilde{w}}_j\right)-2trace\left(\left(\sum _{j=1}^L{\tilde{w}}_j{\tilde{r}}_{c,j}^T\right)R\right).\end{array}$$

Define $W:={\sum }_{j=1}^L{\tilde{w}}_j{\tilde{r}}_{c,j}^T$ so that

$$R^{*}=\underset{R\in SO\left(3\right)}{arg\; min}trace\left(E^{T}E\right)=\underset{R\in SO\left(3\right)}{arg\; max}trace\left(WR\right).$$

This stems from the other terms being independent of R.

Let the singular value decomposition of $W=U\mathrm{\Sigma }{V}^T$ where U, V are the left and right singular vectors of W, respectively and $\mathrm{\Sigma }$ is a $3\times 3$ diagonal matrix consisting of its singular values. The problem of maximizing $trace\left(WR\right)$ over the set of all rotation matrices is referred to as the “Orthogonal Procrustes problem” (see [26]).

Theorem 3. 

$R^{*}=V{U}^T$ maximizes $trace\left(WR\right)$ over the set of all proper rotation matrices.

Proof. 

We have due to the property of trace of a product of matrices:

$$\begin{array}{c}\hfill trace\left(WR\right)=trace\left(U\mathrm{\Sigma }{V}^{T}R\right)=trace\left(U{V}^{T}R\mathrm{\Sigma }\right).\end{array}$$

Note that V, R and U are all orthonormal matrices, so $X=U{V}^{T}R$ is also an orthonormal matrix. Suppose $x_{ii}$, $i=1,2,3$ denotes the diagonal elements of X, then

$$\begin{array}{c}\hfill trace\left(WR\right)=trace\left(X\mathrm{\Sigma }\right)=\sum _{i=1}^3{\sigma }_i{x}_{ii}\le \sum _{i=1}^3{\sigma }_i\end{array}$$

where, ${\sigma }_i$, $i=1,2,3$ are the singular values of W. The maximum value in the above equation is achieved when $X=U{V}^{T}R=I$ i.e., $R^{*}=V{U}^T$. □

Remark 1. 

The minimum value of

$$trace\left(E^{T}E\right)=\sum _{j=1}^L\left({\tilde{r}}_{c,j}^T{\tilde{r}}_{c,j}+{\tilde{w}}_j^T{\tilde{w}}_j\right)-2({\sigma }_1+{\sigma }_2+{\sigma }_3).$$

Remark 2. 

The location estimate of the origin of the body frame is $r_0=\overline{r}-R^{*}\overline{w}.$ If one places the on-board receivers in such a way that the “center of mass” of the rigid body (vehicle) coincides with the center of mass of the receivers, then the estimate of $r_0$ is the “corrected” estimate of the location of the center of mass. The error in localization (both in the location estimation and orientation) from the “best” estimates of the locations of the receiver is given by the sum of the singular values of W, a norm referred to as the nuclear norm of W.

Remark 3. 

The updated estimate of the jth receiver’s location will be given by:

$$r_{c,j}=r_0+R^{*}{w}_j.$$

5. Computational and Experimental Results

This section presents numerical simulations and experimental results to corroborate the effectiveness of the algorithms proposed in this article. All the simulations were performed on a Dell Precision Workstation with 12 GB RAM. The simulation setup is as follows: We assume that LBL acoustic beacons are positioned at predetermined locations underwater (by either anchoring them to sea floor or by suspending GIBs from the surface), such that the vehicle’s route is completely contained in the convex hull of the beacons. The beacon locations (assumed to be known a priori) are shown in Figure 1. The AUV to be localized is equipped with four onboard receivers that are fixed at the origin and the three unit vectors of the vehicle reference frame; without loss of generality, the origin of the vehicle reference frame is assumed to be located at the center of mass of the vehicle. Now, given the range measurements of all the onboard receivers from the beacons at various time instances, the problem is to determine the position of the center of mass of the vehicle and its orientation.

5.1. Simulation of Range Measurements

In reality, the range measurements are available from the One Way Travel Time (OWTT) of acoustic signals (with time synchronization), that are transmitted from the beacons and received by the onboard receivers on the vehicle. In the current example, these range values are numerically simulated using the following procedure: the vehicle is assumed to follow a particular trajectory in which the coordinates of the on-board receiver located at the center of mass of the vehicle at any time instant t is given by $\left(2.5\right(1+cost),2.5sint,5sin(t/2\left)\right)$; the trajectory is shown in Figure 1. The other three on-board receivers are assumed to move along the tangent, normal, and bi-normal directions of the curve. These directions are unique at each instant of time and are calculated using the Frenet–Serret equations. The reason for choosing such a complex trajectory, which involves rotation along all three axes, is to showcase the robustness of the algorithm in estimating both the vehicle’s orientation and location. The range measurements between each beacon–receiver pair are then generated by adding a white Gaussian noise with mean 0 and variance $0.25$, to the true distances. 100 such measurements for each beacon–receiver pair are generated as the vehicle travels along its path. Using these range measurements the estimate of the position of each on-board receiver is computed using the algorithm presented in Section 4.1. The estimate of the position of the vehicle is then provided by the position estimate of the on-board receiver located at the center of mass of the vehicle. In the following section, we shall analyze the performance of the position estimation algorithms.

Figure 1. Illustration of the beacon positions and the simulated vehicle trajectory, lying within the convex hull of the beacons, used for numerical experiments.

Figure 1. Illustration of the beacon positions and the simulated vehicle trajectory, lying within the convex hull of the beacons, used for numerical experiments.

5.2. Position Estimation Algorithm Performance

$\varphi (·)$ enables the conversion of range measurements obtained using each beacon to the radii of the spheres centered on the beacons and that contain the location of the on-board receiver. For the purpose of this section, we assume that the function $\varphi (·)$ is computed offline. The intersection of all the spheres is the set of all possible receiver locations. For the purpose of analyzing the performance of the position estimation algorithms, we concern ourselves only with the position of the onboard receiver located at the center of mass of the vehicle. The Chebyshev center of the feasible region is computed using the cutting-plane algorithm that is proposed in Section 4.1.1 using an LP for each set of range measurements and the center of the maximum volume ellipsoid that can be inscribed in this feasible region is computed directly by solving an SDP, derived in Section 3.1.2, using SCS [25]—an off-the-shelf commercial SDP solver.

The average number of cuts, over all the 100 runs, added by the cutting-plane algorithm to compute the Chebyshev center was 29 and the average computation time was $0.0079$ s. The Figure 2 shows the error, as a percentage of the maximum range measurement, in the position estimate of the center of the mass of the vehicle using the cutting plane algorithm for the Chebyshev center. The average error was found to be $1.55\%$ and the maximum error was $5.73\%$.

As for the position estimate computed as the maximum volume ellipsoid center, the average computation time was found to be $0.14$ s. The Figure 3 shows the percentage error in the position estimate of the center of mass of the vehicle obtained by solving the SDP for the maximum volume ellipsoid center. The average error was found to be around $1.42\%$ and the maximum error was found to be $3.25\%$.

Though the time taken for computing the Chebyshev center is at least 10 times less than the maximum volume ellipsoid center, the error performance for the maximum volume ellipsoid center based position estimate is better and hence, throughout the rest of the article, we will use the maximum volume ellipsoid center as the estimate for the position of an on-board receiver.

Next, we analyze the dependence of the Maximum Volume Ellipsoid (MVE) position estimates on the measurement noise. For this study, we navigate the vehicle in two different trajectories, shown in Figure 1 and Figure 4. For each trajectory, we first compute the true range values. Then, we generate the measured ranges by adding white Gaussian noise to the true ranges. To perform a sweeping study, we start with a variance of 1% of the maximum true range and gradually increase it to 10%. We select an arbitrary calibration function, and for each chosen variance, we compute the MVE position estimates as described above. We plot the mean % error in the estimates against the variance (as a percentage) for both the trajectories in Figure 5.

Observe from the figure that for both the trajectories, the mean error remains below 5%, highlighting the robustness of the method. The similar error profiles for the trajectories underline the pivotal role of calibration. We discuss the determination of the calibration function, $\varphi ·$ in the next subsection.

5.3. Determination of $\varphi (·)$

The sensing model, as detailed in Section 3.2, assumes that the receiver always lies in the intersection of spheres centered at the beacons from which the range measurements corresponding to the receiver are obtained. For the purpose of simulation, we assume that the function $\varphi (·)$ is a polynomial of degree 4, i.e., $\varphi \left(x\right)=a_0+a_{1}x+a_2{x}^2+a_3{x}^3+a_4{x}^4\Rightarrow {\varphi }^{\prime }\left(x\right)=a_1+a_{2}x+a_3{x}^2+a_4{x}^3$. For the computation of the coefficients, data sets containing plausible true range values and multiple noisy range measurements corresponding to each true value are required. Experimentally, these data sets can be obtained by placing the receivers at different distances and recording the beacon range measurements multiple times. For simulation, these values were uniformly chosen from the interval $[d-\epsilon ,d+\epsilon ]$, where d is a true range value. The permissible true distance values are restricted to a certain interval based on the beacon locations; the true range values were restricted to the interval $[4,18]$ units. 25 values uniformly distributed in this interval are picked to form the set of true range values, and for each element in the set, 100 corresponding range measurements from the interval $[d-\epsilon ,d+\epsilon ]$ are chosen; this simulation procedure is set up to mimic the error present in actuality. Once the data set is available, the coefficients of the fitting polynomial can be obtained as a solution of an SDP, as discussed in Section 3.2.

Using the results in Section 3.2, the constraint (8) for the fourth degree polynomial can be re-written as ${\left(A\left(x\right)\right)}^2(x-D^l)+{\left(B\left(x\right)\right)}^2(D^u-x)⩾0$, where $A\left(x\right)={\left[x\right]}_k^{T}P{\left[x\right]}_k,B\left(x\right)={\left[x\right]}_k^{T}Q{\left[x\right]}_k,{\left[x\right]}_k={[1x{x}^2\dots x^k]}^T$, $P=\left[\begin{array}{cc}{p}_{11}& p_{12}\\ p_{12}& p_{22}\end{array}\right]⪰0,$ and $Q=\left[\begin{array}{cc}{q}_{11}& q_{12}\\ q_{12}& q_{22}\end{array}\right]⪰0$. Expanding the constraint yields the following equations:

$$a_1=-D^l{p}_{11}+D^u{q}_{11}$$

$$2a_2=p_{11}-2D^l{p}_{12}+2D^u{q}_{12}-q_{11}$$

$$3a_3=2P_{12}-D^l{p}_{22}+D^u{q}_{22}-2q_{12}$$

$$4a_4=p_{22}-q_{22}.$$

Furthermore, the constraint (9) for the fourth degree polynomial $\varphi (·)$ can be written as

$$a_0+a_1{D}_k^l+a_2{\left(D_k^l\right)}^2+a_3{\left(D_k^l\right)}^3+a_4{\left(D_k^l\right)}^4⩾d_k,\forall k\in K,$$

and the objective function is equivalent to minimizing the function

$$\sum _{k\in K}(a_0+a_1{D}_k^u+a_2{\left(D_k^u\right)}^2+a_3{\left(D_k^u\right)}^3+a_4{\left(D_k^u\right)}^4-d_k).$$

For the simulation set discussed above, solving the SDP using SCS as an off-the-shelf commercial solver yields a solution. The computation time is in the order of milliseconds, implying that this computation can be performed online. The error performance for the position estimation algorithm using this $\varphi (·)$ function that was computed online is shown in Figure 2 and Figure 3 for the Chebyshev center and the maximum volume ellipsoid center, respectively.

The function $\varphi (·)$ can also be computed online, adaptively, in the absence of multiple range measurements for a true range value using the technique of sensor fusion. This technique usually requires other sensors in place to estimate the position of the onboard receiver of the vehicle (say GPS); the position estimates from these other sensors can be used as data sets to compute the function $\varphi (·)$. In this case, the position estimation algorithm would either result in a huge error initially which improves over time, or would be infeasible; when either of the two situations occurs, the function $\varphi (·)$ can be recomputed using the values from the alternate sensory equipment in place as the true position estimate and the noise-contaminated range measurement as the reported range measurement. The position estimation algorithm becomes infeasible if the range measurements do not lie within the calibration limits $[D^l,D^u]$ in (8). If the algorithm is unable to estimate the position at a particular instant, it uses the next best available estimate (say GPS) at the same instant to recalibrate itself. Then, the algorithm continues to give estimates till it becomes infeasible again, and the process continues. The Figure 6 shows the error performance of the algorithm with and without online re-computation of the function. Initially, the errors are equal because up to data point 42, the function $\varphi (·)$ did not change. Once infeasibility occurs, the online re-computation process changes $\varphi (·)$ and improves the error performance. The average percent error ignoring the points of infeasibility was observed to be $5.13\%$ when the function $\varphi (·)$ was recomputed online.

5.4. Errors in the Orientation Estimates

The receiver locations obtained from either the Chebyshev or maximum volume ellipsoid center estimates aid in estimating the positions of all the 4 on-board receivers; one at the center of mass of the vehicle and the other three along the unit vectors of the vehicle reference frame at unit distance from the origin of the frame. However, these estimates need not conform to the rigid body constraints of the vehicle. Hence, these estimates are corrected using an orientation-preserving rotation. This rotation is a proper orthogonal rotation computed via the solution to the Orthogonal Procrustes problem detailed in Section 4.2. This rotation also provides an estimate of the orientation of the vehicle. At each time step, the receiver locations were updated and the vehicle orientation is obtained as a rotation matrix $R^{†}$ using a singular value decomposition, as detailed in Section 4.2. Furthermore, in simulation, since we know the actual path the vehicle takes, we also know the actual orientation of the vehicle R. The difference between the two rotations is then computed as the geodesic distance between $R^{†}$ and R given by

$$d(R,R^{†})={∥log\left(R^T{R}^{†}\right)∥}_F$$

in $SO\left(3\right)$, where ${\parallel ·\parallel }_F$ denotes the Forbenius norm (see Section 4.2); logarithm of a matrix is another matrix such that the matrix exponential of the latter matrix equals the original matrix. This distance is measured in radians. The Figure 7 and Figure 8 show the errors in the orientation estimates when the Chebyshev center and the maximum volume center are used for position estimation, respectively; the average errors in degrees were $2.68$ and $1.87$, respectively. In the next section, we present experimental results for the algorithms presented in this paper.

5.5. Experimental Results

To demonstrate the performance of the proposed localization framework in real-world conditions, we conducted field experiments at the Underwater Systems and Technology Laboratory (LSTS), University of Porto, Portugal.

5.5.1. Experimental Setup

We equipped a Light Autonomous Underwater Vehicle (LAUV) with three onboard acoustic receivers and maneuvered it within a long-baseline (LBL) network consisting of four acoustic transponders (beacons). Three of the beacons were moored to the seafloor, and one was suspended from a surface vessel, which also served as the base station. We deployed a CTD (Conductivity–Temperature–Depth) profiler to record environmental parameters during the experiments and monitored the mission in real time from the base station using Neptus, the LSTS command-and-control and visualization software. To validate the ground truth, we also deployed a Remotely Operated AUV (ROV-AUV) equipped with an external localization system to cross-check the estimated vehicle locations. Figure 9 illustrates the experimental setup.

5.5.2. Environmental Conditions

The LAUV operated at depths ranging from 10 m to 20 m, as reported by the onboard depth sensor. The water column exhibited mild stratification, with temperature values ranging from approximately 15 °C in deeper layers to 24.5 °C near the surface. These conditions yielded stable sound-speed values between 1509.2 m/s and 1510.3 m/s.

5.5.3. Mission Execution and Data Collection

We programmed the LAUV to follow a closed-loop trajectory, which it repeated ten times. In Figure 10, we highlight the portion of the trajectory lying within the convex hull of the beacons—the region relevant to our localization framework—using green dots.

During the mission, the beacons emitted acoustic signals at regular intervals, enabling them to measure ranges to the onboard receivers based on the one-way travel times (OWTT). These measurements naturally incorporate several sources of error, including sensor noise, range-dependent signal degradation, beacon-position drift due to mooring slack, multipath reflections, and timing latency. Our calibration map, $\varphi (·)$, is specifically designed to upper-bound the combined effect of these disturbances.

5.5.4. Estimation and Results

We used the range measurements from nine of the ten LAUV runs to learn the calibration function $\varphi (·)$. Then, using the learned function and range measurements for the tenth run, we estimate the vehicle’s location at every instance using the Maximum Volume Ellipsoid method proposed in the work by solving the SDP described by ${\mathcal{L}}_4$. We plot the estimated vehicle positions as blue dots in Figure 10. The estimates show good agreement with the expected trajectory, with the exception of a few deviations.

To enable scale-invariant evaluation of localization accuracy, we report the Euclidean distance between the estimated and ground-truth positions as a percentage of the largest range measurement recorded during the experiment. Figure 11 depicts the error over the evaluation run. The average and maximum errors are 1.98% and 5.26%, respectively. The largest deviations are likely due to beacon drift caused by mooring slack and surface wave motion. We explicitly address this effect in Appendix B, where we present a modified formulation that accounts for beacon-position uncertainty using a robust convex model.

For comparison, we also computed position estimates using an arbitrary, non-calibrated version of $\varphi (·)$; these are shown in red in Figure 10. The resulting estimates exhibit significantly larger errors and inconsistent trajectory alignment. This comparison serves as a baseline and highlights the critical importance of solving the proposed SDP to learn a well-calibrated $\varphi (·)$. Since orientation estimation relies on accurate position estimates, this calibration step directly impacts the final 6-DOF pose accuracy.

6. Conclusions

In this article, we have developed an infrastructure-based position and orientation estimation scheme for underwater vehicles, using range measurements obtained from acoustic LBL beacons. The scheme employs convex optimization techniques to estimate the location of the vehicle, which is assumed to travel in the convex hull of the beacons. The highlights of the algorithm involve a robust sensing model that accounts for various uncertainties arising in the underwater environment, such as volatility of sound speed in water, high latency, and data losses, etc. The sensing function also accounts for the decreasing reliability of sensors with an increase in their distance from the vehicle and can be computed by solving a semidefinite program. Recent advancements in SDP solvers enable adaptive online computation of this sensing function. The position estimates obtained using this scheme are unique and invariant to scaling. This method not only provides bounded-error estimates but also provides a measure of the uncertainty of the estimates. A simple extension to the algorithm also provides orientation estimates of the vehicle. These are obtained as a solution to the “Orthogonal Procrustes” problem using singular value decomposition. The effectiveness of the algorithm is validated with experimental and simulation results. Additionally, an extension of the algorithm considering the continuous change in beacon locations due to water currents in provided in Appendix B. Applications of this algorithm include offshore drilling, installation of pipes in an underwater region, etc. Even though this algorithm is developed for underwater vehicles, it is universal and can also be applied to terrestrial vehicles. Future work can be focused on relaxing the assumption that the vehicle moves in the convex hull of the beacons and accounting for conical uncertainty in beacon locations.