Optimal Beacon Placement for Self-Localization Using Three Beacon Bearings

Abstract

Autonomous vehicles need to localize themselves within the environment in order to effectively perform most tasks. In situations where a Global Navigation Satellite System such as the Global Positioning System cannot be used for localization, other methods are required. One self-localization method is to use signals transmitted by beacons at known locations to determine the relative distance and bearing of the vehicle from the beacons. Estimation performance is influenced by the beacon–vehicle geometry and the investigation into the optimal placement of beacons is of interest to maximize the estimation performance. In this article, a new solution to the optimal beacon placement problem for self-localization of a vehicle on a two-dimensional plane using angle-of-arrival measurements is proposed. The inclusion of heading angle in the estimation problem differentiates this work from angle-of-arrival target localization, making the optimization problem more difficult to solve. First, an expression of the determinant of the Fisher information matrix for an arbitrary number of beacons is provided. Then, a procedure for analytically determining the optimal angular separations for the case of three beacons is presented. The use of three beacons is motivated by practical considerations. Numerical simulations are used to demonstrate the optimality of the proposed method.

1. Introduction

Autonomous vehicles usually require accurate estimation of their position and heading in a global coordinate system to effectively accomplish mission objectives. Global Navigation Satellite Systems (GNSS) such as the Global Positioning System (GPS) are widely used for localization. GNSS is not a viable localization solution in situations where line-of-sight communication with the navigation satellites is interrupted (e.g., indoor operation, or when the satellite signals are likely to be jammed). GNSS is also unsuitable for use in low-power systems that lack sufficient hardware capability [1]. When GNSS is impractical, an alternate localization approach is necessary, such as using signals transmitted by a set of beacons to determine relative distances and bearings. An example of localization using beacon measurements is angle-of-arrival (AoA) localization. AoA localization is a technique for determining the position of an emitter by triangulating bearing measurements received from multiple sensors. Bearing measurements can be acquired passively and can provide distance and orientation information [2,3], which makes them useful in applications such as self-localization. For self-localization, a vehicle uses AoA measurements from multiple transmitters to estimate its position and heading. The relative placement of the receiver and transmitters affects AoA estimation accuracy [4]. Investigating geometries that yield the best estimation accuracy provides useful insight into the optimal path an Unmanned Aerial Vehicle (UAV) should take in an optimal path-planning application [5].

For the AoA self-localization problem, the objective is to place the beacons to produce an optimal estimate of the two-dimensional (2D) position and heading of a vehicle from the measured beacon bearings (see, e.g., [6,7]). The optimal beacon placement is realized by positioning beacons at fixed but arbitrary distances from the UAV and selecting the beacon angular separations that minimize estimation uncertainty. Including the vehicle heading into the optimization problem greatly increases the difficulty of finding an analytical solution when compared to AoA target location estimation. We propose a new solution to the optimal placement of three beacons for self-localization using AoA measurements. Three beacons are the minimum sized constellation required for complete 2D localization [8], and thus the least expensive and the easiest deployed. It is desirable to limit the number of beacons used in many applications, such as underwater self-location using an acoustic positioning system.

Acoustic positioning systems such as the Long Baseline system (LBL) are an alternative to GNSS technology for self-location in underwater environments. GNSS technology is unsuitable for this application as electromagnetic signals from the satellites’ experience strong attenuation in water. LBL schemes use acoustic transponders placed on the seabed that act as beacons to self-locate [9]. These systems have been designed to work with as few as a single beacon to reduce the logistics associated with deploying more beacons [9,10]. Minimizing the number of localization beacons allows for energy conservation in communication and reduces computational complexity [11], motivating the use of three beacons.

For RF self-localization, the beacons operate at different frequencies. Including more beacons in the self-localization architecture requires a larger receiver bandwidth, resulting in increased hardware cost. The cost of including extra beacons is substantial if mobile beacons are considered. Providing mobility to beacons requires mounting them on vehicles with GNSS capability and the power capacity to provide transmission [12]. There are restrictions on the number of vehicles available to operate as a mobile beacon in resource-constrained applications. In these applications, it is useful to know the optimal configuration that uses the minimum amount of resources. When many beacons are present, the algorithm can be used to evaluate the subset of beacons that are closest to an optimal placement or require the lowest cost to configure optimally. In summary, the motivation for using three beacons is as follows. Using the minimum number of beacons for self-localization leads to reduced costs, placement logistics, computational complexity, and energy required for communication.

To the best of our knowledge, no optimal beacon geometry analysis for position and orientation self-localization using AoA measurements has been hitherto reported in the open literature.

In this article, we present an analytical solution to the optimal beacon placement problem for self-localization of a vehicle on a two-dimensional plane using angle-of-arrival measurements.

The key contributions of the study are:

• A simplified expression for the determinant of the FIM for vehicle self-localization using AoA measurements for an arbitrary number of beacons.
• An analytical method for calculating angular separations between beacons that satisfy the D-optimality criterion when three beacons are used.
• A mathematical proof that our solution satisfies the sufficient and necessary conditions for optimality.
• Simulations that confirm the optimality of the proposed approach.

The rest of this article is organized as follows. Related work is discussed in Section 2. The problem is introduced, and the determinant of the FIM is expressed in Section 3. Analysis of MSE and the FIM is undertaken in Section 4, including discussion of the complexity of the optimization problem and the introduction of several different forms for the objective function. The specific case of three beacons and one vehicle is introduced, and an analytical solution is formulated in Section 5. The results of numerical simulations used to verify the optimality of the proposed approach are presented in Section 6, and a conclusion is presented in Section 7.

2. Related Work

Placing beacons to minimize self-localization uncertainty is similar to placing sensors for minimizing target location estimation error. For target location estimation, multiple sensors are placed rather than beacons, and only position estimation uncertainty is minimized. In both applications, optimal geometries are realized by placing sensors or beacons such that some scalar objective function is either maximized or minimized [13]. The same process can be used to determine optimal trajectories for mobile sensors and beacons [5]. The objective functions are selected such that their optimization results in estimation error minimization, and they are usually based on the Cramér–Rao lower bound (CRLB) of the estimator or the Fisher information matrix (FIM). Popular objective functions include the D-optimality criterion and the A-optimality criterion.

The D-optimality criterion maximizes the determinant of the FIM and thus minimizes the volume of the confidence ellipsoid for the localization estimates [13]. The D-optimality criterion has been used in stationary bearings-only target localization [4,5,14], and to develop trajectories for localizing stationary targets [15,16] and maneuvering targets [17,18,19]. The A-optimality criterion minimizes the trace of the inverse FIM, which minimizes the mean squared error (MSE) of the estimates. The A-optimality criterion has been used to determine optimal flight paths for multiple UAVs using AoA measurements to localize a stationary target [20,21] or a moving target [22,23,24], and for 3D AoA target localization [25,26]. The diversity of the eigenvalues of the FIM has been used as an alternative criterion for the optimal placement of sensors for target position estimation [27]. In two dimensions, it produces the same result as using the D-optimality criterion.

An advantage of the D-optimality criterion over the A-optimality criterion is that it does not depend on the scale of the variables [28,29]. This advantage is beneficial for the application presented as the vehicle states are position and heading. The optimal geometries produced using the A-optimality criterion would be dependent on the units of measurement, unlike those produced using D-optimality.

3. Problem Definition

Consider the self-localization geometry in Figure 1. The objective is to determine the relationship between the beacon bearings in the global coordinates $\varphi +{\theta }_1$, $\varphi +{\theta }_2$, …, $\varphi +{\theta }_N$ to optimize the estimates of $p$ and $\varphi$ for given and fixed beacon distances from the platform $d_i=\left|\right|p-r_i\left|\right|$, $i=1,2,\dots ,N$ for $N\ge 3$. Here, $\varphi$ is the vehicle heading, $p$ is the vehicle position, ${\theta }_i$ is the relative bearing angle between the vehicle heading, and the ith beacon and $r_i$ is the position of the ith beacon.

The position and heading estimation problem is formulated as an optimization problem. The optimization criterion adopted for this work is the D-optimality criterion, chosen because it is invariant under scale changes in the parameters [28]. Assuming that the beacon bearing measurements are corrupted by i.i.d. zero-mean Gaussian bearing noise with covariance $\Sigma =diag({\sigma }_1^2,{\sigma }_2^2,\dots ,{\sigma }_N^2)$, the FIM is given by

$$\mathsf{\Phi }={\mathit{J}}_0^{\top }{\Sigma }^{-1}{\mathit{J}}_0,$$

(1)

where ${\mathit{J}}_0$ is the Jacobian matrix:

$${\mathit{J}}_0=\left[\begin{array}{ccc}\frac{sin({\theta }_1+\varphi )}{\left|\right|p-r_1\left|\right|}& -\frac{cos({\theta }_1+\varphi )}{\left|\right|p-r_1\left|\right|}& -1\\ \frac{sin({\theta }_2+\varphi )}{\left|\right|p-r_2\left|\right|}& -\frac{cos({\theta }_2+\varphi )}{\left|\right|p-r_2\left|\right|}& -1\\ ⋮& ⋮& ⋮\\ \frac{sin({\theta }_N+\varphi )}{\left|\right|p-r_N\left|\right|}& -\frac{cos({\theta }_N+\varphi )}{\left|\right|p-r_N\left|\right|}& -1\end{array}\right].$$

(2)

If all of the beacons and the vehicle are co-linear or co-circular, no unique estimate exists, making the Cramér–Rao lower bound CRLB (inverse of FIM) tend to infinity and the determinant of the FIM to zero [7,8,30]. In block matrix form, the FIM can be written as

$$\Phi =\left[\begin{array}{cc}\mathit{A}& {\displaystyle b}\\ {{\displaystyle b}}^{\top }& c\end{array}\right],$$

(3)

where

$$\mathit{A}=\left[\begin{array}{cc}{\displaystyle \sum _{i=1}^N}\frac{1}{{\gamma }_i}sin{\left({\vartheta }_i\right)}^2& -{\displaystyle \sum _{i=1}^N}\frac{1}{{\gamma }_i}sin\left({\vartheta }_i\right)cos\left({\vartheta }_i\right)\\ -{\displaystyle \sum _{i=1}^N}\frac{1}{{\gamma }_i}sin\left({\vartheta }_i\right)cos\left({\vartheta }_i\right)& {\displaystyle \sum _{i=1}^N}\frac{1}{{\gamma }_i}cos{\left({\vartheta }_i\right)}^2\end{array}\right],$$

$${\displaystyle b}=\left[\begin{array}{c}-{\displaystyle \sum _{i=1}^N}\frac{d_i}{{\gamma }_i}sin\left({\vartheta }_i\right){\displaystyle \sum _{i=1}^N}\frac{d_i}{{\gamma }_i}cos\left({\vartheta }_i\right)\end{array}\right],c={\displaystyle \sum _{i=1}^N}\frac{1}{{\sigma }_i^2},{\gamma }_i={\sigma }_i^2{d}_i^2,{\vartheta }_i={\theta }_i+\varphi .$$

(4)

The D-optimality criterion can be used to place beacons for optimal self-localization. The D-optimality criterion selects the beacon angular positions that maximize the determinant of the FIM. Maximizing the determinant of the FIM minimizes the volume of the confidence ellipsoid for the self-localization estimate. The D-optimality criterion is

$$\underset{{\vartheta }_1,{\vartheta }_2,\dots ,{\vartheta }_N}{max}|\mathsf{\Phi }|,$$

(5)

where $|\mathsf{\Phi }|$ denotes the determinant of $\mathsf{\Phi }$. Since the FIM is a block matrix, its determinant can be expressed as [31]

$$|\mathsf{\Phi }|=c^{-1}|\mathit{A}c-{\displaystyle b}{{\displaystyle b}}^{\top }|.$$

(6)

This optimization problem is nontrivial because $|\mathsf{\Phi }|$ has a rather complicated expression involving the multiplication of sums of sines and cosines. In the following section, a simplification of the expression for $|\mathsf{\Phi }|$ for the three-beacon case is presented, followed by a generalization to any number of beacons. For simplicity, we assume all the bearing measurements have the same variance, i.e., ${\sigma }_i^2={\sigma }^2$, $i=1,\dots ,N$.

4. Analysis of Mean Square Error and Determinant of Fisher Information Matrix

The CRLB is the inverse of the FIM:

$$\frac{1}{|\mathsf{\Phi }|}\left[\begin{array}{ccc}{A}_{22}c-b_2^2& -A_{12}c+b_1^2& A_{12}{b}_2-A_{22}{b}_1\\ -A_{21}c+b_2{b}_1& A_{11}c-b_1^2& -A_{11}{b}_2+A_{21}{b}_1\\ A_{21}{b}_2-A_{22}{b}_1& -A_{11}{b}_2+A_{12}{b}_1& A_{11}{A}_{22}-A_{12}{A}_{21}\end{array}\right],$$

where $A_{ij}$ is the $\left\{ij\right\}$th entry of $\mathit{A}$ and $A_{21}$ = $A_{12}$. The MSE is given by the trace of the CRLB [5]:

$$\begin{array}{cc}\hfill \frac{A_{22}c-b_2^2+A_{11}c-b_1^2+A_{11}{A}_{22}-A_{12}{A}_{21}}{|\mathsf{\Phi }|}& =\frac{1}{|\mathsf{\Phi }|{\sigma }^4}\{N{\displaystyle \sum _{i=1}^N}\frac{1}{d_i^2}-{\left({\displaystyle \sum _{i=1}^N}\frac{sin\left({\vartheta }_i\right)}{d_i}\right)}^2\hfill \\ \hfill -{\left({\displaystyle \sum _{i=1}^N}\frac{cos\left({\vartheta }_i\right)}{d_i}\right)}^2+\frac{1}{4}{\left({\displaystyle \sum _{i=1}^N}\frac{1}{d_i^2}\right)}^2& -\frac{1}{4}{\left({\displaystyle \sum _{i=1}^N}\frac{sin\left(2{\vartheta }_i\right)}{d_i^2}\right)}^2-\frac{1}{4}{\left({\displaystyle \sum _{i=1}^N}\frac{cos\left(2{\vartheta }_i\right)}{d_i^2}\right)}^2\}.\hfill \end{array}$$

(7)

It is evident from the above expression that the beacon geometry that maximizes $|\mathsf{\Phi }|$ does not necessarily minimize the MSE, unlike the case for the bearings-only localization problem [5]. Therefore, the A-optimality criterion (which minimizes the MSE directly) and the D-optimality criterion adopted here will produce different optimal geometries. Turning our attention to the D-optimality criterion, the expression for $|\mathsf{\Phi }|$ for the case of $N=3$ can be simplified to

$$|\mathsf{\Phi }|={\left(\frac{sin({\vartheta }_2-{\vartheta }_3)}{{\sigma }^3{d}_2{d}_3}+\frac{sin({\vartheta }_1-{\vartheta }_2)}{{\sigma }^3{d}_1{d}_2}+\frac{sin({\vartheta }_3-{\vartheta }_1)}{{\sigma }^3{d}_1{d}_3}\right)}^2,$$

(8)

the optimization problem becomes

$$\underset{{\vartheta }_1,{\vartheta }_2,{\vartheta }_3}{max}{\left(\frac{sin({\vartheta }_2-{\vartheta }_3)}{d_2{d}_3}+\frac{sin({\vartheta }_1-{\vartheta }_2)}{d_1{d}_2}+\frac{sin({\vartheta }_3-{\vartheta }_1)}{d_1{d}_3}\right)}^2.$$

(9)

An analytical solution to this optimization problem will be developed in Section 5. An expression for the determinant can be found for an arbitrary number of beacons using the same simplification techniques to derive (8). When $N\ge 3$, the determinant has as many square terms in the form of (8) as the number of unique combinations of three beacons, i.e., $\frac{N!}{3!(N-3)!}=\frac{N^3-3N^2+2N}{6}$ terms:

$$\sum _{\begin{array}{c}k<l<m\end{array}}{\left(\frac{sin({\vartheta }_l-{\vartheta }_m)}{{\sigma }^3{d}_l{d}_m}+\frac{sin({\vartheta }_m-{\vartheta }_k)}{{\sigma }^3{d}_m{d}_k}+\frac{sin({\vartheta }_k-{\vartheta }_l)}{{\sigma }^3{d}_k{d}_l}\right)}^2,$$

(10)

where $k,l,m\in \{1,\dots ,N\}$, and $d_k,d_l,d_m$ and ${\vartheta }_k,{\vartheta }_l,{\vartheta }_m$ are the beacon distances and angles, respectively. This expression is only practical for a small number of beacons as the number of terms grows in the order of $N^3$ with N. Using the block matrix form in (6), and defining $x_i=\frac{1}{d_i}cos\left({\vartheta }_i\right)$ and $y_i=\frac{1}{d_i}sin\left({\vartheta }_i\right)$, the expression for $|\mathsf{\Phi }|$ in (10) can be expressed more compactly as

$$|\mathsf{\Phi }|=\frac{1}{{\sigma }^{6}N}det\left(\left[\begin{array}{cc}{\Sigma }_{11}& {\Sigma }_{12}\\ {\Sigma }_{12}& {\Sigma }_{22}\end{array}\right]\right),$$

(11)

where

$$\begin{array}{cc}\hfill {\Sigma }_{11}& =N{\displaystyle \sum _{i=1}^N}{y}_i^2-{\left({\displaystyle \sum _{i=1}^N}{y}_i\right)}^2,{\Sigma }_{12}=-N{\displaystyle \sum _{i=1}^N}{y}_i{x}_i+\left({\displaystyle \sum _{i=1}^N}{y}_i\right)\left({\displaystyle \sum _i^N}{x}_i\right),\hfill \\ \hfill {\Sigma }_{22}& =N{\displaystyle \sum _{i=1}^N}{x}_i^2-{\left({\displaystyle \sum _{i=1}^N}{x}_i\right)}^2.\hfill \end{array}$$

The difficulty in finding an analytical solution is mainly due to the presence of terms consisting of sums of squares and squares of sums. Observing

$$\begin{array}{c}{\displaystyle \sum _{i=1}^N}{x}_i^2-\frac{1}{N}{\left({\displaystyle \sum _{i=1}^N}{x}_i\right)}^2={\displaystyle \sum _{i=1}^N}{(x_i-\overline{x})}^2,\hfill \\ {\displaystyle \sum _{i=1}^N}{y}_i^2-\frac{1}{N}{\left({\displaystyle \sum _{i=1}^N}{y}_i\right)}^2={\displaystyle \sum _{i=1}^N}{(y_i-\overline{y})}^2,\hfill \end{array}$$

where $\overline{x}=\frac{1}{N}{\displaystyle \sum _{i=1}^N}{x}_i$ and $\overline{y}=\frac{1}{N}{\displaystyle \sum _{i=1}^N}{y}_i$, (11) can be rewritten as

$$\begin{array}{cc}\hfill |\mathsf{\Phi }|& =\frac{N}{{\sigma }^6}\left|\begin{array}{cc}{\displaystyle \sum _{i=1}^N}{\left(y_i-\overline{y}\right)}^2& -{\displaystyle \sum _{i=1}^N}\left(y_i-\overline{y}\right)\left(x_i-\overline{x}\right)\\ -{\displaystyle \sum _{i=1}^N}\left(y_i-\overline{y}\right)\left(x_i-\overline{x}\right)& {\displaystyle \sum _{i=1}^N}{\left(x_i-\overline{x}\right)}^2\end{array}\right|\hfill \end{array}$$

(12)

$$\begin{array}{cc}\hfill & =\frac{N}{{\sigma }^6}\left\{\left({\displaystyle \sum _{i=1}^N}{\left(y_i-\overline{y}\right)}^2\right)\left({\displaystyle \sum _{i=1}^N}{\left(x_i-\overline{x}\right)}^2\right)-{\left({\displaystyle \sum _{i=1}^N}\left(y_i-\overline{y}\right)\left(x_i-\overline{x}\right)\right)}^2\right\}.\hfill \end{array}$$

(13)

5. Three Beacons and One Vehicle

In this section, we derive an analytical solution for the optimal geometry when $N=3$ using (8) and (9). Assuming the beacons are at arbitrary but fixed distances from the vehicle, we first determine the stationary points of $|\mathsf{\Phi }|$ by setting its gradient to zero:

$$\begin{array}{c}\frac{\partial }{\partial \mathit{\vartheta }}|\mathsf{\Phi }\left({\displaystyle \mathit{\vartheta }}\right)|=2P\left[\begin{array}{c}\frac{cos({\vartheta }_1-{\vartheta }_3)}{d_1{d}_3}-\frac{cos({\vartheta }_1-{\vartheta }_2)}{d_1{d}_2}\\ \frac{cos({\vartheta }_2-{\vartheta }_3)}{d_2{d}_3}-\frac{cos({\vartheta }_1-{\vartheta }_2)}{d_1{d}_2}\\ \frac{cos({\vartheta }_2-{\vartheta }_3)}{d_2{d}_3}-\frac{cos({\vartheta }_1-{\vartheta }_3)}{d_1{d}_3}\end{array}\right]={\displaystyle 0},\end{array}$$

(14)

where $\mathit{\vartheta }={[{\vartheta }_1,{\vartheta }_2,{\vartheta }_3]}^{\top }$ and $P=\frac{sin({\vartheta }_2-{\vartheta }_3)}{d_2{d}_3}+\frac{sin({\vartheta }_1-{\vartheta }_2)}{d_2{d}_1}+\frac{sin({\vartheta }_1-{\vartheta }_3)}{d_1{d}_3}$. Referring to (8), it is clear that any angle combination for which $P=0$ will make $|\mathsf{\Phi }|=0$ and thus corresponds to a minimum. This implies the solutions that maximize $|\mathsf{\Phi }|$ must satisfy

$$\left[\begin{array}{c}{d}_{2}cos({\vartheta }_1-{\vartheta }_3)-d_{3}cos({\vartheta }_1-{\vartheta }_2)\\ d_{1}cos({\vartheta }_2-{\vartheta }_3)-d_{3}cos({\vartheta }_1-{\vartheta }_2)\\ d_{1}cos({\vartheta }_2-{\vartheta }_3)-d_{2}cos({\vartheta }_1-{\vartheta }_3)\end{array}\right]=\left[\begin{array}{c}0\\ 0\\ 0\end{array}\right].$$

(15)

From (9), it is clear that the maximum value of the determinant depends on the angular separation of beacons. By defining ${\beta }_1\triangleq {\vartheta }_1-{\vartheta }_2,{\beta }_2\triangleq {\vartheta }_2-{\vartheta }_3,\mathrm{and}{\beta }_3\triangleq {\vartheta }_3-{\vartheta }_1,$ the determinant of the FIM can be parameterized using the two variables ${\beta }_1$ and ${\beta }_2$ as ${\beta }_3=-{\beta }_2-{\beta }_1$. A necessary condition for optimality is that the gradient of $|\mathsf{\Phi }(\mathit{\vartheta }\left)\right|$ with respect to ${\beta }_1$ and ${\beta }_2$ should satisfy

$$\left[\begin{array}{c}{d}_{3}cos\left({\beta }_1\right)-d_{2}cos({\beta }_1+{\beta }_2)\\ d_{1}cos\left({\beta }_2\right)-d_{2}cos({\beta }_1+{\beta }_2)\end{array}\right]=\left[\begin{array}{c}0\\ 0\end{array}\right].$$

(16)

By defining $X_i\triangleq tan({\beta }_i/2)$, $i=1,2,3$, and rewriting the equation above in terms of $X_1,X_2$ using the identity $tan(({\beta }_1+{\beta }_2)/2)=(X_1+X_2)/(1-X_1{X}_2)$, we get

$$\begin{array}{c}{d}_3-d_2+X_1^2(d_2-d_3)+X_2^2(d_2+d_3)-X_1^2{X}_2^2(d_3+d_2)+4X_1{X}_2{d}_2=0,\\ d_1-d_2+X_1^2(d_1+d_2)+X_2^2(d_2-d_1)-X_1^2{X}_2^2(d_1+d_2)+4X_1{X}_2{d}_2=0,\end{array}$$

which can be re-expressed as a system of nonlinear algebraic equations

$$A_i{X}_i^6+B_i{X}_i^4+C_i{X}_i^2+D_i=0,$$

(17)

where

$$\begin{array}{cc}\hfill X_i& \triangleq tan({\beta }_i/2),\hfill \\ \hfill A_i& \triangleq d_k^2{(d_i+d_j)}^2,\hfill \\ \hfill B_i& \triangleq -6d_i{d}_j{d}_k^2-4d_i^2{d}_j^2-d_i^2{d}_k^2-d_j^2{d}_k^2,\hfill \\ \hfill C_i& \triangleq 6d_i{d}_j{d}_k^2-4d_i^2{d}_j^2-d_i^2{d}_k^2-d_j^2{d}_k^2,\hfill \\ \hfill D_i& \triangleq d_k^2{(d_i-d_j)}^2,\hfill \\ \hfill (i,j,k)& \in \left\{\right(1,2,3),(2,3,1\left)\right\}.\hfill \end{array}$$

The sixth-order even polynomials in (17) have three pairs of roots each. In the general case where $d_1,d_2,d_3$ are different, $A_i<0$, $B_i<0$, $D_i>0$, implying there are two sign changes between consecutive coefficients $A_i,B_i,C_i,D_i$. According to Descartes’ rule of signs [32], the associated polynomial $A_i{z}^3+B_i{z}^2+C_{i}z+D_i$, for some variable z, has two positive real roots and one negative real root. The negative root must be real because a cubic polynomial has either one or three real roots. Thus, the sixth-order polynomials in (17) have two positive roots, two negative roots and two imaginary roots each. Among these six roots, we only consider the two positive roots because the imaginary roots are not applicable and the negative roots are simply the negated versions of the positive roots. Figure 2 shows that, if $\{{\beta }_1^{*},{\beta }_2^{*},{\beta }_3^{*}\}$ satisfies (17), so does $\{-{\beta }_1^{*},-{\beta }_2^{*},-{\beta }_3^{*}\}$. The optimal geometries are symmetric with respect to the value of the objective function, reflecting all the beacons about the vehicle position gives an equivalent optimal geometry.

The sufficient condition for a stationary point to be a maximum is that the associated Hessian matrix should be negative-definite, i.e.,

$$H\triangleq \left[\begin{array}{cc}{H}_{11}& H_{12}\\ H_{21}& H_{22}\end{array}\right]=\left[\begin{array}{cc}\frac{{\partial }^2|\mathsf{\Phi }|}{\partial {\beta }_1^2}& \frac{{\partial }^2|\mathsf{\Phi }|}{\partial {\beta }_1\partial {\beta }_2}\\ \frac{{\partial }^2|\mathsf{\Phi }|}{\partial {\beta }_1\partial {\beta }_2}& \frac{{\partial }^2|\mathsf{\Phi }|}{\partial {\beta }_2^2}\end{array}\right]$$

has only negative eigenvalues. If ${\beta }_1,{\beta }_2$ satisfy the necessary condition, the sufficient condition reduces to two inequalities:

$$\begin{array}{cc}\hfill H_{11}+H_{22}& <0,\hfill \end{array}$$

(18a)

$$\begin{array}{cc}\hfill 0<H_{11}{H}_{22}-H_{12}^2& <\frac{{(H_{11}+H_{22})}^2}{4}.\hfill \end{array}$$

(18b)

As demonstrated in the supplementary material in Appendix A, the inequalities in (18) are equivalent to

$$\begin{array}{c}F\left[F+d_{2}sin(-{\beta }_1-{\beta }_2)\right]>0,\end{array}$$

(19a)

$$\begin{array}{c}{d}_2{d}_{3}sin(-{\beta }_1-{\beta }_2)sin{\beta }_1+d_1{d}_{2}sin(-{\beta }_1-{\beta }_2)sin{\beta }_2+d_1{d}_{3}sin{\beta }_{1}sin{\beta }_2>0,\end{array}$$

(19b)

$$\begin{array}{c}{(d_{1}sin{\beta }_2-d_{3}sin{\beta }_1)}^2+4d_2^2{sin}^2(-{\beta }_1-{\beta }_2)>0,\end{array}$$

(19c)

where $F=d_{1}sin{\beta }_2+d_{2}sin(-{\beta }_1-{\beta }_2)+d_{3}sin{\beta }_1$. At this point, we conclude that if ${\beta }_1^{*},{\beta }_2^{*},{\beta }_3^{*}$ satisfy (17) and the inequalities (19), they are the solution to the optimization problem (9).

Theorem 1.

The positive roots of the polynomials in (17) that satisfy

$$X_i>1,i=1,2$$

(20)

are the solution to the optimization problem (9).

A proof of Theorem 1 is provided in Appendix A. Theorem 1 is readily applicable to the case where two or three of the beacon ranges are equal. If $d_i=d_j$, the polynomial $A_i{z}^3+B_i{z}^2+C_{i}z+D_i$ has either one positive root or two positive roots. In the former case, the only positive root is the optimizing solution, while, in the latter case, the larger positive root is the optimizing solution. If $d_i=d_j=d_k=d$, the polynomial $A_i{z}^3+B_i{z}^2+C_{i}z+D_i$ becomes $d^4{z}^2(z-3)$, implying $tan({\beta }_1^{*}/2)=tan({\beta }_2^{*}/2)=\pm \sqrt{3}$, or, equivalently, ${\beta }_1^{*}={\beta }_2^{*}={\beta }_3^{*}=\pm 2\pi /3$.

6. Simulation Results

This section presents simulation results confirming the validity of the proposed analytical method. Two simulation tasks were performed.

In the first simulation task, the optimal angular separations of the beacons, ${\beta }_1^{*},{\beta }_2^{*}$, were calculated using the proposed analytical method for 1000 sets of beacon distances, where each distance is uniformly distributed between 0 and 100 units. For comparison, the optimal angular separations were also computed using (i) the MATLAB function ga, which implements a genetic algorithm; and (ii) the MATLAB function fminsearch, which implements a derivative-free method for finding the minimum of an unconstrained multivariable function. The histograms in Figure 3a,b help us visualize the distributions of differences ${|\mathsf{\Phi }|}_{\mathrm{a}}^{*}-{|\mathsf{\Phi }|}_{\mathrm{ga}}^{*}$ and ${|\mathsf{\Phi }|}_{\mathrm{a}}^{*}-{|\mathsf{\Phi }|}_{\mathrm{fmin}}^{*}$, where ${|\mathsf{\Phi }|}_{\mathrm{a}}^{*}$, ${|\mathsf{\Phi }|}_{\mathrm{ga}}^{*}$, and ${|\mathsf{\Phi }|}_{\mathrm{fmin}}^{*}$ are the maximum values of $|\mathsf{\Phi }|$ calculated using the proposed analytical method, the MATLAB function ga, and the MATLAB function fminsearch, respectively. Our observation that ${|\mathsf{\Phi }|}_{\mathrm{a}}^{*}-{|\mathsf{\Phi }|}_{\mathrm{ga}}^{*}$ and ${|\mathsf{\Phi }|}_{\mathrm{a}}^{*}-{|\mathsf{\Phi }|}_{\mathrm{fmin}}^{*}$ are always positive confirms that the maximum value calculated analytically is always larger than the maximum value calculated using either of the other methods.

Figure 4 shows a surface plot and a contour plot of the objective function $|\mathsf{\Phi }|$ as a function of ${\beta }_1$ and ${\beta }_2$. The objective function has two maxima associated with two sets of angular separations that are negated version of each other (cf. Figure 2). The surface of the objective function is point-symmetric about the origin.

The second simulation task was to evaluate the estimation accuracy of maximum likelihood estimation (MLE) for different beacon geometries.

The maximum likelihood estimates, denoted ${[{\widehat{p}}_{ML},{\widehat{\varphi }}_{ML}]}^{\top }$, were obtained by maximizing the log-likelihood function of the noisy bearing measurements $lnp\left(\tilde{\mathit{\theta }}\right|p,\varphi )$ over the vehicle states $(p,\varphi )$, i.e.,

$${[{\widehat{p}}_{ML},{\widehat{\varphi }}_{ML}]}^{\top }=\underset{p\in {\mathbb{R}}^2,\varphi \in \mathbb{R}}{arg\; min}{J}_{ML}({\displaystyle p},\varphi ),$$

(21)

where $J_{ML}(\mathit{p},\varphi )$ is the maximum likelihood cost function [33]:

$$J_{ML}({\displaystyle p},\varphi )\triangleq \frac{1}{2}{\mathit{e}}^{\top }({\displaystyle p},\varphi ){\Sigma }^{-1}{\displaystyle e}({\displaystyle p},\varphi ),{\displaystyle e}({\displaystyle p},\varphi )\triangleq \tilde{\mathit{\theta }}-{\displaystyle \mathit{\theta }}({\displaystyle p},\varphi ).$$

(22)

The problem of estimating the vehicle states by minimizing the maximum likelihood cost function in Equation (22) has no closed-form solution, but it can be solved numerically using algorithms such as the Nelder–Mead simplex algorithm, which the MATLAB function fminsearch implements. For this simulation task, the beacon distances associated with Figure 4 were used, and the angular separations of the beacons were varied. As ${\beta }_1$ and ${\beta }_2$ were varied from $-\pi$ to $\pi$ at a step size of $\Delta =\frac{\pi }{15}$ radians, $31\times 31=961$ pairs of $({\beta }_1,{\beta }_2)$ values were generated. For each $({\beta }_1,{\beta }_2)$ value pair, 1000 MLEs were performed to estimate the vehicle states $(p,\varphi )$ using AoA measurements corrupted by a zero-mean Gaussian noise with standard deviation $\sigma =1^{°}$. The maximum likelihood estimator was initialized at the true vehicle state values to ensure convergence. For each $({\beta }_1,{\beta }_2)$ pair, the determinant of the inverse estimation error covariance matrix $|{\Sigma }_m^{-1}|$ was calculated and used to produce Figure 5.

Observe in Figure 5 that the shape of $|{\Sigma }_m^{-1}|$ matches that of $|\Phi |$ in Figure 4. It is clear that, in practice, the objective function is maximized near the analytically determined optimal beacon placement, as the peaks of the plot in Figure 5 align closely with ${\beta }_1^{*}$ and ${\beta }_2^{*}$. When the number of MLEs approaches infinity and the grid step size $\Delta$ approaches zero, the maxima of $|{\Sigma }_m^{-1}|$ are expected to overlap exactly with the maxima of $|\Phi |$.

One of the limitations of this study is the assumption that the measurement noise variance is constant. The constant–variance assumption is common, although in practice the variance can be range-dependent [23,34,35]. Nevertheless, the proposed method is readily modifiable to take into account an alternative measurement noise model, which will depend on the system or application under consideration.

One of the key differentiators in the optimal geometry for self-localization and location estimation is the relationship between optimal geometries for a given set of beacon distances. For any given optimal geometry of an AoA location estimation problem, other geometries that produce the same D-optimality criterion value can be determined by making a point reflection of any number of beacons about the vehicle position [5]. For the AoA self-localization problem using three beacons, there are only two optimal geometries. The two optimal geometries are related by making a point reflection of all the beacons about the vehicle position. The resulting geometries take the form of two mirror-symmetric triangles (see Figure 2).

7. Conclusions

We have developed a method for optimally placing three beacons to minimize AoA self-localization uncertainty. Theorem 1 can be used to determine the optimal angular separations among three beacons such that the D-optimality criterion is satisfied. We proved that the proposed analytical solution satisfies the necessary and sufficient conditions for optimality. Additionally, our simulation results confirm the optimality of the proposed approach. Two simulation tasks were described. In the first simulation task, the optimal beacon angular separations calculated analytically were compared with the beacon angular separations calculated using two numerical methods. The results show that the value of the determinant of the FIM associated with the analytically obtained beacon angular separations is always larger than the value associated with the numerically obtained beacon angular separations. In the second simulation task, for one set of beacon distances, the value of the determinant of the FIM was computed for 961 sets of beacon angular separations using maximum likelihood estimation. The results show that the value of the determinant of the FIM is maximized at beacon angular separations that are close to the analytical solution.

Our future work is related to optimal path planning for self-localization, i.e., the determination of a vehicle trajectory that minimizes self-localization uncertainty. The task can be performed by controlling the vehicle to achieve the best possible self-localization geometry at discrete time intervals. We will extend the results reported here to the problem of optimal path planning.