Accurate Rigid Body Localization Using DoA Measurements from a Single Base Station

Abstract

Rigid body localization (RBL) is to simultaneously estimate the position and attitude of a rigid target. In this paper, we focus on the RBL problem using a single base station (BS) and direction of arrival (DoA) measurements. Several wireless sensors are mounted on the rigid body of interest, and their topology information is known a priori. The single BS measures the DoAs of wireless sensor signals and fuses them with the sensor topology information to estimate the position and orientation of the rigid body and achieve RBL. We propose two RBL methods, namely, the observation matching (OM) algorithm and topology matching (TM) algorithm with refinement. The emerging participatory searching algorithm (PSA) is adopted in both methods to solve the nonlinear matching problems. Simulations show that, compared with the existing approach, the OM method can achieve better RBL accuracy under high DoA noise levels, while the performance of the TM algorithm with refinement is closer to the constrained Cramér–Rao bound (CCRB) under low DoA noise levels.

1. Introduction

1.1. Background

A rigid body is a solid body whose deformation can be neglected. Large rigid bodies include airplanes, ships, and vehicles, while virtual reality (VR) helmets, human bodies, and heads of robots are typical small rigid bodies. Rigid body localization (RBL) estimates, relative to a predetermined reference frame, both the position and attitude of a rigid object in three-dimensional (3-D) space [1,2].

High-precision position and attitude information of a rigid object are essential for many military and civilian applications. For example, RBL can be used to acquire the 3-D position and orientation of unmanned aerial vehicles (UAVs) [3], aircrafts, and submarines. It can also be applied to monitor the tilts of buildings and bridges in real time. In VR systems, both the position and attitude information of the VR helmet are necessary for providing a corresponding virtual picture [4]. Last but not least, in the intelligent parking scenario, the position and heading direction of the vehicle are indispensable [5].

1.2. Related Works

Conventionally, the position and attitude estimation for an object have been investigated separately. Among the positioning technologies, the Global Navigation Satellite System (GNSS) is well-known but only efficient in open outdoor environments. Alternatively, in wireless sensor networks (WSNs), the location of the object can be found by observing the signal from wireless devices equipped on the target [6,7,8]. For fine-grained positioning, commonly used parameters are the time of arrival (ToA), time difference of arrival (TDoA), and direction of arrival (DoA), to just list a few.

As for the attitude estimation for rigid objects, the use of an inertial measurement unit (IMU), such as the gyro sensor [9] or the three-axis magnetometer [10], is a straightforward choice. However, IMU-based attitude estimation suffers from long-term unilateral error accumulation, while the three-axis magnetometer is based on the strength of Earth’s magnetic field, which may be affected by complex ground environments. In addition, these attitude estimation schemes alone are not able to provide the position information of a rigid object at the same time.

For simultaneously determining the position and attitude of a rigid target, several techniques have been proposed in literature. An efficient approach for the RBL of outdoor, large-scale rigid bodies was developed based on the GNSS [11,12,13]. In this scheme, the absolute position was determined by pseudo-range observations, while the attitude was estimated from the measured carrier phases of the satellite signals received at multiple GNSS antennas. However, application of the above GNSS-based scheme was restricted to outdoor scenarios, and the use of multiple GNSS antennas led to higher implementation costs. Computer vision-based RBL is another popular solution [14,15]. This scheme uses a single (or multiple) theodolite to find the 3-D coordinates of feature points on the rigid body, and then it applies methods like singular value decomposition (SVD) [16] to obtain the relative displacement vector and attitude information with respect to the reference frame. Although this method has high estimation accuracy, its performance relies heavily on the precise coordinate observations of feature points, which poses significant demand for light conditions as well as on computational resources.

The RBL approaches based on small-scale WSNs can mitigate the weakness of the aforementioned two RBL schemes. In general, the WSN-based technique is implemented as follows. Firstly, several wireless sensors are mounted on the surface of the rigid body, which transforms the point target into an extended one. Next, the physical observations of wireless sensor signals are acquired by the base stations (BSs) (i.e., the anchor nodes). Finally, the 3-D position and attitude information of the rigid object are obtained by fusing the known topology information of the wireless sensors and sensor signal observations.

State-of-the-art WSN-based RBL schemes mainly consider the use of ToA measurements (also referred to as the range measurements) to determine sensor-BS distances, which are then exploited with the topology information for RBL [17,18,19,20]. In the ToA-based RBL model, the position and attitude of the rigid body relative to a reference frame are obtained via finding the associated rotation matrix and translation vector. This is a challenging problem, because the rotation matrix to be determined belongs to the 3-D special orthogonal $SO\left(3\right)$ group. In [18], the authors first linearized the RBL model by adopting a squared-range (SR) formulation. They then proposed to use a serial of unitarily constrained least square estimators to compute the estimate of the target position and orientation. The Cramér–Rao lower bound (CRLB) for the ToA-based RBL task was also derived. To further improve computation efficiency, authors in [19] employed the divide and conquer (DAC) strategy and developed a noniterative algorithm with higher RBL accuracy obtained via a refinement stage. Alternatively, a semidefinite relaxation (SDR) technique was adopted to find the maximum likelihood (ML) solution to ToA-based RBL [20,21]. The developed SDR-based methods were shown to have better performances under high noise levels.

Nevertheless, deficiencies of the ToA-based RBL solutions are also obvious. First, it requires three or more BSs spatially separated at known, fixed positions. This will increase system complexity, and it may even be impractical in circumstances such as spacecraft docking or where the BSs are moving. Another aspect is that it has high requirement for time synchronization between wireless sensors and BSs, which is quite challenging in practice.

The DoAs of the wireless signal can be obtained via applying array signal processing techniques [22], and they can be used for RBL as well. As an alternative to ToA, the DoA measurement completely eliminates the need for precise sensor BS synchronization. In addition, two-dimensional (2-D) DoA information, namely the azimuth and pitch angles, makes it possible to perform RBL using a single BS [23]. Last, DoA estimation methods are readily available. For example, the estimation of signal parameters via rotational invariance technique (ESPRIT) and multiple signal classification (MUSIC) algorithms are widely adopted DoA estimators. In this paper, we shall consider RBL based on the DoAs obtained at a single BS.

1.3. Contributions

Similar to existing ToA-based schemes, we assume in this paper that several wireless sensors have already been mounted on the rigid body of interest, and their topology is known precisely. DoA information of these wireless sensors is measured by the BS. By fusing the DoA measurements with the wireless sensor topology, the RBL is achieved. In this paper, we present two methods for the RBL task, namely the observation matching (OM) algorithm and the enhanced topology matching (TM) algorithm with refinement. Both methods utilize an emerging population-based optimization framework called participatory searching algorithm (PSA) [24]. Our contributions are summarized as follows.

(1) In the OM method, we first exploit the multidimensional scaling (MDS) technique [25] to fuse the wireless sensor topology information and obtain the reference frame. Then, the reference frame undergoes an instantaneously determined translation and rotation, and the current frame is obtained. The theoretical 2-D DoA of each sensor in the obtained current frame is then compared against the actual DoA measurements to search for the optimal translation and rotation parameters. In this way, the unknown translation and rotation parameters applied to the reference frame can be found, which accomplishes the RBL task.

(2) The TM method with refinement has three processing steps. In Step 1, the 3-D coordinates of all wireless sensors are expressed in terms of the DoA measurements by iteratively matching the estimated sensor topology against the known topology information. We obtain the sensor coordinates as the coarsely estimated current frame. In Step 2, we conduct a refinement process using the weighted least squares (WLS) technique to better explore the noise-free sensor topology information and update the coarsely estimated sensor coordinates in Step 1. In Step 3, the translation vector and rotation matrix that relate the refined current frame to the reference frame are found via an SVD-based approach.

(3) As a benchmark for evaluating the performance of the two proposed methods, we present in detail the derivation of the constrained Cramér–Rao bound (CCRB) for the considered problem of RBL using DoAs from a single BS. The constraint is from the fact that the rotation matrix to be estimated belongs to the $SO\left(3\right)$ group.

The rest of the paper is organized as follows. In Section 2, we formulate the single BS RBL problem in consideration and derive the CCRB. The two proposed RBL approaches are presented in Section 3. A brief introduction and the realization of the PSA algorithm are given in Section 4. In Section 5, through simulations, we evaluate the performance of proposed RBL approaches. Finally, the conclusion is drawn in Section 6.

For clarity, we denote matrices and vectors with upper-case and lower-case bold-face letters. ${\tan }^{-1}(\ast )$ represents 1/$\tan (\ast )$. ${\mathbf{X}}^{\mathbf{T}}$ is the transpose of $\mathbf{X}$, and $\hat{\mathbf{x}}$ denotes the estimate of $\mathrm{x}$. $\left|\right|\ast \left|\right|$ represents the Euclidian norm, and $\otimes$ is the Kronecker product. ${\mathbf{I}}_{\mathbf{n}}$ is the $\mathrm{n}$-by-$\mathrm{n}$ identity matrix. $\mathrm{vec}\left(\mathbf{X}\right)$ and $\mathrm{Rank}\left(\mathbf{X}\right)$ are, respectively, a column vector obtained by stacking the columns of the matrix $\mathbf{X}$ and the rank of $\mathbf{X}$. $\mathrm{diag}\left(\mathbf{x}\right)$ is a diagonal matrix with its diagonal elements equal to the elements in $\mathbf{x}$.

2. Problem Formulation and Constrained Cramér–Rao Bound (CCRB)

2.1. Rigid Body Localization (RBL) Based on Direction of Arrival (DoA) Measurements from a Single Base Station (BS)

To decide the position and orientation of a rigid body in 3-D space, we mounted $K$ wireless sensors on the rigid object surface to serve as feature points. In the reference frame, the position of the $k$-th sensor was denoted as $c_k={\left[c_{x,k},c_{y,k},c_{z,k}\right]}^T$. Collecting them yielded $C=\left[c_1,\dots ,c_K\right]\in {ℝ}^{3\times K}$. The sensor positions $c_k$ under the reference frame can be determined by the known sensor topology information. They are known quantities, as will be evident in the next section. Without the loss of generality, the reference frame was set to be at the origin $\mathit{O}$ where the single BS was located.

On the other hand, under the current frame of the rigid body, which is to be estimated, the 3-D coordinates of wireless sensors were collected in $S=\left[s_1,\dots ,s_K\right]\in {ℝ}^{3\times K}$. Here, $s_k={\left[s_{x,k},s_{y,k},s_{z,k}\right]}^T$ are the 3-D coordinates of the $k$-th sensor under the current frame. According to the rigid body transformation rule [16], $S$ can be obtained by applying to $C$ a rotation and translation (see Figure 1). Mathematically, we have:

$$S=QC+t\otimes {\mathbf{1}}_{1\times K},$$

(1)

where $t={\left[x,y,z\right]}^T$ is the unknown translation vector. $Q$ is the unknown rotation matrix and it belongs to the 3-D orthogonal group:

$$SO\left(3\right)=\left\{Q\in {ℝ}^{3\times 3}:Q{Q}^T=Q^{T}Q=I_3,\det \left(Q\right)=1\right\}.$$

(2)

$Q$ can also be specified using the Euler angles (i.e., roll $\varphi$ (rotation about $x$-axis), pitch $\phi$ (rotation about $y$-axis), and yaw $\psi$ (rotation about $z$-axis)). Mathematically, $Q$ can be expressed as [25]:

$$Q={\left[q_1,q_2,q_3\right]}^T=\left[\begin{array}{ccc}{c}_{\phi }{c}_{\psi }& c_{\phi }{s}_{\psi }& -s_{\phi }\\ s_{\varphi }{s}_{\phi }{c}_{\psi }-c_{\varphi }{s}_{\psi }& s_{\varphi }{s}_{\phi }{s}_{\psi }+c_{\varphi }{c}_{\psi }& s_{\varphi }{c}_{\phi }\\ c_{\varphi }{s}_{\phi }{c}_{\psi }+s_{\varphi }{s}_{\psi }& c_{\varphi }{s}_{\phi }{s}_{\psi }-s_{\varphi }{s}_{\psi }& s_{\varphi }{c}_{\phi }\end{array}\right],$$

(3)

where $s_x$ and $c_x$ represent $\sin x$ and $\cos x$, respectively.

The RBL of a rigid body requires estimating the translation vector $t$ and rotation matrix $Q$. For this purpose, two pieces of information are utilized in this work. The first one is the exactly known topology information of the wireless sensors. If the full-connected scenario is considered, the sensor topology information is embedded in $\frac{K\left(K-1\right)}{2}$ Euclidean distances among all sensor pairs [26]. They can be collected in a column vector:

$$d={\left[d_{1,2},\dots ,d_{i,j},\dots ,d_{K-1,K}\right]}^T,i,j=1,\cdots ,K,j>i,$$

(4)

where $d_{i,j}=\left|\right|c_i-c_j\left|\right|=\left|\right|s_i-s_j\left|\right|$ is the Euclidean distance between $i$-th and $j$-th sensors.

Another piece of information for RBL is provided by the BS at the origin. They are the 2-D DoAs of the wireless sensor signals. We assumed that the BS was equipped with an L-shaped antenna array (LSAA). In particular, sufficient antenna elements were deployed along the $x$- and $z$-axes to measure the angles between the arriving signal directions of the $K$ sensors and the $x$- and $z$-axes, which were denoted as $\mathit{\alpha }=\left[{\alpha }_1,\dots ,{\alpha }_K\right]$ and $\mathit{\beta }=\left[{\beta }_1,\dots ,{\beta }_K\right]$, respectively, as shown in Figure 1. Discussions on specific DoA estimation techniques are beyond the scope of this paper and interested readers are directed to, e.g., [27]. We assume the measured DoAs of the $k$-th sensor, ${\alpha }_k$ and ${\beta }_k$, are subject to independent Gaussian noise such that:

$$d={\left[d_{1,2},\dots ,d_{i,j},\dots ,d_{K-1,K}\right]}^T,i,j=1,\cdots ,K,j>i,$$

(5)

where ${\left[{\alpha }_k^o,{\beta }_k^o\right]}^T$ collects the true DoA values.

We are interested in determining the translation vector $t$ and rotation matrix $Q$ that relate the reference frame to the current frame (see (1)) from the noise-free sensor distance information in $d$ (see (4)) and 2-D DoA measurements in (5).

2.2. CCRB

We shall derive the CCRB for the DoA-based RBL problem formulated in the previous section. The parameters to be estimated included the translation vector $t={\left[x,y,z\right]}^T$ and the rotation matrix $Q$. We introduced a $12\times 1$ composite unknown vector $\mathit{\rho }={\left[q_1{}^T,q_2{}^T,q_3{}^T,t^T\right]}^T$. Because $Q$ belonged to $SO\left(3\right)$ (see (2)), the unknown vector $\mathit{\rho }$ was then subject to equality constraints. The associated CCRB of $\mathit{\rho }$ has form [28]:

$$CCRB\left(\mathit{\rho }\right)\ge U{\left(U^{T}F\left(\mathit{\rho }\right)U\right)}^{-1}{U}^T,$$

(6)

where $U\in {ℝ}^{12\times P}$ depends on the equality constraints from (2), and $P$ is the number of the constraints. $F\left(\mathit{\rho }\right)\in {ℝ}^{12\times 12}$ is the unconstrained Fisher information matrix (FIM) of the composite unknown vector $\mathit{\rho }$. We shall provide their definitions below.

According to (2), $\mathit{\rho }$ satisfies the following $P=6$ equality constraints:

$$\mathit{h}\left(\mathit{\rho }\right)={\left[q_1{}^T{q}_1-1,q_2{}^T{q}_2-1,q_3{}^T{q}_3-1,q_1{}^T{q}_2,q_1{}^T{q}_3,q_2{}^T{q}_3\right]}^T={\mathbf{0}}_{P\times 1}.$$

(7)

The Jacobian matrix for $\mathit{h}\left(\mathit{\rho }\right)$ is, thus, given by:

$$\mathbf{H}\left(\mathit{\rho }\right)=\frac{\partial \mathbf{h}\left(\mathit{\rho }\right)}{\partial \mathit{\rho }}\left[\begin{array}{cccc}2q_1{}^T& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{1\times 3}& 2q_2{}^T& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}& 2q_3{}^T& {\mathbf{0}}_{1\times 3}\\ q_2{}^T& q_1{}^T& {\mathbf{0}}_{1\times 3}& {\mathbf{0}}_{1\times 3}\\ q_3{}^T& {\mathbf{0}}_{1\times 3}& q_1{}^T& {\mathbf{0}}_{1\times 3}\\ {\mathbf{0}}_{1\times 3}& q_3{}^T& q_2{}^T& {\mathbf{0}}_{1\times 3}\end{array}\right].$$

(8)

The matrix $U\left(\mathit{\rho }\right)$ required in (6) can then be computed as the orthonormal basis of the null-space of $H\left(\mathit{\rho }\right)$ (i.e., $H{\left(\mathit{\rho }\right)}^{T}U\left(\mathit{\rho }\right)={\mathbf{0}}_{6\times 6}$ and $\mathrm{Rank}\left(U\left(\mathit{\rho }\right)\right)=12-P=6$) because $\mathrm{Rank}\left(H\left(\mathit{\rho }\right)\right)$ was equal to the number of constraints $P=6$. A particular matrix $U\left(\mathit{\rho }\right)$ satisfying $H{\left(\mathit{\rho }\right)}^{T}U\left(\mathit{\rho }\right)={\mathbf{0}}_{6\times 6}$ is:

$$U=\left[\begin{array}{cc}\begin{array}{ccc}-q_3& {\mathbf{0}}_{3\times 1}& q_2\\ {\mathbf{0}}_{3\times 1}& -q_3& -q_1\\ q_1& q_2& {\mathbf{0}}_{3\times 1}\end{array}& {\mathbf{0}}_{9\times 3}\\ {\mathbf{0}}_{3\times 3}& \sqrt{2}·I_3\end{array}\right].$$

(9)

As for the FIM $F\left(\mathit{\rho }\right)$, according to (5), the DoA measurement elements were corrupted by the independent additive Gaussian noise, i.e., ${\left[{\alpha }_k,{\beta }_k\right]}^T~\mathcal{N}\left({\left[{\alpha }_k^o,{\beta }_k^o\right]}^T,\mathrm{diag}\left({\sigma }^2,{\sigma }^2\right)\right)$. As a result, the likelihood of $\mathit{\alpha }$ and $\mathit{\beta }$ can be shown to be equal to:

$$p\left(\mathit{\alpha },\mathit{\beta };\mathit{\rho }\right)=\frac{1}{{\left(2\pi {\sigma }^2\right)}^{K/2}}\exp \left\{-\left[\frac{1}{2{\sigma }^2}\ast {{\displaystyle \sum }}_{k=1}^K{\left({\alpha }_k-{\alpha }_k^o\right)}^2+\frac{1}{2{\sigma }^2}\ast {{\displaystyle \sum }}_{k=1}^K{\left({\beta }_k-{\beta }_k^o\right)}^2\right]\right\},$$

(10)

where ${\alpha }_k^o=\arctan \left(\sqrt{y_k^2+z_k^2}/x_k\right)\mathrm{and}{\beta }_k^o=\arctan \left(\sqrt{x_k^2+y_k^2}/z_k\right)$. According to (1), $x_k=q_1{}^T{c}_k+x$, $y_k=q_2{}^T{c}_k+y$, and $z_k=q_3{}^T{c}_k+z$. $F\left(\mathit{\rho }\right)$ can be found via $F\left(\mathit{\rho }\right)=-E\left[\frac{{\partial }^2\ln p\left(\mathit{\alpha },\mathit{\beta };\mathit{\rho }\right)}{\partial \mathit{\rho }{\mathit{\rho }}^T}\right]$ [29]. Putting $F\left(\mathit{\rho }\right)$ and (9) back into (6) completed the CCRB derivation.

3. Proposed RBL Algorithms

In this section, we present two RBL algorithms for the DoA-based RBL problem described in Section 2. Both methods first converted the 2-D DoA measurements in (5) into the projected angles to facilitate their development. According to Figure 1, the projected azimuth angle ${\theta }_{k,x}$ and projected pitch angle ${\theta }_{k,z}$ can be computed from the 2-D DoA from sensors ${\alpha }_k$ and ${\beta }_k$ via [30]:

$$\begin{array}{c}{\theta }_{k,x}=\arctan \sqrt{\frac{{\tan }^2{\beta }_k+1}{{\tan }^2{\alpha }_k{\tan }^2{\beta }_k-1}};\\ {\theta }_{k,z}=\arctan \sqrt{\frac{{\tan }^2{\alpha }_k+1}{{\tan }^2{\alpha }_k{\tan }^2{\beta }_k-1}}.\end{array}$$

(11)

Notably, ${\theta }_{k,x}$ and ${\theta }_{k,z}$ are no longer independently distributed. Under the low DoA noise condition, we can express (11) as, after invoking the first order Taylor-Series expansion,

$${[{\theta }_{k,x},{\theta }_{k,z}]}^T={[{\theta }_{k,x}^o,{\theta }_{k,z}^o]}^T+{[\Delta {\theta }_{k,x},\Delta {\theta }_{k,z}]}^T\approx {[{\theta }_{k,x}^o,{\theta }_{k,z}^o]}^T+{\mathbf{J}}_k·{[\Delta {\theta }_{k,x},\Delta {\theta }_{k,z}]}^T,$$

(12)

where $J_k=\left[\begin{array}{cc}\frac{\partial {\theta }_{k,x}^o}{\partial {\alpha }_k^o}& \frac{\partial {\theta }_{k,x}^o}{\partial {\beta }_k^o}\\ \frac{\partial {\theta }_{k,z}^o}{\partial {\alpha }_k^o}& \frac{\partial {\theta }_{k,z}^o}{\partial {\beta }_k^o}\end{array}\right]$ is the Jacobian of ${\left[{\theta }_{k,x},{\theta }_{k,z}\right]}^T$ with respect to ${\alpha }_k,{\beta }_k$ evaluated at ${\left[{\alpha }_k^o,{\beta }_k^o\right]}^T$. From (12), it can be approximately established that ${\left[{\theta }_{k,x},{\theta }_{k,z}\right]}^T~\mathcal{N}\left({\left[{\theta }_{k,x}^o,{\theta }_{k,z}^o\right]}^T,{\Sigma }_k\right)$, in which ${\Sigma }_k=J_k\mathrm{diag}\left({\sigma }^2,{\sigma }^2\right)J_k^T$. The true projected angles can be related to the sensor coordinates via ${\left[{\theta }_{k,x}^o,{\theta }_{k,z}^o\right]}^T={\left[\arctan \left(x_k/y_k\right),\arctan \left(z_k/y_k\right)\right]}^T$.

3.1. Observation Matching (OM) Approach

Within the proposed OM algorithm, we first built the reference frame $C$ using the sensor topology information. They were then matched to the DoA observations to determine the translation vector and rotation matrix to achieve RBL. This was why the method was called observation matching (OM).

Specifically, the MDS technique [31] based on eigenvalue decomposition (EVD) was adopted to build the reference frame from the known topology information in $d$ (see its definition in (4)). For this purpose, we constructed a symmetric squared distance matrix $D_{M\times M}$ whose element in the $i$-th row and $j$-th column was:

$$D\left(i,j\right)=\{\begin{array}{c}{d}_{i,j}^2,ifi\ne j\\ 0,ifi=j\end{array},$$

(13)

where $M=\frac{K\left(K-1\right)}{2}$, $i=1,2,\dots ,M$, and $j=1,2,\dots ,M.$

A centering matrix $E=I_M-\frac{1_{M\times M}}{M}$ was defined and $D$ was converted into $B=-ED{E}^T/2=E{C}^{T}C{E}^T$. The rank of $B$ was 3 since the sensors in the reference frame were distributed in a 3-D space. Performing the EVD of $B$ yielded $B=X\mathrm{diag}\left({\lambda }_1,{\lambda }_2,{\lambda }_3\right)X^T$, where ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$ were the three largest nonzero eigenvalues in descending order, and the columns of $X\in {ℝ}^{M\times 3}$ were the three associated eigenvectors. As a result, the reference frame can be expressed as:

$$C=\left[X\mathrm{diag}\left(\sqrt{{\lambda }_1},\sqrt{{\lambda }_2},\sqrt{{\lambda }_3}\right)\right]{}^T.$$

(14)

Note that the obtained reference frame $C$ was noise-free, as the wireless sensor topology was known precisely.

We proceeded to fuse $C$ with the DoA measurements in $\mathit{\theta }$ using the least square (LS) criterion to estimate the translation vector $t$ and rotation matrix $Q$. According to (1) and (3), the current frame $S$ could be completely specified by six parameters, which consisted of the coordinates of the translation vector $t={\left[x,y,z\right]}^T$ as well as three Euler angles ${\left[\varphi ,\phi ,\psi \right]}^T$. Let $\tilde{S}=\left[{\tilde{s}}_1,\dots ,{\tilde{s}}_K\right]$ denote the current frame given $t$ and ${\left[\varphi ,\phi ,\psi \right]}^T$, which is expressed as:

$${\tilde{s}}_k=Q\left(\varphi ,\phi ,\psi \right)c_k+{\left[x,y,z\right]}^T.$$

(15)

${\tilde{s}}_k$ can be used to obtain reconstructed 2-D DoA $\tilde{\mathit{\theta }}=\left[{\tilde{\mathit{\theta }}}_1,\dots ,{\tilde{\mathit{\theta }}}_K\right]$, where ${\tilde{\mathit{\theta }}}_k={\left[{\tilde{\theta }}_{k,x},{\tilde{\theta }}_{k,z}\right]}^T$ and

$$\left[{\tilde{\theta }}_{k,x},{\tilde{\theta }}_{k,z}\right]=\left[\arctan \left({\tilde{x}}_k/{\tilde{y}}_k\right),\arctan \left({\tilde{z}}_k/{\tilde{y}}_k\right)\right].$$

(16)

We introduced the following cost function, which was the inverse of the squared error between the reconstructed and actual DoA observations:

$$f_1\left(x,y,z,\varphi ,\phi ,\psi \right)=1/{{\displaystyle \sum }}_{k=1}^K{\left({\tilde{\mathit{\theta }}}_k-{\mathit{\theta }}_k\right)}^T\left({\tilde{\mathit{\theta }}}_k-{\mathit{\theta }}_k\right).$$

(17)

By maximizing (17) and imposing the constraints that 1) the translation vector should lie in the area of interest, and 2) the Euler angles are in the range of $\left(0,2\pi \right)$, we could accomplish the DoA-based RBL task via solving:

$$\underset{x,y,z,\varphi ,\phi ,\psi }{\max }{f}_1(x,y,z,\varphi ,\phi ,\psi |\mathit{\theta },d)\mathrm{subject}\mathrm{to}\{\begin{array}{c}x,y,z\in monitoringarea\\ \varphi ,\phi ,\psi \in \left(0,2\pi \right)\end{array}.$$

(18)

3.2. Topology Matching (TM) with Refinement

In this subsection, we introduce the algorithm of topology matching (TM) with refinement. This method had three stages. Stage 1 obtained coarse estimates of the wireless sensor positions, while Stage 2 refined the sensor positions using a WLS algorithm. Finally, Stage 3 conducted an SVD-based position and orientation estimation for the rigid target. It is worthwhile to point out that RBL can actually be achieved by carrying out the first and third steps only, a method which has been proposed in [23] and will be referred to as the existing TM method. However, this technique did not fully explore the noiseless sensor topology information to update the estimated sensor positions before identifying the rigid object’s position and orientation. In this work, we shall utilize WLS minimization to obtain the maximum likelihood (ML) estimate of the wireless sensor coordinates in the current frame in Stage 2. We shall detail the processing in each stage below.

In Stage 1, coarse estimates of the 3-D coordinates of all sensors in the current frame were obtained. According to Figure 1, we can explore the 2-D DoA measurements ${\theta }_{k,x}$ and ${\theta }_{k,z}$ and express the position of the k-th sensor in the current frame in terms of its $y$-axis coordinate as [23]:

$${\widehat{s}}_k={\left[\tan {\theta }_{k,x},1,\tan {\theta }_{k,z}\right]}^T·y_k.$$

(19)

We can see that to determine $S=\left[s_1,\dots ,s_K\right]$, we just needed to find $y={\left[y_1,\dots ,y_K\right]}^T$.

Putting (19) into the definition of the sensor distance vector $d$ given in (4) yielded an estimated version of $d$ given by:

$$\widehat{d}={\left[{\widehat{d}}_{1,2},\dots ,{\widehat{d}}_{K-1,K}\right]}^T,{\widehat{d}}_{i,j}=\left|\right|{\widehat{s}}_i-{\widehat{s}}_j\left|\right|,i,j=1,\cdots ,K,i<j.$$

(20)

Similar to (17), we established a cost function that relied on the inversion of the squared error between the reconstructed and known sensor distances. Mathematically, the cost function is:

$$f_2\left(y\right)=1/{{\displaystyle \sum }}_{i=1}^{K-1}{{\displaystyle \sum }}_{j=i+1}^K{\left(d_{i,j}-{\widehat{d}}_{i,j}\right)}^2.$$

(21)

We then formulated the following constrained maximization problem with respect to $y$ to find the coarse sensor positions in the current frame $\widehat{S}$:

$$\underset{y}{\max }{f}_2\left(y\right)\mathrm{subject}\mathrm{to}y\in monitoringarea.$$

(22)

Note that the sensor positions ${\widehat{s}}_k$ are expressed in terms of the noisy DoA observations ${\theta }_{k,x}$ and ${\theta }_{k,z}$. But this would not be taken into account if we simply searched for the optimal $y$ to solve (22). In this case, the obtained sensor position estimates may be significantly deviated from the true values. We thus applied an estimate refinement stage, Stage 2, to improve $\widehat{S}$. An iterative WLS estimator was adopted here.

To update sensor position estimates from solving (22) and putting the results into (19), we introduced a $\left\{2K+\frac{K\left(K-1\right)}{2}\right\}\times 1$ composite measurement vector $\mathsf{\Theta }={\left[{\mathit{\vartheta }}^T{d}^T\right]}^T$, where $\mathit{\vartheta }={\left[{\theta }_{1,x},\dots ,{\theta }_{K,x},{\theta }_{1,z},\dots ,{\theta }_{K,z}\right]}^T$ collected the Gaussian noise-corrupted DoA observations. We assume the accurate sensor distance vector $d$ was also subject to zero-mean Gaussian noise with a very small variance ${\epsilon }^2$ (say, ${\epsilon }^2$ = ${10}^{-6}$). In the considered DoA-based RBL problem, the true values of these measurements are related to the 3-D coordinates of sensors in the current frame via

$$\begin{array}{c}{\left[{\theta }_{k,x}^o,{\theta }_{k,z}^o\right]}^T=\left[\arctan \left(x_k/y_k\right),\arctan \left(z_k/y_k\right)\right];\\ d_{i,j}=\left|\right|s_i-s_j\left|\right|=\sqrt{{\left(s_i-s_j\right)}^T\left(s_i-s_j\right)}.\end{array}$$

(23)

Expanding (23) around the obtained coarse sensor position estimates in $\widehat{S}$ using the Taylor-series expansion up to the first-order terms, we arrive at:

$$\mathsf{\Theta }\approx \mathsf{\Theta }\left(\mathrm{vec}\left(\widehat{S}\right)\right)+G·\left(\mathrm{vec}\left(S\right)-\mathrm{vec}\left(\widehat{S}\right)\right),$$

(24)

where $\mathrm{vec}\left(S\right)={\left[s_1^T,\dots ,s_K^T\right]}^T$, $\mathrm{vec}\left(\widehat{S}\right)={\left[{\widehat{s}}_1^T,\dots ,{\widehat{s}}_K^T\right]}^T$, and $G\in {ℝ}^{\left\{2K+\frac{K\left(K-1\right)}{2}\right\}\times 3K}$ is the partial derivative of $\mathsf{\Theta }$ with respect to the true sensor positions $S$, evaluated at $\widehat{S}$. It is defined as:

$$G=\left[\begin{array}{c}{\frac{\partial {\mathit{\vartheta }}^T}{\partial \mathrm{vec}\left(S\right)}|}_{S=\widehat{S}}\\ {{\frac{\partial d}{\partial \mathrm{vec}\left(S\right)}}^T|}_{S=\widehat{S}}\end{array}\right].$$

(25)

$\frac{\partial {\mathit{\vartheta }}^T}{\partial \mathrm{vec}\left(S\right)}$ is a $2K\times 3K$ matrix and it is the partial derivative of $\mathit{\vartheta }$ with respect to $\mathrm{vec}\left(S\right)$. $\frac{\partial {\mathit{\vartheta }}^T}{\partial \mathrm{vec}\left(S\right)}$ is equal to:

$$\left[\begin{array}{ccc}\frac{\partial {\theta }_{1,x}}{\partial x_1}\frac{\partial {\theta }_{1,x}}{\partial y_1}\frac{\partial {\theta }_{1,x}}{\partial z_1}& \cdots & \mathbf{0}\\ ⋮& \ddots & ⋮\\ \mathbf{0}& \cdots & \frac{\partial {\theta }_{K,x}}{\partial x_K}\frac{\partial {\theta }_{K,x}}{\partial y_K}\frac{\partial {\theta }_{K,x}}{\partial z_K}\\ \frac{\partial {\theta }_{1,z}}{\partial x_1}\frac{\partial {\theta }_{1,z}}{\partial y_1}\frac{\partial {\theta }_{1,z}}{\partial z_1}& \cdots & \mathbf{0}\\ ⋮& \ddots & ⋮\\ \mathbf{0}& \cdots & \frac{\partial {\theta }_{K,z}}{\partial x_K}\frac{\partial {\theta }_{K,z}}{\partial y_K}\frac{\partial {\theta }_{K,z}}{\partial z_K}\end{array}\right].$$

(26)

${\frac{\partial d}{\partial \mathrm{vec}\left(S\right)}}^T$ has a dimensionality of $\frac{K\left(K-1\right)}{2}\times 3K$. It is indeed the partial derivative of $d$ with respect to $\mathrm{vec}\left(S\right)$, which is given by:

$$\left[\begin{array}{ccc}\frac{\partial d_{1,2}}{\partial x_1}\frac{\partial d_{1,2}}{\partial y_1}\frac{\partial d_{1,2}}{\partial z_1}& \frac{\partial d_{1,2}}{\partial x_2}\frac{\partial d_{1,2}}{\partial y_2}\frac{\partial d_{1,2}}{\partial z_2}& {\mathbf{0}}_{1\times 3(K-2)}\\ ⋮& ⋮& ⋮\\ {\mathbf{0}}_{1\times 3(K-2)}& \frac{\partial d_{(K-1),K}}{\partial x_{K-1}}\frac{\partial d_{(K-1),K}}{\partial y_{K-1}}\frac{\partial d_{(K-1),K}}{\partial z_{K-1}}& \frac{\partial d_{(K-1),K}}{\partial x_K}\frac{\partial d_{(K-1),K}}{\partial y_K}\frac{\partial d_{(K-1),K}}{\partial z_K}\end{array}\right].$$

(27)

From (24), the correction to the coarsely estimated sensor positions in $\widehat{S}$ can be estimated by invoking the WLS technique, which can be shown to be given by:

$$\Delta =\mathrm{vec}\left(S\right)-\mathrm{vec}\left(\widehat{S}\right)={\left(G^T{W}^{-1}G\right)}^{-1}{G}^T{W}^{-1}\left(\mathsf{\Theta }-\mathsf{\Theta }\left(\mathrm{vec}\left(\widehat{S}\right)\right)\right),$$

(28)

where $W$ is the covariance square matrix of measurement noise with the size of $2K+\frac{K\left(K-1\right)}{2}$. From the discussion above (23), we have that $W$ is a block diagonal matrix equal to

$$W=\left[\begin{array}{ccc}\mathrm{diag}\left({\left[{\Sigma }_1\right(1,1),\dots ,{\Sigma }_K(1,1\left)\right]}^T\right)& \mathrm{diag}\left({\left[{\Sigma }_1\right(1,2),\dots ,{\Sigma }_K(1,2\left)\right]}^T\right)& {\mathbf{0}}_{\frac{K(K-1)}{z}\times K}\\ \mathrm{diag}\left({\left[{\Sigma }_1\right(2,1),\dots ,{\Sigma }_K(2,1\left)\right]}^T\right)& \mathrm{diag}\left({\left[{\Sigma }_1\right(2,2),\dots ,{\Sigma }_K(2,2\left)\right]}^T\right)& {\mathbf{0}}_{\frac{K(K-1)}{z}\times K}\\ {\mathbf{0}}_{\frac{K(K-1)}{z}\times K}& {\mathbf{0}}_{\frac{K(K-1)}{z}\times K}& \mathrm{diag}{({\mathbf{1}}_{1\times \frac{K(K-1)}{z}}·{\epsilon }^2)}^T\end{array}\right],$$

(29)

where ${\Sigma }_k\left(m,n\right)$ means the element located at the $m$-row and $n$-column in ${\Sigma }_k$ derived in (12).

With $\Delta$ given in (28), we can refine the initially obtained coarse estimates of the sensor positions via evaluating $\mathrm{vec}\left(\widehat{S}\right)+\Delta$. We repeated the above refinement process by setting $\mathrm{vec}\left(\widehat{S}\right)$ to be equal to $\mathrm{vec}\left(\widehat{S}\right)+\Delta$ until a maximum number of iterations were executed or when ${\Delta }^T\Delta$ was sufficiently small, which indicated convergence.

After refining the sensor position estimates, the proposed TM algorithm proceeded to its Stage 3 processing, where the RBL was accomplished by estimating the rotation matrix $Q$ and translation vector $t$. Using the refined sensor positions and (1), we achieve RBL via solving the following LS problem:

$$\underset{Q,t}{\min }{{\displaystyle \sum }}_{k=1}^K{\left(Q{c}_k+t-{\widehat{s}}_k\right)}^T\left(Q{c}_k+t-{\widehat{s}}_k\right).$$

(30)

Taking partial derivative of the above cost function with respect to $t$ and setting the result to be zero yields:

$$\widehat{t}={{\displaystyle \sum }}_{k=1}^K{\widehat{s}}_k/K-Q{{\displaystyle \sum }}_{k=1}^K{c}_k/K.$$

(31)

Substituting (31) back into (30) transforms the original problem into:

$$\underset{Q}{\min }{{\displaystyle \sum }}_{k=1}^K{\left|\left|Q{c}_k^{\prime }-{\widehat{s}}_k^{\prime }\right|\right|}^2,$$

(32)

where ${\widehat{s}}_k^{\prime }={\widehat{s}}_k-{\displaystyle \sum }_{j=1}^K{\widehat{s}}_j/K$ and $c_k^{\prime }=c_k-{\displaystyle \sum }_{j=1}^K{c}_j/K$. This implied that the centroids of both the current frame $\widehat{S}$ and the reference frame $C$ were set to the origin. Expanding the cost function in (32) leads to:

$$\underset{Q}{\min }{{\displaystyle \sum }}_{k=1}^K{c}_k^{\prime }{}^T{c}_k^{\prime }+{\widehat{s}}_k^{\prime }{}^T{\widehat{s}}_k^{\prime }-2{\widehat{s}}_k^{\prime }{}^{T}Q{c}_k^{\prime }.$$

(33)

The first two items in (33) are known, hence, to solve (33). We just needed to find the rotation matrix $Q$ that maximized the third term ${\widehat{s}}_k^{\prime }{}^{T}Q{c}_k^{\prime }$ under the constraint that $Q$ belonged to $SO\left(3\right)$.

The SVD-based approach was adopted here to determine $Q$. For this purpose, define $H={\displaystyle \sum }_{k=1}^K{c}_k^{\prime }·{\widehat{s}}_k^{\prime }{}^T$ and its SVD is $H=U\Lambda V^{\mathit{T}}$, where $U$ and $V$ are orthonormal matrices and $\Lambda$ is a diagonal matrix whose diagonal elements are the singular values. Then, the optimal rotation matrix estimate $\widehat{Q}$, also the solution to (33), is equal to [16]:

$$\widehat{Q}=V{U}^T.$$

(34)

Putting $\widehat{Q}$ into (31), we have that the estimated translation vector is:

$$\widehat{t}={{\displaystyle \sum }}_{k=1}^K({\widehat{s}}_k-V{U}^T{c}_k)/K.$$

(35)

This completes the RBL process based on the topology matching (TM) approach with refinement.

4. Solving (18) and (22) Using a Participatory Searching Algorithm (PSA)

The two proposed RBL methods both required solving a constrained optimization problem for accomplishing the RBL task. In particular, the OM approach needed to solve (18) for the translation vector and rotation matrix, while for the TM algorithm, the problem in (22) had to be solved to obtain the initial sensor position estimates in the current frame. In this work, we adopted the participatory search algorithm (PSA), an emerging population-based heuristic search algorithm, to find the optimal solutions to (18) and (22). The PSA was motivated by the participatory learning model proposed in [24,32].

The participatory learning diagram mainly consisted of the following three steps (see Figure 2), emulating the general learning process:

Step 1. Calculate the compatibility degree ${\delta }_{w,v}$ between the current knowledge $w\left(t\right)$ and learning information $v\left(t\right)$ for the $t$-th iteration using

$${\delta }_{w,v}(t)=1-{{\displaystyle \sum }}_{n=1}^N|w_n(t)-v_n(t)|/N,$$

(36)

where $N$ is the dimensionality of the unknowns. In this work, $N$ was equal to 6 for the problem in (18), and $v_1,\dots ,v_6$ represented the elements in $\left[x,y,z,\varphi ,\phi ,\psi \right]$. On the other hand, $N$ was equal to $K$ when solving (22), and $v_1,\dots ,v_K$ were indeed $y_1,\dots ,y_K$.

Step 2. Update the arousal index $a_t$ based on the compatibility degree via

$$a_{t+1}=\left(1-\beta \right)a_t+\beta \left(1-{\delta }_{w,v}\right),$$

(37)

where the rate of the arousal change $\beta \in \left(0,1\right)$ is a uniformly distributed random variable. The lower compatibility degree between the current knowledge and learning information was, the less confident the learning system was in the current knowledge, which led to an increased arousal index.

Step 3. Modify the current knowledge based on the compatibility degree, arousal index, the current knowledge, and the incoming information via

$$w\left(t+1\right)=w\left(t\right)+\alpha {\delta }_{w,v}^{1-a_{t+1}}\left[w\left(t\right)-v\left(t\right)\right],$$

(38)

where the uniform random number $\alpha \in \left(0,1\right)$ is the basic learning rate.

The PSA algorithm followed the participatory learning paradigm presented above, and it carried out four processing stages [24], namely Pairing, Selection, Recombination, and Mutation, which are summarized in Algorithm 1. Essentially, the current population and current best individual took the role of the current knowledge in the participatory learning paradigm, while the newly produced population can be considered as the new knowledge.

The Paring operation aimed at finding each individual’s most similar mate according to (36). This encouraged that Selection and Recombination were performed between mates, from which good offspring were more likely to be produced to improve performance. The output of PSA was the individual leading to the maximum value of the associated objective function (see (17) and (21)).

Algorithm 1 Participatory Search Algorithm (PSA)

Initialization: Randomly set $v_{best}$ in the restricted area; $a_t=0$; $t=0$; $f$ is the fitness (objective) function.

do

Randomly generate $M$ individuals $v_1,\dots ,v_M$ in the restricted area and form $M+1$ individuals: $v=\left[v_1,\dots ,v_M,v_{best}\right]$. Each individual contains $N$ unknowns;

Update $v_{best}$ with ${\mathrm{argmax}}_{r}f\left(r\right),r\in \mathbf{v}$;

Pairing:

For each individual $v_m$, find its mate $v_m^{\prime }$ by

$v_m^{\prime }={\mathrm{argmax}}_r{\delta }_{r,v_m},r\in v$, $r\ne v_m$;

Let $v\prime =\left[v_1^{\prime },\dots ,v_{M+1}^{\prime }\right]$;

for $m=1:M$

Selection:

if ${\delta }_{v_m,v_{best}}>{\delta }_{v_m^{\prime },v_{best}}$ then $p_m^s=v_m$;

else $p_m^s=v_m^{\prime }$; end if

Recombination:

Choose $\alpha ,\beta \in \left(0,1\right)$ randomly;

Compute $a_{t+1}=\left(1-\beta \right)a_t+\beta \left[1-{\delta }_{v_m^{\prime },v_m}\right]$;

Generate offspring $p_m^r=v_m+\alpha {\delta }_{v_m^{\prime },v_m}^{1-a_{t+1}}\left(v_m-v_m^{\prime }\right)$;

Mutation:

Get $p_m^t=v_{best}+\alpha {\delta }_{p_m^s,p_m^r}^{1-a_{t+1}}\left(p_m^s-p_m^r\right)$

if $f\left(p_m^t\right)>f\left(v_{best}\right)$ then $v_{best}=p_m^r$; end if

if $f\left(p_m^s\right)>f\left(v_{best}\right)$ then $v_{best}=p_m^s$; end if

end

$t=t+1$;

while $t<t_{max}$

return $v_{best}$

5. Performance Evaluation

Simulations were conducted to evaluate the performance of the proposed two algorithms, namely the OM method and TM algorithm with refinement, both of which achieved the DoA-based RBL with a single base station. Instead of comparing against the ToA-based RBL schemes, we shall investigate the DoA-based RBL performance as a function of the size of the rigid target and the DoA measurement noise level. The existing TM method without the refinement process, which was developed in [23], was simulated as a benchmark technique. The CCRB derived in Section 2.2 was included as well for the purpose of comparison. It is well-known that the CCRB gives a lower bound on the estimation variance of all the unbiased estimators of deterministic parameters with equality constraints. It has been used extensively in localization literature because of its good prediction on the performance limit over a small noise region.

5.1. Simulation Setup

The simulation setup was as follows. The single BS was deployed at the origin. $K=4$ wireless sensors were mounted on the rigid body to be localized. The topology information of the sensors was given in the sensor distance vector $d=D·{\left[1,1,1,\sqrt{2},\sqrt{2},\sqrt{2}\right]}^T$, which indicated that the sensors were distributed on the vertices of an isosceles trirectangular tetrahedron, and $D$ denoted the size of the rigid body. As a result, the reference frame is given by

$$C=\left[\begin{array}{cc}\begin{array}{cc}0& 1\\ 0& 0\\ 0& 0\end{array}& \begin{array}{cc}0& 0\\ 1& 0\\ 0& 1\end{array}\end{array}\right]·D.$$

(39)

In the simulation experiments performed in the next subsection, Section 5.2, we considered the scenarios where $D$ was equal to 1, 1.5 and 2 m.

We set the true Euler angles $\phi$, $\varphi$, and $\psi$ to be 10°, 60°, and −25°, respectively, with respect to the reference frame. The associated rotation matrix $Q$ is thus given by, according to (3),

$$Q=\left[\begin{array}{ccc}0.4532& -0.211& 0.8660\\ 0.5525& 0.8290& -0.087\\ -0.700& 0.5178& 0.4924\end{array}\right].$$

(40)

We assumed a translation vector $t={\left[662\right]}^T$. Accordingly, the ground truth of current frame $S$ of the rigid target will be generated through putting the specified values of $t$ and $Q$ into (1).

The noise of the DoA measurements from sensors in the current frame to the single BS, $\Delta {\alpha }_k$ and $\Delta {\beta }_k$, had the same noise standard deviation of $\sigma$ degrees. The number of ensemble runs $N_{Iter}$ was equal to 500. The estimation root mean squared error (RMSE) of a parameter $\mathit{X}$ was computed using:

$$\mathrm{RMSE}\left(\mathit{X}\right)=\sqrt{\frac{1}{N_{Iter}}{{\displaystyle \sum }}_{n=1}^{N_{Iter}}||\mathit{X}-{\widehat{\mathit{X}}}_n||{}^2},$$

(41)

where ${\widehat{\mathit{X}}}_n$ was the estimate of $\mathit{X}$ in the $n$-th ensemble run. The Euclidean norm was used when $\mathit{X}$ was a vector, and the Frobenius norm applied when $\mathit{X}$ was a matrix. The estimation bias of $\mathit{X}$ was obtained by:

$$\mathrm{Bias}\left(\mathit{X}\right)=||\frac{1}{N_{Iter}}{{\displaystyle \sum }}_{n=1}^{N_{Iter}}\left(\mathit{X}-{\widehat{\mathit{X}}}_n\right)||.$$

(42)

5.2. Simulation Results Evaluation

In Figure 3, Figure 4 and Figure 5, we plotted the RMSEs of the translation vector $t$ (the location information of the rigid target) and the rotation matrix $Q$ (the attitude information of the rigid target) estimates, when the sizes of the rigid target D were set to be 1, 1.5, and 2 m, and $\sigma$ was equal to 10⁻², 10^−1.6, 10^−1.2, 10^−0.8, 10^−0.4 and 1 degrees.

As we expected, the estimation RMSEs decreased as the DoA noise level reduced. In addition, increasing the size of the rigid target D enhanced RBL accuracy. The reason was that the difference between the DoAs from the wireless sensors increased with larger values of $D$, which mitigated the effect of DoA noise on estimation performance.

As shown in Figure 3, Figure 4 and Figure 5, under low DoA noise conditions, the two considered TM-based methods outperformed the OM-based algorithm. Meanwhile, we also observed from the ‘zoom-in’ windows in these figures that the proposed TM technique with WLS-based refinement achieved better RBL accuracy, and its performance was closer to the CCRB than that of the existing TM method in [23]. The level of performance improvement was more evident under smaller rigid body sizes and DoA noise levels. This enhancement mainly came from the improved sensor node positions resulting from the introduction of the additional processing stage, Stage 2, as shown in

Table 1. Comparison of the positioned RMSEs of the wireless nodes.

	Noise	$\mathit{\sigma }={10}^{-2}\mathbf{deg}$	$\mathit{\sigma }={10}^{-1.6}\mathbf{deg}$
Size
$D=1\mathrm{m}$		11.3/8.7 (1.14 dB) 1	26.6/22.3 (0.77 dB)
$D=1.5\mathrm{m}$		8.7/7.3 (0.76 dB)	20.9/16.4 (1.05 dB)
$D=2\mathrm{m}$		7.3/6.5 (0.50 dB)	17.6/15.2 (0.64 dB)

¹ Positioned RMSEs (in mm) of the existing TM/the proposed TM with refinement (improvement in dB).

where the positioned RMSEs of the wireless nodes in the current frame are provided. These results confirmed the effectiveness of the refinement stage.

When the DoA noise level became larger, the estimation performance of both the TM method from [23] and the proposed TM method with refinement exhibited apparent deviation from the CCRB, while the OM-based method offered an accuracy close to the CCRB. This was because the PSA algorithm adopted to solve the TM problem in (22) experienced a decrease in the global convergence rate. We illustrated this point in Figure 6, where the success rate of global convergence of the three methods in consideration was depicted as a function of the DoA measurement noise standard deviation $\sigma$ and the rigid target size D. The proposed OM method achieved a global convergence rate very close to one, even when the rigid target size was $D=1$ m and the DoA noise was as high as $\sigma =1°$, which verified the robustness of the OM technique. On the other hand, the global convergence rates of the two TM techniques were sensitive to the decrease in the rigid target size and/or the increase in the DoA noises. The improved performance of the OM method was because it utilized $2K=8$ DoA measurements for observation matching in (18), while with the TM methods, only $\frac{K\left(K-1\right)}{2}=6$ sensor distances were used for topology matching in (22). Intuitively, the more measurements that were used, the more robust the RBL technique was to measurement noises.

Figure 7 shows the results for the estimation bias. The proposed TM method with refinement yielded the smallest bias in the case of low-level DoA measurement noise, while the proposed OM method performed better when measurement noise was large, which was expected from the obtained RMSE results.

Finally, analysis of the computational costs of the proposed techniques as conducted. The particle swarm optimization (PSO) algorithm was adopted for searching for the solution in the existing TM method [23]. Here, for fairness, we replaced it with PSA. This study was performed using MATLAB on a desktop with a 3.3 GHz CPU and 8GB RAM.

We illustrated the computational cost of the three RBL schemes in consideration when the DoA noise standard deviation was equal to $\sigma ={10}^{-1.6}$ degrees and the size of the rigid target was 1.5 m. We set the PSA iteration number to be 200, and the WLS iteration number in the TM method with refinement was 20. Figure 8 shows the global convergence success rate and execution time as a function of the number of individuals used in PSA.

To ensure convergence, the individual number should be at least 100. In this case, the execution time of the OM-based RBL method was around 2.5 s per ensemble run, and those of the TM-based RBL methods were around 1.5 s. The former technique had higher complexity because it had to jointly estimate six parameters ($x,y,z,\varphi ,\phi ,\psi$), while the TM-based methods only needed to find four parameters. In addition, the newly introduced WLS-based refinement process did not introduce significant increases in the execution times of the TM-based RBL method.

6. Conclusions

In this paper, we considered the problem of DoA-based RBL adopting a single BS. The rigid target is mounted with wireless sensors, whose signal arriving angles can be acquired by the single BS equipped with an LSAA. The known topology information and the measured DoAs of the wireless sensors were fused to determine the position and orientation of the rigid target, with respect to the predetermined reference frame. We proposed two estimation approaches, namely the OM method and TM method with a refinement process. The PSA was adopted for the matching processes in both approaches to find the optimal parameters. We conducted simulations and compared their estimation accuracy with that of an existing benchmark, as well as the CCRB. Results show that the TM method with a refining process performed better in lower level noise conditions, while the OM method was more robust against higher level noise.