Target Localization and Sensor Movement Trajectory Planning with Bearing-Only Measurements in Three Dimensional Space

Abstract

In order to improve the accuracy of bearing-only localization in three dimensional (3D) space, this paper proposes a novel bias compensation method and a new single-sensor maneuvering trajectory algorithm, respectively. Compared with traditional methods, the bias compensation method estimates the unknown variance of bearing noise consistently, which is utilized in pseudo-linear target localization to achieve higher precision. The sensor maneuvering algorithm designs the next moment sensor location in consideration of all the past sensor locations, unlike other approaches that only consider finite past locations. Research shows that the trajectories generated by our algorithm have greater Fisher information matrix (FIM) determinants and better localization accuracy.

1. Introduction

Target localization has applications in both civilian and military domains including wireless communications, environmental monitoring [1], network security [2,3], medical diagnosis, etc. The commonly used methods for target localization can be characterized into four categories according to the type of sensors: bearing-only (also called angle of arrival (AOA)), range-only, time-of-arrival (time-difference-of-arrival) and received signal strength [4]. The problem of using only angle measurements to locate a target is referred to as bearing-only target localization which is the focus of this paper.

The bearing-only target positioning problem can usually be solved in the following two categories. The first category is based on Kalman filtering and its various evolutionary versions [5,6], Kalman filter, extended Kalman filter and unscented Kalman filter have been applied to the bearing-only target localization problem in [7,8]. Due to nonlinear-to-linear transformation of the angle measurement equations, there exists a serious deviation problem. Dogançay et al. [9] propose a new two-step pseudo-linear Kalman filter for bearing-only target tracking in 3D space. In the second category, many batch form methods based on the ordinary least squares are used to deal with target localization under small measurement noise [10,11,12]. To deal with the serious deviation of the pseudo-linear estimator (PLE) in two dimensional (2D) target localization problem, Dogançay [13] proposes a closed-form reduced-bias PLE to realize the asymptotic unbiased estimation of target motion parameters by using the instrumental variable (IV) method. In the case of small noise, Dogançay also proposes pseudo-linear estimator for AOA target motion analysis in 3D space [14]. Adib et al. [15] proposes a two-stage weighted Stansfield geolocation algorithm based on maximum likelihood estimation in 3D space, and it has the advantages of consistency and statistical efficiency. In addition, the position of the sensor has a great influence on positioning accuracy [4,16]. In order to minimize the uncertainty of the target estimation, the position of the sensor at the next moment needs to be reasonably planned. The optimal localization of the sensor is generally determined by maximizing the determinant of FIM or minimizing the trace of error covariance matrix of the estimator [2,17,18,19]. The optimal trajectory of a single mobile sensor is determined by maximizing the determinant of FIM under the condition of adding state constraints on the observer trajectory [20]. When the absolute elevation of the sensor is constant, Sheng et al. [21] proposes a new simple sensor optimal deployment criterion based on minimizing the trace of error covariance matrix. Based on the optimization criterion of minimizing the mean square error (MSE), an own-ship path optimization algorithm by using the gradient-descent method in $xy$ plane and the grid search optimization method in z axis is presented [22]. A distributed path optimization algorithm is applied when the communication distance constraints and no-fly zone are considered [23].

Most estimation methods generally assume that the variance of measurement noise is known a priori for the purpose of bias compensation. In fact, the noise variance is generally unknown and needs to be approximated in advance. The least-square solution for the target dynamics is calculated and plugged into bearing measurement equations to compute the noise in the first step. Then the “true” noise is used to construct the pseudolinear vector and form the final bias-compensation PLE result which is still biased [24]. As the first contribution of this paper, we analyze the properties of Gaussian noise along with the angle measurement equations and newly discover the potential relationship between the determinant of the extended coefficient matrix of the equation and the noise variance. A BC method for target localization on $xy$ plane and the target position in z-axis direction based on the pseudo-linear estimation method is proposed thereafter.

Optimizing the increment of FIM determinant over a finite time horizon rather than the whole time span makes trajectory planning less effective [25]. As the second contribution of this paper, a sensor trajectory planning algorithm based on greedy strategies maximizes the increment between two consecutive FIM determinants to realize the next location of a single sensor. It is proven that the increment equals the aggregation of FIM determinants constituted by the sensor location at next time step and the combination of all past locations. Considering the spherical constraints of target safety area and sensor movement area, the optimal solution always sits on the area surface. Based on this fact, an analytical solution is suggested to solve the maximization problem.

The rest of this paper is organized as follows. Section 2 formulates the problem of target localization in 3D space, including the notations and localization methods. Section 3 proposes a BC estimator and bias compensation weighted instrumental variable (BC-WIV) estimator. In Section 4, the sensor maneuvering algorithm in the composition of two trajectories is designed in detail with the analysis of the FIM increment. Numerical examples are studies to validate the method studies in Section 5. Section 6 concludes the whole paper and points out future research directions.

2. Bearing-Only Localization in 3D Space

As shown in Figure 1, the location of target $\mathbf{T}={\left[\begin{array}{ccc}{x}_t& y_t& z_t\end{array}\right]}^{\prime }$ is unknown in 3D space. T needs to be estimated based on the azimuth ${\widehat{\varphi }}_i$ and elevation angle ${\widehat{\theta }}_i$ measurements collected from a mobile sensor ${\mathbf{S}}_i={\left[\begin{array}{ccc}{x}_{s_i}& y_{s_i}& z_{s_i}\end{array}\right]}^{\prime }$$(i=1,\dots ,n)$ at n locations. t and ${\mathbf{s}}_{\mathbf{i}}$ are the coordinates of the target and the sensor on $xy$ plane, respectively. The true bearing information ${\varphi }_i$ and ${\theta }_i$ received by the sensor from the target are:

$${\varphi }_i={tan}^{-1}\left(\frac{y_t-y_{s_i}}{x_t-x_{s_i}}\right),$$

(1)

$${\theta }_i={tan}^{-1}\left(\frac{z_t-z_{s_i}}{\sqrt{{(x_t-x_{s_i})}^2+{(y_t-y_{s_i})}^2}}\right).$$

(2)

The bearing information ${\widehat{\varphi }}_i$ and ${\widehat{\theta }}_i$ are the sensor measurements corrupted by the noise $e_i$ and ${\delta }_i$ at n locations, i.e.,

$${\widehat{\varphi }}_i={\varphi }_i+e_i,$$

(3)

$${\widehat{\theta }}_i={\theta }_i+{\delta }_i,$$

(4)

where $e_i$ and ${\delta }_i$ are independent identically distributed white Gaussian noise with zero mean and unknown variance of ${\sigma }^2$ and ${\xi }^2$, respectively.

The problem of target localization in 3D space is solved in two steps. Firstly, the bearing line is projected onto the $xy$ plane. The corresponding 2D estimator is used to solve the resulting 2D positioning problem. Then the estimated position of T along the z axis is obtained from the estimated x and y coordinates. The 2D pseudolinear estimator (PLE) algorithm is used to estimate t by using n azimuth measurements ${\widehat{\varphi }}_i$. To turn a nonlinear problem into a linear one, small noise assumption also appears in the development and analysis of the Stansfield algorithm [26] and the total-least-squares solution [27] for localization. The pseudolinear equation from (1) and (3) between t and the given information is described as:

$$sin{\widehat{\varphi }}_i{x}_t-cos{\widehat{\varphi }}_i{y}_t=sin{\widehat{\varphi }}_i{x}_{s_i}-cos{\widehat{\varphi }}_i{y}_{s_i}+\left|\right|\mathbf{t}-{\mathbf{s}}_i\left|\right|sin{e}_i,$$

(5)

which can be expressed in matrix form as:

$${\mathbf{A}}_n\mathbf{t}={\mathbf{B}}_n+{\mathbf{n}}_n$$

(6)

where

$$\begin{array}{cc}\hfill {\mathbf{A}}_n& =\left[\begin{array}{cc}sin{\widehat{\varphi }}_1& -cos{\widehat{\varphi }}_1\\ ⋮& ⋮\\ sin{\widehat{\varphi }}_n& -cos{\widehat{\varphi }}_n\end{array}\right],{\mathbf{B}}_n=\left[\begin{array}{c}sin{\widehat{\varphi }}_1{x}_{s_1}-cos{\widehat{\varphi }}_1{y}_{s_1}\\ ⋮\\ sin{\widehat{\varphi }}_n{x}_{s_n}-cos{\widehat{\varphi }}_n{y}_{s_n}\end{array}\right],\hfill \\ \hfill {\mathbf{n}}_n& =\left[\begin{array}{ccc}\left|\right|\mathbf{t}-{\mathbf{s}}_1\left|\right|& 0& 0\\ 0& \ddots & 0\\ 0& 0& \left|\right|\mathbf{t}-{\mathbf{s}}_n\left|\right|\end{array}\right]\left[\begin{array}{c}sin{e}_1\\ ⋮\\ sin{e}_n\end{array}\right].\hfill \end{array}$$

(7)

The 2D PLE estimator of (6) is:

$${\widehat{\mathbf{t}}}_{\mathbf{p}\mathbf{l}\mathbf{e}}={\left({\mathbf{A}}_n^{{}^{\prime }}{\mathbf{A}}_n\right)}^{-1}{\mathbf{A}}_n^{{}^{\prime }}{\mathbf{B}}_n,$$

(8)

where ${\mathbf{A}}_n^{{}^{\prime }}$ represents the transpose of ${\mathbf{A}}_n$.

The target position of the projection in 2D space has been estimated. According to the geometric relationship, the target position in the z axis direction can be estimated according to the elevation angle measurements and other information as:

$$\begin{array}{c}\hfill z_0=\frac{1}{n}\sum _{i=1}^n\left(z_i+\sqrt{{\left(x_0-x_{s_i}\right)}^2+{\left(y_0-y_{s_i}\right)}^2}tan{\widehat{\theta }}_i\right),\end{array}$$

(9)

where $x_0$ and $y_0$ are the target position estimated by different methods in 2D space.

3. Bias Compensation Estimator for Localization in 3D Space

3.1. Bias Compensation Localization in 2D Space

In [28], a bias compensation method for bearing-only localization is proposed, which can effectively solve the problem of nonlinear localization. Dividing $cos{\widehat{\varphi }}_i$ on both sides of (5) gives:

$$tan{\widehat{\varphi }}_i{x}_t-y_t=tan{\widehat{\varphi }}_i{x}_{s_i}-y_{s_i}+\frac{\left|\right|\mathbf{t}-{\mathbf{s}}_i\left|\right|sin{e}_i}{cos{\widehat{\varphi }}_i}.$$

(10)

Rewrite (10) into the matrix form as:

$${\mathbf{Z}}_n={\overline{\mathbf{W}}}_n\mathbf{t}+{\mathbf{C}}_n{\mathbf{N}}_n,$$

(11)

where

$$\begin{array}{cc}\hfill {\mathbf{Z}}_n& =\left[\begin{array}{c}tan{\widehat{\varphi }}_1{x}_{s_1}-y_{s_1}\\ ⋮\\ tan{\widehat{\varphi }}_n{x}_{s_n}-y_{s_n}\end{array}\right],{\overline{\mathbf{W}}}_n=\left[\begin{array}{cc}tan{\widehat{\varphi }}_1& -1\\ ⋮& ⋮\\ tan{\widehat{\varphi }}_n& -1\end{array}\right],\hfill \end{array}$$

$$\begin{array}{cc}\hfill {\mathbf{C}}_n& =\left[\begin{array}{ccc}{\displaystyle -\frac{\left|\right|\mathbf{t}-{\mathbf{s}}_{\mathbf{1}}\left|\right|}{cos{\widehat{\varphi }}_1}}& 0& 0\\ 0& \ddots & 0\\ 0& 0& {\displaystyle -\frac{\left|\right|\mathbf{t}-{\mathbf{s}}_{\mathbf{n}}\left|\right|}{cos{\widehat{\varphi }}_n}}\end{array}\right],{\mathbf{N}}_n=\left[\begin{array}{c}sin{e}_1\\ ⋮\\ sin{e}_n\end{array}\right].\hfill \end{array}$$

(12)

Based on

$$\begin{array}{cc}\hfill {\widehat{\sigma }}^2& =min\lambda ,\hfill \\ \hfill s.t.& det\left(\frac{1}{n}{\left[\begin{array}{cc}{\overline{\mathbf{W}}}_n& {\mathbf{Z}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\overline{\mathbf{W}}}_n& {\mathbf{Z}}_n\end{array}\right]-\lambda \left[\begin{array}{ccc}1& 0& -\frac{1}{n}\sum _{i=1}^n{x}_{s_i}\\ 0& 0& 0\\ -\frac{1}{n}\sum _{i=1}^n{x}_{s_i}& 0& \frac{1}{n}\sum _{i=1}^n{x}_{s_i}^2\end{array}\right]\right)=0\hfill \end{array}$$

(13)

of ${\sigma }^2$, the Bai estimator is:

$$\begin{array}{c}\hfill \begin{array}{c}\hfill {\widehat{t}}_{{\widehat{\sigma }}^2}={\left(\frac{1}{n}{\overline{\mathbf{W}}}_n^{\prime }{\overline{\mathbf{W}}}_n-{\widehat{\sigma }}^2\left[\begin{array}{cc}1& 0\\ 0& 0\end{array}\right]\right)}^{-1}\left(\frac{1}{n}{\overline{\mathbf{W}}}_n^{\prime }{\mathbf{Z}}_n+{\widehat{\sigma }}^2\left[\begin{array}{c}\frac{1}{n}\sum _{i=1}^n{x}_{s_i}\\ 0\end{array}\right]\right).\end{array}\end{array}$$

(14)

Inspired by [28], a corresponding bias compensation estimator is designed for (5). Firstly, the matrix ${\mathbf{A}}_n$ and ${\mathbf{B}}_n$ are processed as:

$$\frac{1}{n}{\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]=\frac{1}{n}\sum _{i=1}^n\left[\begin{array}{ccc}{a}_{11}^i& a_{12}^i& b_1^i\\ a_{12}^i& a_{22}^i& b_2^i\\ b_1^i& b_2^i& c^i\end{array}\right],$$

(15)

where

$$\begin{array}{cc}\hfill a_{11}^i& ={sin}^2{\varphi }_i{cos}^2{e}_i+2sin{\varphi }_{i}cos{\varphi }_{i}cos{e}_{i}sin{e}_i+{cos}^2{\varphi }_i{sin}^2{e}_i,\hfill \\ \hfill a_{12}^i& =-sin{\varphi }_{i}cos{\varphi }_i{cos}^2{e}_i-{cos}^2{\varphi }_{i}cos{e}_{i}sin{e}_i\hfill \end{array}$$

(16)

$$\begin{array}{cc}\hfill & +{sin}^2{\varphi }_{i}cos{e}_{i}sin{e}_i+cos{\varphi }_{i}sin{\varphi }_i{sin}^2{e}_i,\hfill \end{array}$$

(17)

$$\begin{array}{cc}\hfill a_{22}^i& ={cos}^2{\varphi }_i{cos}^2{e}_i-2cos{\varphi }_{i}sin{\varphi }_{i}cos{e}_{i}sin{e}_i+{sin}^2{\varphi }_i{sin}^2{e}_i,\hfill \end{array}$$

(18)

$$\begin{array}{cc}\hfill b_1^i& =a_{11}^i{x}_{s_i}+a_{12}^i{y}_{s_i},\hfill \end{array}$$

(19)

$$\begin{array}{cc}\hfill b_2^i& =a_{12}^i{x}_{s_i}+a_{22}^i{y}_{s_i},\hfill \end{array}$$

(20)

$$\begin{array}{cc}\hfill c^i& =a_{11}^i{x}_{s_i}^2+2a_{12}^i{x}_{s_i}{y}_{s_i}+a_{22}^i{y}_{s_i}^2.\hfill \end{array}$$

(21)

To derive the bias compensation estimator in detail, two lemmas are proposed.

Lemma 1.

$\mathbb{E}(cos{e}_{i}sin{e}_i)=0$, $\mathbb{E}\left({sin}^2{e}_i\right)=(1-e^{-2{\sigma }^2})/2$, $\mathbb{E}\left({cos}^2{e}_i\right)=(1+e^{-2{\sigma }^2})/2$ where $\mathbb{E}(·)$ represents the mathematical expectation.

Proof of Lemma 1.

Since $cos{e}_{i}sin{e}_i=sin2e_i/2$ is an odd function, we have:

$$\frac{1}{2\sqrt{2\pi }\sigma }{\int }_{-\infty }^{\infty }sin2e_i·e^{-\frac{e_i^2}{2{\sigma }^2}}d{e}_i=0.$$

(22)

From

$$\begin{array}{ccc}\hfill \mathbb{E}({cos}^2{e}_i-{sin}^2{e}_i)& =& \mathbb{E}(cos2e_i)\hfill \\ & =& \sum _{i=0}^{\infty }\frac{{(-1)}^i{2}^{2i}}{\left(2i\right)!}·\frac{1}{\sqrt{2\pi }\sigma }{\int }_{-\infty }^{\infty }{e}_i^{2i}{e}^{-\frac{e_i^2}{2{\sigma }^2}}d{e}_i\hfill \\ & =& \sum _{i=0}^{\infty }\frac{{(-1)}^i{2}^{2i}}{\left(2i\right)!}·\frac{{\sigma }^{2i}\left(2i\right)!}{2^i\left(i\right)!}\hfill \\ & =& \sum _{i=0}^{\infty }\frac{{(-1)}^i{\left(2{\sigma }^2\right)}^i}{\left(i\right)!}\hfill \\ & =& e^{-2{\sigma }^2}\hfill \end{array}$$

(23)

and $\mathbb{E}({cos}^2{e}_i+{sin}^2{e}_i)=1$, $\mathbb{E}\left({sin}^2{e}_i\right)=(1-e^{-2{\sigma }^2})/2$, $\mathbb{E}\left({cos}^2{e}_i\right)=(1+e^{-2{\sigma }^2})/2$. □

Define

$$P\left(\gamma \right)=\frac{1}{n}{\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]-\frac{\gamma }{n}\left[\begin{array}{ccc}n& 0& {\displaystyle \sum _{i=1}^n{x}_{s_i}}\\ 0& n& {\displaystyle \sum _{i=1}^n{y}_{s_i}}\\ {\displaystyle \sum _{i=1}^n{x}_{s_i}}& {\displaystyle \sum _{i=1}^n{y}_{s_i}}& {\displaystyle \sum _{i=1}^n{x}_{s_i}^2+\sum _{i=1}^n{y}_{s_i}^2}\end{array}\right]$$

(24)

as the extended coefficient matrix.

Lemma 2.

${\gamma }_{min}=\mathbb{E}\left({sin}^2{e}_i\right)$ is the smallest real value $s.t.det\left(P\right(\gamma \left)\right)=0$ in probability as $n\to \infty$.

Proof of Lemma 2.

With Lemma 1, we have:

$$\frac{1}{n}{\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\mathbf{A}}_n& {\mathbf{B}}_n\end{array}\right]-P\left({\gamma }_{min}\right)=\frac{{\gamma }_{min}}{n}\left[\begin{array}{ccc}n& 0& {\displaystyle \sum _{i=1}^n{x}_{s_i}}\\ 0& n& {\displaystyle \sum _{i=1}^n{y}_{s_i}}\\ {\displaystyle \sum _{i=1}^n{x}_{s_i}}& {\displaystyle \sum _{i=1}^n{y}_{s_i}}& {\displaystyle \sum _{i=1}^n{x}_{s_i}^2+\sum _{i=1}^n{y}_{s_i}^2}\end{array}\right]$$

in probability where:

$${\displaystyle P\left({\gamma }_{min}\right)=\frac{\mathbb{E}(cos\left(2e_i\right))}{n}\ast \sum _{i=1}^n\left(\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\\ sin{\varphi }_i{x}_{s_i}-cos{\varphi }_i{y}_{s_i}\end{array}\right]{\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\\ sin{\varphi }_i{x}_{s_i}-cos{\varphi }_i{y}_{s_i}\end{array}\right]}^{{}^{\prime }}\right).}$$

(25)

Through observation, $P\left({\gamma }_{min}\right)$ is positive semi-definite. Its null space is one dimensional in the form of: $\beta {\left[\begin{array}{ccc}{x}_t& y_t& -1\end{array}\right]}^{\prime }$, where $\beta \in \mathbb{R}$. Let vector $\mathbf{v}=\left[\begin{array}{ccc}{v}_1& v_2& v_3\end{array}\right]$ be arbitrary in ${\mathbb{R}}^{3\times 1}$. So we have

$${\displaystyle {\mathbf{v}}^{{}^{\prime }}P\left(\gamma \right)\mathbf{v}=v^{{}^{\prime }}P\left({\gamma }_{min}\right)\mathbf{v}-\frac{\gamma -{\gamma }_{min}}{n}\ast {\mathbf{v}}^{{}^{\prime }}\left[\begin{array}{ccc}n& 0& {\displaystyle \sum _{i=1}^n{x}_{s_i}}\\ 0& n& {\displaystyle \sum _{i=1}^n{y}_{s_i}}\\ {\displaystyle \sum _{i=1}^n{x}_{s_i}}& {\displaystyle \sum _{i=1}^n{y}_{s_i}}& {\displaystyle \sum _{i=1}^n{x}_{s_i}^2+\sum _{i=1}^n{y}_{s_i}^2}\end{array}\right]\mathbf{v}.}$$

(26)

The second term

$$\begin{array}{cc}\hfill & \frac{\gamma -{\gamma }_{min}}{n}{\mathbf{v}}^{{}^{\prime }}\left[\begin{array}{ccc}n& 0& {\displaystyle \sum _{i=1}^n{x}_{s_i}}\\ 0& n& {\displaystyle \sum _{i=1}^n{y}_{s_i}}\\ {\displaystyle \sum _{i=1}^n{x}_{s_i}}& {\displaystyle \sum _{i=1}^n{y}_{s_i}}& {\displaystyle .\sum _{i=1}^n{x}_{s_i}^2+\sum _{i=1}^n{y}_{s_i}^2}\end{array}\right]\mathbf{v}\hfill \\ \hfill & =\frac{\gamma -{\gamma }_{min}}{n}\sum _{i=1}^n(v_1^2+2v_1{v}_3{x}_{s_i}+v_2^2+2v_2{v}_3{y}_{s_i}+v_3^2{x}_{s_i}^2+v_3^2{y}_{s_i}^2)\hfill \\ \hfill & =(\gamma -{\gamma }_{min})({(v_1+\frac{v_3}{n}\sum _{i=1}^n{x}_{s_i})}^2+{(v_2+\frac{v_3}{n}\sum _{i=1}^n{y}_{s_i})}^2\hfill \\ \hfill & +\frac{v_3^2}{2n^2}\sum _{i=1}^n\sum _{j=1}^n{(x_{s_i}-x_{s_j})}^2+\frac{v_3^2}{2n^2}\sum _{i=1}^n\sum _{j=1}^n{(y_{s_i}-y_{s_j})}^2)>0,\hfill \end{array}$$

(27)

when $\gamma <{\gamma }_{min}$, the first term ${\mathbf{v}}^{{}^{\prime }}P\left({\gamma }_{min}\right)\mathbf{v}$ in (26) is no less than 0. This makes ${\mathbf{v}}^{{}^{\prime }}P\left(\gamma \right)\mathbf{v}$ always greater than 0. Hence, the minimum $\gamma$ that leads to ${\mathbf{v}}^{{}^{\prime }}P\left(\gamma \right)\mathbf{v}=0$ or $det\left(P\right(\gamma \left)\right)=0$ satisfies $\gamma ={\gamma }_{min}$ when $\mathbf{v}=\beta {\left[\begin{array}{ccc}{x}_t& y_t& -1\end{array}\right]}^{\prime }$. □

It is worthwhile to point out that ${\gamma }_{min}$ can be found by getting the minimum root of the third-order polynomial function $det\left(P\right(\gamma \left)\right)=0$. To sum up, the 2D positioning results with bias compensation can be obtained, as

Theorem 1.

$${\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}={\left(\frac{1}{n}{\mathbf{A}}_n^{{}^{\prime }}{\mathbf{A}}_n-\left[\begin{array}{cc}{\gamma }_{min}& 0\\ 0& {\gamma }_{min}\end{array}\right]\right)}^{-1}\ast \left(\frac{1}{n}{\mathbf{A}}_n^{{}^{\prime }}{\mathbf{B}}_n-\frac{{\gamma }_{min}}{n}\sum _{i=1}^n\left[\begin{array}{c}{x}_{s_i}\\ y_{s_i}\end{array}\right]\right)$$

(28)

converges totin probability as $n\to \infty$ when $\mathbb{E}\left(cos2e_i\right)\ne 0$.

Proof of Theorem 1.

As $n\to \infty$,

$$\frac{1}{n}{\mathbf{A}}_n^{{}^{\prime }}{\mathbf{A}}_n-\left[\begin{array}{cc}{\gamma }_{min}& 0\\ 0& {\gamma }_{min}\end{array}\right]=\frac{\mathbb{E}(cos\left(2e_i\right))}{n}\sum _{i=1}^n\left(\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\end{array}\right]{\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\end{array}\right]}^{{}^{\prime }}\right),$$

(29)

$$\frac{1}{n}{\mathbf{A}}_n^{{}^{\prime }}{\mathbf{B}}_n-\frac{{\gamma }_{min}}{n}\sum _{i=1}^n\left[\begin{array}{c}{x}_{s_i}\\ y_{s_i}\end{array}\right]=\frac{\mathbb{E}(cos\left(2e_i\right))}{n}\sum _{i=1}^n\left(\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\end{array}\right]{\left[\begin{array}{c}sin{\varphi }_i\\ -cos{\varphi }_i\end{array}\right]}^{{}^{\prime }}\right)\left[\begin{array}{c}{x}_t\\ y_t\end{array}\right].$$

(30)

From (29) and (30), Theorem 1 stands when $\mathbb{E}(cos2e_i)\ne 0$. □

3.2. Bias Compensation Method in Z-Axis

The geometric relationship in 3D space is:

$$z_{t}cos{\widehat{\theta }}_i={\widehat{r}}_{i}sin{\widehat{\theta }}_i+z_{i}cos{\widehat{\theta }}_i,$$

(31)

where ${\widehat{r}}_i=\sqrt{{\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}\left(1\right)-x_{s_i}\right)}^2+{\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}\left(2\right)-y_{s_i}\right)}^2}$. Rewrite (31) into the matrix form

$${\mathbf{Y}}_n{z}_t={\mathbf{F}}_n,$$

(32)

where

$${\mathbf{Y}}_n=\left[\begin{array}{c}cos{\widehat{\theta }}_1\\ ⋮\\ cos{\widehat{\theta }}_n\end{array}\right],{\mathbf{F}}_n=\left[\begin{array}{c}{z}_{s_1}cos{\widehat{\theta }}_1+{\widehat{r}}_{1}sin{\widehat{\theta }}_1\\ ⋮\\ z_{s_n}cos{\widehat{\theta }}_n+{\widehat{r}}_{n}sin{\widehat{\theta }}_n\end{array}\right].$$

(33)

Similarly, we handle the matrix ${\mathbf{Y}}_n$ and ${\mathbf{F}}_n$,

$$\frac{1}{n}{\left[\begin{array}{cc}{\mathbf{Y}}_n& {\mathbf{F}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\mathbf{Y}}_n& {\mathbf{F}}_n\end{array}\right]=\frac{1}{n}\sum _{i=1}^n\left[\begin{array}{cc}{d}_1^i& d_2^i\\ d_2^i& d_3^i\end{array}\right],$$

(34)

where

$$\begin{array}{cc}\hfill d_1^i& ={cos}^2{\theta }_i{cos}^2{\delta }_i-2cos{\theta }_{i}sin{\theta }_{i}cos{\delta }_{i}sin{\delta }_i+{sin}^2{\theta }_i{sin}^2{\delta }_i,\hfill \\ \hfill d_2^i& =z_{s_i}{d}_1^i+{\widehat{r}}_i(sin{\theta }_{i}cos{\theta }_i{cos}^2{\delta }_i-{sin}^2{\theta }_{i}cos{\delta }_{i}sin{\delta }_i\hfill \end{array}$$

(35)

$$\begin{array}{cc}\hfill & +{cos}^2{\theta }_{i}cos{\delta }_{i}sin{\delta }_i-sin{\theta }_{i}cos{\theta }_i{sin}^2{\delta }_i),\hfill \\ \hfill d_3^i& =z_{s_i}^2{d}_1^i+2z_{s_i}{\widehat{r}}_i(sin{\theta }_{i}cos{\theta }_i{cos}^2{\delta }_i-{sin}^2{\theta }_{i}cos{\delta }_{i}sin{\delta }_i+{cos}^2{\theta }_{i}cos{\delta }_{i}sin{\delta }_i\hfill \end{array}$$

(36)

$$\begin{array}{cc}\hfill & -sin{\theta }_{i}cos{\theta }_i{sin}^2{\delta }_i)+{\widehat{r}}_i^2({sin}^2{\theta }_i{cos}^2{\delta }_i+2sin{\theta }_{i}cos{\theta }_{i}sin{\delta }_{i}cos{\delta }_i+{cos}^2{\theta }_i{sin}^2{\delta }_i).\hfill \end{array}$$

(37)

We also define

$$Q\left(\mu \right)=\frac{1}{n}{\left[\begin{array}{cc}{\mathbf{Y}}_n& {\mathbf{F}}_n\end{array}\right]}^{\prime }\left[\begin{array}{cc}{\mathbf{Y}}_n& {\mathbf{F}}_n\end{array}\right]-\frac{\mu }{n}\left[\begin{array}{cc}n& {\displaystyle \sum _{i=1}^n{z}_{s_i}}\\ {\displaystyle \sum _{i=1}^n{z}_{s_i}}& {\displaystyle \sum _{i=1}^n{z_{s_i}}^2+\sum _{i=1}^n{{\widehat{r}}_i}^2}\end{array}\right].$$

(38)

According to Lemma 2, the same reasoning process can be concluded. ${\mu }_{min}=\mathbb{E}\left({sin}^2{\delta }_i\right)$ is the smallest real value $s.t.det\left(Q\right(\mu \left)\right)=0$ in probability as $n\to \infty$. The difference is ${\mu }_{min}$ can be found by getting the minimum root of a second-order equation $det\left(Q\right(\mu \left)\right)=0$.

Theorem 2.

$${\widehat{z}}_{\mathbf{b}\mathbf{c}}={\left(\frac{1}{n}{\mathbf{Y}}_n^{{}^{\prime }}{\mathbf{Y}}_n-{\mu }_{min}\right)}^{-1}\left(\frac{1}{n}{\mathbf{Y}}_n^{{}^{\prime }}{\mathbf{F}}_n-\frac{{\mu }_{min}}{n}\sum _{i=1}^n{z}_{s_i}\right)$$

(39)

converges to $z_t$ in probability as $n\to \infty$ when $\mathbb{E}\left(\mathbf{c}os2{\delta }_i\right)\ne 0$.

Proof of Theorem 2. 

As $n\to \infty$,

$$\frac{1}{n}{\mathbf{Y}}_n^{{}^{\prime }}{\mathbf{Y}}_n-{\mu }_{min}=\frac{\mathbb{E}(cos\left(2{\delta }_i\right))}{n}\sum _{i=1}^n\left(cos{\theta }_i\ast cos{\theta }_i\right),$$

(40)

$$\frac{1}{n}{\mathbf{Y}}_n^{{}^{\prime }}{\mathbf{F}}_n-\frac{{\mu }_{min}}{n}\sum _{i=1}^n{z}_{s_i}=\frac{\mathbb{E}(cos\left(2{\delta }_i\right))}{n}\sum _{i=1}^n\left(cos{\theta }_i\ast cos{\theta }_i\ast z_t\right).$$

(41)

From (40) and (41), Theorem 2 stands when $\mathbb{E}(cos2{\delta }_i)\ne 0$. □

From (28) and (39), the 3D bias compensation estimator result is ${\widehat{\mathbf{T}}}_{\mathbf{b}\mathbf{c}}={\left[\begin{array}{c}{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}};{\widehat{z}}_{\mathbf{b}\mathbf{c}}\end{array}\right]}^{\prime }$.

3.3. BC-WIV Estimator in 3D Space

To improve the accuracy, the researchers often apply the weighted instrumental variable algorithm by constructing instrumental variable matrix ${\mathbf{G}}_{\mathbf{w}\mathbf{i}\mathbf{v}}$ and weighted matrix ${\mathbf{W}}_{\mathbf{w}\mathbf{i}\mathbf{v}}$,

$${\mathbf{G}}_{\mathbf{w}\mathbf{i}\mathbf{v}}=\left[\begin{array}{cc}sin{\overline{\varphi }}_1& -cos{\overline{\varphi }}_1\\ ⋮& ⋮\\ sin{\overline{\varphi }}_n& -cos{\overline{\varphi }}_n\end{array}\right],{\mathbf{W}}_{\mathbf{w}\mathbf{i}\mathbf{v}}=\left[\begin{array}{ccc}\left|\right|\mathbf{t}-{\mathbf{s}}_1{\left|\right|}^2& 0& 0\\ 0& \ddots & 0\\ 0& 0& \left|\right|\mathbf{t}-{\mathbf{s}}_n{\left|\right|}^2\end{array}\right],$$

(42)

where ${\overline{\varphi }}_i$ is calculated from 2D PLE estimator via (8) and

$${\overline{\varphi }}_i={tan}^{-1}\left(\frac{{\widehat{\mathbf{t}}}_{\mathbf{p}\mathbf{l}\mathbf{e}}\left(2\right)-y_{s_i}}{{\widehat{\mathbf{t}}}_{\mathbf{p}\mathbf{l}\mathbf{e}}\left(1\right)-x_{s_i}}\right).$$

(43)

Then the 2D weighted instrumental variable estimator can be expressed as:

$${\widehat{\mathbf{t}}}_{\mathbf{w}\mathbf{i}\mathbf{v}}={\left({\mathbf{G}}_{\mathbf{w}\mathbf{i}\mathbf{v}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{w}\mathbf{i}\mathbf{v}}^{-1}{\mathbf{A}}_n\right)}^{-1}{\mathbf{G}}_{\mathbf{w}\mathbf{i}\mathbf{v}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{w}\mathbf{i}\mathbf{v}}^{-1}{\mathbf{B}}_n.$$

(44)

Similarly, the accuracy of bias compensation method in 2D space can be improved by constructing matrix,

$${\mathbf{G}}_{\mathbf{b}\mathbf{c}}=\left[\begin{array}{cc}sin{\varphi }_1^{\prime }& -cos{\varphi }_1^{\prime }\\ ⋮& ⋮\\ sin{\varphi }_n^{\prime }& -cos{\varphi }_n^{\prime }\end{array}\right],{\mathbf{W}}_{\mathbf{b}\mathbf{c}}=\left[\begin{array}{ccc}\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_1{\left|\right|}^2& 0& 0\\ 0& \ddots & 0\\ 0& 0& \left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_n{\left|\right|}^2\end{array}\right],$$

(45)

where

$${\varphi }_i^{\prime }={tan}^{-1}\left(\frac{{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}\left(2\right)-y_{s_i}}{{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}\left(1\right)-x_{s_i}}\right).$$

(46)

The 2D bias compensation weighted instrumental variable estimator is:

$${\widehat{t}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}={\left({{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}\mathbf{A}}_n\right)}^{-1}{{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}\mathbf{B}}_n.$$

(47)

Here

$$\frac{1}{n}{{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{\ast }{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}\mathbf{A}}_n\approx \frac{\mathbb{E}(cos{e}_i)}{n}\sum _{i=1}^n\left[\begin{array}{cc}{\displaystyle \frac{{sin}^2{\varphi }_i}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}& {\displaystyle -\frac{sin{\varphi }_{i}cos{\varphi }_i}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}\\ {\displaystyle -\frac{sin{\varphi }_{i}cos{\varphi }_i}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}& {\displaystyle \frac{{cos}^2{\varphi }_i}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}\end{array}\right],$$

(48)

$$\frac{1}{n}{{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{\ast }{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}\mathbf{B}}_n\approx \frac{\mathbb{E}(cos{e}_i)}{n}\sum _{i=1}^n\left[\begin{array}{c}{\displaystyle \frac{{sin}^2{\varphi }_i{x}_{s_i}-sin{\varphi }_{i}cos{\varphi }_i{y}_{s_i}}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}\\ {\displaystyle \frac{-sin{\varphi }_{i}cos{\varphi }_i{x}_{s_i}+{cos}^2{\varphi }_i{y}_{s_i}}{\left|\right|{\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{s}}_{\mathbf{i}}{\left|\right|}^2}}\end{array}\right]$$

(49)

in probability as $n\to \infty$. Hence ${\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}$ is an asymptotically unbiased estimator. Its covariance

$$\begin{array}{cc}\hfill & \mathbb{E}\left(\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}-\mathbf{t}\right){\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}-\mathbf{t}\right)}^{{}^{\prime }}\right)\hfill \\ \hfill & \approx {\left(\frac{1}{n}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}\right)}^{-1}\left(\frac{1}{n^2}{{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}}^{-1}\mathbb{E}\left(({\mathbf{B}}_n-{\mathbf{A}}_n\mathbf{t}){({\mathbf{B}}_n-{\mathbf{A}}_n\mathbf{t})}^{{}^{\prime }}\right){{\mathbf{W}}_{\mathbf{b}\mathbf{c}}}^{-1}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}\right){\left(\frac{1}{n}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}\right)}^{-1}\hfill \\ \hfill & \approx \frac{\mathbb{E}\left({sin}^2{e}_i\right)}{n}{\left(\frac{1}{n}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{\mathbf{b}\mathbf{c}}^{-1}{\mathbf{G}}_{\mathbf{b}\mathbf{c}}\right)}^{-1}\hfill \\ \hfill & \approx \mathbf{C}\mathbf{R}\mathbf{L}\mathbf{B}\hfill \end{array}$$

(50)

when $\mathbb{E}\left({sin}^2{e}_i\right)$ is small as $n\to \infty$. In z-axis direction, by constructing matrix

$${\mathbf{G}}_{z\mathbf{b}\mathbf{c}}=\left[\begin{array}{c}cos{\overline{\theta }}_1\\ ⋮\\ cos{\overline{\theta }}_n\end{array}\right],{\mathbf{W}}_{z\mathbf{b}\mathbf{c}}=\left[\begin{array}{ccc}\left|\right|{\widehat{\mathbf{T}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{S}}_1{\left|\right|}^2& 0& 0\\ 0& \ddots & 0\\ 0& 0& \left|\right|{\widehat{\mathbf{T}}}_{\mathbf{b}\mathbf{c}}-{\mathbf{S}}_{\mathbf{n}}{\left|\right|}^2\end{array}\right],$$

(51)

where

$$\begin{array}{cc}\hfill {\overline{\theta }}_i& ={tan}^{-1}\left(\frac{{\widehat{z}}_{bc}-z_{s_i}}{{\widehat{r}}_{bc-wiv}}\right),\hfill \end{array}$$

(52)

$$\begin{array}{cc}\hfill {\widehat{r}}_{bc-wiv}& =\sqrt{{\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}\left(1\right)-x_{s_i}\right)}^2+{\left({\widehat{\mathbf{t}}}_{\mathbf{b}\mathbf{c}-\mathbf{w}\mathbf{i}\mathbf{v}}\left(2\right)-y_{s_i}\right)}^2}.\hfill \end{array}$$

(53)

The bias compensation weighted instrumental variable estimator in z axis is:

$${\widehat{z}}_{bc-wiv}={\left({\mathbf{G}}_{z\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{z\mathbf{b}\mathbf{c}}^{-1}{\mathbf{Y}}_{\mathbf{n}}\right)}^{-1}{{\mathbf{G}}_{z\mathbf{b}\mathbf{c}}^{{}^{\prime }}{\mathbf{W}}_{z\mathbf{b}\mathbf{c}}^{-1}\mathbf{F}}_n.$$

(54)

4. Sensor Trajectory Design

In this section we propose the one-step-ahead sensor maneuvering strategies for ${\mathbf{S}}_{k+1}$$(n\ge k\ge 3)$ based on ${\mathbf{S}}_1,\dots ,{\mathbf{S}}_k$ and discuss the properties of such strategies. It is worth pointing out beforehand that ${\mathbf{S}}_1,\dots ,{\mathbf{S}}_3$ are prefixed for motion model initialization and do not need to be designed.

4.1. Constraints

As shown in Figure 2, sensor trajectory planning is implemented under the following constraints [17,29]:

Target safety area: In order to ensure no collision between the sensor and the target, the distance between ${\mathbf{S}}_{k+1}$ and ${\widehat{\mathbf{T}}}^k$ should be greater than or equal to the safe distance R.

Sensor motion area: Since the motion of the sensor is limited by the maximum speed, the sensor motion area is set to be spherical centering at ${\mathbf{S}}_k$ with the radius of r (including the boundary of the sphere). Their boundaries are governed by polynomial equations $f_{tsb}(x_{s_{k+1}},y_{s_{k+1}},z_{s_{k+1}};{\widehat{\mathbf{T}}}^k)=0$ and $f_{smb}(x_{s_{k+1}},y_{s_{k+1}},z_{s_{k+1}};{\mathbf{S}}_k)=0$, where

$$f_{tsb}(x_{s_{k+1}},y_{s_{k+1}},z_{s_{k+1}};{\widehat{\mathbf{T}}}^k)=\left|\right|{\mathbf{S}}_{k+1}-{\widehat{\mathbf{T}}}^k{\left|\right|}^2-R^2,$$

(55)

$$f_{smb}(x_{s_{k+1}},y_{s_{k+1}},z_{s_{k+1}};{\mathbf{S}}_k)=\left|\right|{\mathbf{S}}_{k+1}-{\mathbf{S}}_k{\left|\right|}^2-r^2.$$

(56)

4.2. Sensor Trajectory Planning Strategies

The position of sensor is deployed to improve the subsequent target positioning accuracy. According to [19], define:

$${\mathbf{M}}_1=\frac{1}{{\sigma }^2}\sum _{i=1}^{k+1}\frac{1}{\left|\right|{t-s}_i{\left|\right|}^2}\left[\begin{array}{ccc}{sin}^2{\varphi }_i& -\frac{1}{2}sin2{\varphi }_i& 0\\ -\frac{1}{2}sin2{\varphi }_i& {cos}^2{\varphi }_i& 0\\ 0& 0& 0\end{array}\right],$$

(57)

$${\mathbf{M}}_2=\frac{1}{{\xi }^2}\sum _{i=1}^{k+1}\frac{1}{\left|\right|{\mathbf{T}-\mathbf{S}}_i{\left|\right|}^2}\left[\begin{array}{ccc}{sin}^2{\theta }_i{cos}^2{\varphi }_i& \frac{1}{2}{sin}^2{\theta }_{i}sin2{\varphi }_i& -\frac{1}{2}sin2{\theta }_{i}cos{\varphi }_i\\ \frac{1}{2}{sin}^2{\theta }_{i}sin2{\varphi }_i& {sin}^2{\theta }_i{sin}^2{\varphi }_i& -\frac{1}{2}sin2{\theta }_{i}sin{\varphi }_i\\ -\frac{1}{2}sin2{\theta }_{i}cos{\varphi }_i& -\frac{1}{2}sin2{\theta }_{i}sin{\varphi }_i& {cos}^2{\theta }_i\end{array}\right],$$

(58)

where the matrix ${\mathbf{M}}_1$ represents the azimuth FIM over $k+1$ measurements, as it corresponds to the 2D projection of the localization problem onto $xy$ plane, and the matrix ${\mathbf{M}}_2$ represents the elevation FIM over $k+1$ measurements. Therefore, the FIM over $k+1$ measurements in 3D space is:

$$\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}={\mathbf{M}}_{\mathbf{1}}+{\mathbf{M}}_{\mathbf{2}}.$$

(59)

Lemma 3.

$$det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)=\sum _{\{p_1,p_2,p_3\}}^{\left(\genfrac{}{}{0pt}{}{2(k+1)}{3}\right)}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{1}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{2}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{3}}}\right),$$

(60)

where the set $\{p_1,p_2,p_3\}$ is the combination selected from $\{1,\dots ,2(k+1\left)\right\}$.

${\mathbf{V}}_{2\mathbf{i}-\mathbf{1}}$ and ${\mathbf{V}}_{\mathbf{2}\mathbf{i}}$$(i=1,2,3,...,k+1)$ are the azimuth FIM and the elevation FIM, respectively.

Proof of Lemma 3. 

By the definition of matrix determinant, we have:

$$\begin{array}{cc}\hfill det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)& =\sum _{\{p_1,p_2,p_3\}}^{\left(\genfrac{}{}{0pt}{}{2(k+1)}{3}\right)}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{1}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{2}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{3}}}\right)\hfill \\ \hfill & +{\mathsf{\Psi }}_1\sum _{\{p_4,p_5\}}^{\left(\genfrac{}{}{0pt}{}{2(k+1)}{2}\right)}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{4}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{5}}}\right)+{\mathsf{\Psi }}_2\sum _{\left\{p_6\right\}}^{\left(\genfrac{}{}{0pt}{}{2(k+1)}{1}\right)}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{6}}}\right),\hfill \end{array}$$

(61)

where the set $\{p_4,p_5\}$ and $\left\{p_6\right\}$ are the combinations selected from $\{1,\dots ,2(k+1\left)\right\}$. ${\Psi }_1$ and ${\Psi }_2$ are constant factors decided by the combination numbers. Under $\mathrm{r}\mathrm{a}\mathrm{n}\mathrm{k}\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{4}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{5}}}\right)\le 2$ and $\mathrm{rank}\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{6}}}\right)=1$, $det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{4}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{5}}}\right)=det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{6}}}\right)=0$. Then (61) turns into (60). □

Theorem 3.

$$\begin{array}{cc}\hfill det\left({\mathbf{F}\mathbf{I}\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)& =det\left({\mathbf{F}\mathbf{I}\mathbf{M}}_{\mathbf{k}}\right)+\sum _{\{p_7,p_8\}}^{\left(\genfrac{}{}{0pt}{}{2k}{2}\right)}det\left({\mathbf{V}}_{\mathbf{2}\mathbf{k}+\mathbf{1}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{7}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{8}}}\right)\hfill \end{array}$$

(62)

$$\begin{array}{cc}\hfill & +\sum _{\{p_9,p_{10}\}}^{\left(\genfrac{}{}{0pt}{}{2k+1}{2}\right)}det\left({\mathbf{V}}_{\mathbf{2}(\mathbf{k}+\mathbf{1})}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{9}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{10}}}\right),\hfill \end{array}$$

(63)

where the set $\{p_7,p_8\}$ and $\{p_9,p_{10}\}$ are the combination selected from $\{1,\dots ,2k\}$ and $\{1,\dots ,2$$(k+1)\}$.

Proof of Theorem 3. 

From Lemma 3,

$$det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)=\sum _{\{p_{11},p_{12},p_{13}\}}^{\left(\genfrac{}{}{0pt}{}{2k}{3}\right)}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{11}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{12}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{13}}}\right),$$

(64)

where the set $\{p_{11},p_{12},p_{13}\}$ is the combination selected from $\{1,\dots 2k\}$.

$$\begin{array}{cc}\hfill det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)& =\sum _{\{p_7,p_8\}}^{\left(\genfrac{}{}{0pt}{}{2k}{2}\right)}det\left({\mathbf{V}}_{\mathbf{2}\mathbf{k}+\mathbf{1}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{7}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{8}}}\right)\hfill \end{array}$$

(65)

$$\begin{array}{cc}\hfill & +\sum _{\{p_9,p_{10}\}}^{\left(\genfrac{}{}{0pt}{}{2k+1}{2}\right)}det\left({\mathbf{V}}_{\mathbf{2}(\mathbf{k}+\mathbf{1})}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{9}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{10}}}\right)\hfill \end{array}$$

(66)

naturally follows. □

Let the location ${\mathbf{S}}_{k+1}$ on the trajectory is generated pointwisely through the maximization of

$$\begin{array}{cc}\hfill \underset{{\mathbf{S}}_{\mathbf{4}},\dots {\mathbf{S}}_{\mathbf{n}}}{max}det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{n}}\right)\ge \sum _{k=4}^n& \underset{\begin{array}{c}{\mathbf{S}}_{\mathbf{k}+\mathbf{1}}\end{array}}{max}(det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right))+det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right),\hfill \end{array}$$

(67)

which shows the trajectory is suboptimal. According to Theorem 3, it is still a reasonable choice because the design of ${\mathbf{S}}_{k+1}$ considers information given by ${\mathbf{S}}_{k+1}$ and all the past sensor locations jointly. Defining

$$f\left({\mathbf{S}}_{k+1}\right)=det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$$

(68)

as the objective function of solving ${\mathbf{S}}_{k+1}$. Formally defining

Trajectory 1.

the location${\mathbf{S}}_{k+1}$on the trajectory is generated pointwisely through the maximization of

$$\begin{array}{c}\hfill \underset{\begin{array}{c}{\mathbf{S}}_{\mathbf{k}+\mathbf{1}}\end{array}}{max}f\left({\mathbf{S}}_{k+1}\right)\end{array}$$

(69)

$$\begin{array}{cc}\hfill s.t.\left|\right|{\mathbf{S}}_{k+1}-{\mathbf{S}}_k\left|\right|& ⩽r,\hfill \end{array}$$

(70)

$$\begin{array}{cc}\hfill \left|\right|{\mathbf{S}}_{k+1}-{\mathbf{T}}^k\left|\right|& ⩾R\hfill \end{array}$$

(71)

with respect to${\mathbf{S}}_{k+1}$given ${\mathbf{S}}_1,\dots ,{\mathbf{S}}_k$$(k⩾3)$.

When optimizing $f\left({\mathbf{S}}_{k+1}\right)$, unknown T, ${\varphi }_i$, ${\theta }_i$, ${\sigma }^2$ and ${\xi }^2$$(i=1,\dots ,k)$ are replaced by the estimated ${\widehat{\mathbf{T}}}^k$, ${\widehat{\varphi }}_i\left({\widehat{\mathbf{T}}}^k\right)$ and ${\widehat{\theta }}_i\left({\widehat{\mathbf{T}}}^k\right)$, ${\widehat{\sigma }}^2\left({\widehat{\mathbf{T}}}^k\right)$ and ${\widehat{\xi }}^2\left({\widehat{\mathbf{T}}}^k\right)$. This method updates target location and re-estimates all the historical data in each step. To circumvent this weakness, we present

Trajectory 2.

the location${\mathbf{S}}_{k+1}$ on the trajectory is generated pointwisely through the maximization of

$$\begin{array}{c}\hfill \underset{\begin{array}{c}{\mathbf{S}}_{k+1}\end{array}}{max}\widehat{f}\left({\mathbf{S}}_{k+1}\right)\end{array}$$

(72)

$$\begin{array}{cc}\hfill s.t.\left|\right|{\mathbf{S}}_{k+1}-{\mathbf{S}}_k\left|\right|& ⩽r,\hfill \end{array}$$

(73)

$$\begin{array}{cc}\hfill \left|\right|{\mathbf{S}}_{k+1}-{\mathbf{T}}^k\left|\right|& ⩾R\hfill \end{array}$$

(74)

with respect to${\mathbf{S}}_{k+1}$given ${\mathbf{S}}_1,\dots ,{\mathbf{S}}_k$$(k⩾3)$.

When optimizing $\widehat{f}\left({\mathbf{S}}_{k+1}\right)$, ${\sigma }^2$ and ${\xi }^2$ are replaced by $({\sum }_{i=1}^k{\widehat{\sigma }}_i^2\left({\widehat{\mathbf{T}}}^{i-1}\right)+{\widehat{\sigma }}_{k+1}^2\left({\widehat{\mathbf{T}}}^k\right))/(k+1)$ and $({\sum }_{i=1}^k{\widehat{\xi }}_i^2\left({\widehat{\mathbf{T}}}^{i-1}\right)+{\widehat{\xi }}_{k+1}^2\left({\widehat{\mathbf{T}}}^k\right))/(k+1)$, respectively. Distinct with trajectory 1, ${\widehat{\varphi }}_i$ and ${\widehat{\theta }}_i$$(i=1,\dots ,k)$ are not updated by ${\widehat{\mathbf{T}}}^k$.

In Trajectory 1 and 2, the computation time to generate the sensor position at the $(k+1)$th moment includes the time to obtain the objective function and the time to optimize the objective function. Trajectory 1 needs $(k+1)\ast$(49 additions/multiplications, three square roots, nine trigonometric function calculations) and 18 additions/multiplications for the calculation of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$ to obtain the objective function, while trajectory 2 requires only 49 additions/multiplications, three square roots, nine trigonometric functions and 18 additions/multiplications for the calculation of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$. The generation of trajectory 2 reduces $k\ast ($49 additions/multiplications, three square root, nine trigonometric function calculations) calculations compared to trajectory 1. In addition, the objective function structure is similar, the optimization time of the objective function is almost equal depending on the optimization method.

4.3. Optimal Solution Region

Through the analysis of (62), we have:

Theorem 4.

$$det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)=\sum _{\begin{array}{c}{\mathbf{c}}_i\in \{p_7,p_8,p_9\}\\ {\mathbf{c}}_i\notin \{p_{13},p_{14}\}\end{array}}\sum _{j_i\in \{1,\dots 3\}}\left(\prod _{i=1}^{3}sgn\left(j\right)L_{i,j_i}^{\left({\mathbf{c}}_i\right)}\right),$$

(75)

where the set $\{p_7,p_8,p_9\}$ is the combination selected from $\{1,\dots 2(k+1\left)\right\}$ with $\exists p_j(7\le j\le 9)\in \{2k+1,2(k+1)\}$. The set $\{p_{13},p_{14}\}$ is the combination selected from $\{p_7,p_8,p_9\}$. j represents the permutation of $\{j_1,j_2,j_3\}$. The sign function $sgn\left(j\right)=-1$ when the number of exchanges that $\{j_1,j_2,j_3\}$ takes to be the standard order $\{1,2,3\}$ is odd. Otherwise, $sgn\left(j\right)=1$. $L_{i,j_i}^{\left({\mathbf{c}}_i\right)}$ is the ith row and $j_i$th column element in${\mathit{V}}_{{\mathit{c}}_{\mathit{i}}}$.

Proof of Theorem 4. 

From Theorem 3 and the Leibniz formula,

$$det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)=\sum _{\begin{array}{c}{\mathbf{c}}_i\in \{p_7,p_8,p_9\}\end{array}}\sum _{j_i\in \{1,\dots 3\}}\left(\prod _{i=1}^{3}sgn\left(j\right)L_{i,j_i}^{\left(c_i\right)}\right).$$

(76)

In addition,

$$\sum _{\{p_{13},p_{14}\}}det\left({\mathbf{V}}_{{\mathbf{p}}_{\mathbf{13}}}+{\mathbf{V}}_{{\mathbf{p}}_{\mathbf{14}}}\right)=\sum _{c_i\in \{p_{13},p_{14}\}}\sum _{j_i\in \{1,\dots 3\}}\left(\prod _{i=1}^{3}sgn\left(j\right)L_{i,j_i}^{\left({\mathbf{c}}_i\right)}\right)=0.$$

(77)

Deducting (77) from (76) leads to (75). □

From Theorem 4, (62) can be rewritten uniformly into:

$$f\left({\mathbf{S}}_{k+1}\right)=\frac{{\displaystyle \sum _{h=0}^4\sum _{q=0}^4\sum _{l=0}^2{\alpha }_{hql}{x}_{{\mathbf{s}}_{k+1}}^h{y}_{{\mathbf{s}}_{k+1}}^q{z}_{{\mathbf{s}}_{k+1}}^l}}{{\displaystyle \sum _{h^{+}=0}^{10}\sum _{q^{+}=0}^{10}\sum _{l^{+}=0}^6{\beta }_{h^{+}{q}^{+}{l}^{+}}{x}_{{\mathbf{s}}_{k+1}}^{h^{+}}{y}_{{\mathbf{s}}_{k+1}}^{q^{+}}{z}_{{\mathbf{s}}_{k+1}}^{l^{+}}}},$$

(78)

where the coefficient ${\alpha }_{pql}$ and ${\beta }_{p^{+}{q}^{+}{l}^{+}}$ can be zero. According to Theorem 4, when the terms $2k+1$ and $2(k+1)$ in (75) are selected in set $\{p_7,p_8,p_9\}$, the numerator and denominator of $f\left({\mathbf{S}}_{k+1}\right)$ have the highest order with respect to x, y and z. Since ${\mathbf{V}}_{\mathbf{2}\mathbf{k}+\mathbf{1}}$ and ${\mathbf{V}}_{\mathbf{2}(\mathbf{k}+\mathbf{1})}$ contain x, y and z, when only the term $2(k+1)$ in (75) is selected in set $\{p_7,p_8,p_9\}$, neither of the numerator and denominator in (13) has the highest order. To make ${\mathbf{V}}_{\mathbf{2}\mathbf{k}+\mathbf{1}}$ and ${\mathbf{V}}_{\mathbf{2}(\mathbf{k}+\mathbf{1})}$ in matrix form with $x_{s_{k+1}}$, $y_{s_{k+1}}$, $z_{s_{k+1}}$, we get the expression:

$$\begin{array}{cc}\hfill {\mathbf{V}}_{\mathbf{2}\mathbf{k}+\mathbf{1}}& =\frac{1}{{\sigma }_{k+1}^2}\frac{1}{\left|\right|{\widehat{\mathbf{t}}}^k-s_{k+1}{\left|\right|}^2}\hfill \\ \hfill & \left[\begin{array}{ccc}\frac{{(y_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}^2}{\left|\right|{\widehat{\mathbf{t}}}^k-s_{k+1}{\left|\right|}^2}& -\frac{(x_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))(y_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{t}}}^k-s_{k+1}{\left|\right|}^2}& 0\\ -\frac{(x_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))(y_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{t}}}^k-s_{k+1}{\left|\right|}^2}& \frac{{(x_{s_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}^2}{\left|\right|{\widehat{\mathbf{t}}}^k-s_{k+1}{\left|\right|}^2}& 0\\ 0& 0& 0\end{array}\right],\hfill \end{array}$$

(79)

$$\begin{array}{cc}\hfill & {\mathbf{V}}_{\mathbf{2}(\mathbf{k}+\mathbf{1})}=\frac{1}{{\xi }_{k+1}^2}\frac{1}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}\hfill \\ \hfill & \left[\begin{array}{ccc}\frac{{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))}^2{(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}^2}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2}& \frac{{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))}^2(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))(y_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2}& -\frac{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}\\ \frac{{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))}^2(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))(y_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2}& \frac{{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))}^2{(y_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}^2}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2}& -\frac{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))(y_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}\\ -\frac{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}& -\frac{(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))(y_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(2\right))}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}& \frac{\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2}{\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2}.\end{array}\right].\hfill \end{array}$$

(80)

Take the highest order of $x_{{\mathbf{s}}_{k+1}}$ as an example. Only when the item ${(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}^2/\left|\right|{\widehat{\mathbf{t}}}^k-$

${\mathbf{s}}_{k+1}{\left|\right|}^2$ in ${\mathbf{V}}_{2k+1}$ and the item ${(z_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(3\right))}^2{(x_{{\mathbf{s}}_{k+1}}-{\widehat{\mathbf{T}}}^k\left(1\right))}^2/\left|\right|{\widehat{\mathbf{T}}}^k-{\mathbf{S}}_{k+1}{\left|\right|}^2\left|\right|{\widehat{\mathbf{t}}}^k-{\mathbf{s}}_{k+1}{\left|\right|}^2$ in ${\mathbf{V}}_{2(k+1)}$ are selected, the numerator and denominator of $f\left({\mathbf{S}}_{k+1}\right)$ have the highest order with respect to x, which are the 4th order and the 10th order, respectively.

$\overline{\mathrm{\Omega }}=\mathrm{\Omega }\cup \partial \mathrm{\Omega }$ is a compact and connected set in 3D space which $\mathrm{\Omega }$ stands for all internal points of the feasible area, $\partial \Omega$ represents the boundary of the feasible area. When the target point ${\widehat{\mathbf{T}}}^k$ is outside the sensor motion area, make tangent lines from point ${\widehat{\mathbf{T}}}^k$ to the sensor motion area and all tangent points can form a circle. Denote the cone formed by vertex ${\widehat{\mathbf{T}}}^k$ and the circle as ${▵}_{\mathbf{T}\mathbf{A}\mathbf{B}}$ in Figure 3 (the tangent points A and B are symmetrically distributed).

In order to solve the proposed optimization problem quickly, according to the relative position of the sensor motion area and the target safety area, the feasible region of the sensor trajectory optimization problem can be analyzed in following cases.

• When the sensor motion area and the target safety area are separated, define the boundary part of the sensor motion area inside ${▵}_{TAB}$ as surface 1 in Figure 3a. The feasible region is the sensor motion area;
• When the sensor motion area intersects with the target safety area, the feasible region is the sensor motion area where the part inside the target safety area is excluded, and the surface is where part of the target safety boundary is inside the sensor motion area. According to the position of ${\widehat{\mathbf{T}}}^k$, it can be divided into three sub-cases: (i) when ${\widehat{\mathbf{T}}}^k$ is outside the sensor motion area and ${▵}_{TAB}$ is not contained by the target safety area, define the part of the sensor motion boundary inside ${▵}_{TAB}$ with the section inside the target safety area replaced by the section of the target safety boundary inside the sensor movement area, such as surface 2 in Figure 3b. E and F (E and F are symmetrically distributed) are the points where the two spheres intersect; (ii) when the ${\widehat{\mathbf{T}}}^k$ is outside the sensor motion area and if ${▵}_{TAB}$ is contained by the target safety area, define the part of the target safety boundary inside the sensor motion area as surface 3 in Figure 3c; (iii) when the ${\widehat{\mathbf{T}}}^k$ is inside the sensor motion area, define the part of the target safety boundary inside the sensor motion area as surface 4 in Figure 3d;
• when the sensor motion area contains the target safety area, the feasible region is the sensor motion area where the interior section of the target safety area is excluded. The target safety boundary is defined as surface 5 in Figure 3e.

Proposition 1.

In case 1, the maximizer of the f ($\mathbf{S}$${}_{k+1}$) exists on the spherical crown (surface 1). In case 2(i), the maximizer of the f ($\mathbf{S}$${}_{k+1}$) exists on surface 2 where the two areas intersect in the sensor motion area, and the side of the circular truncated cone (lower bottom circle with $\mathbf{A}\mathbf{B}$ as the diameter and upper bottom circle with $E\mathbf{F}$ as the diameter). In cases 2(ii) and 2(iii), the maximizer of the f ($\mathbf{S}$${}_{k+1}$) exists on surfaces 3, 4 where the two areas intersect. In case 3, the maximizer of f ($\mathbf{S}$${}_{k+1}$) is on the boundary of the target safety area (surface 5).

Proof of Proposition 1. 

Regardless of the relative location of the two areas, a straight blue dotted line is drawn between ${\widehat{\mathbf{T}}}^k$ and any point in the feasible region; all the points on this line have the same ${\widehat{\varphi }}_{k+1}\left({\widehat{\mathbf{T}}}^k\right)$ and ${\widehat{\theta }}_{k+1}\left({\widehat{\mathbf{T}}}^k\right)$ as shown. Therefore the maximizer of $f\left({\mathbf{S}}_{k+1}\right)$ on the line segment is the closest one $D^{*}$ to ${\widehat{\mathbf{T}}}^k$ in the Euclidean sense. Gathering up these maximizers gives different surfaces such as the red part in Figure 3. □

4.4. Analytical Derivation for the Global Maximizer

According to (13), the partial derivative of $f\left({\mathbf{S}}_{k+1}\right)$ over $x_{{\mathbf{s}}_{k+1}}$, $y_{{\mathbf{s}}_{k+1}}$ and $z_{{\mathbf{s}}_{k+1}}$ can be written into:

$$\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial x_{{\mathbf{s}}_{k+1}}}=\frac{{\displaystyle \sum _{h=0}^{13}\sum _{q=0}^{14}\sum _{l=0}^8{\alpha }_{hql}^1(y_{{\mathbf{s}}_{k+1}}^q,z_{{\mathbf{s}}_{k+1}}^l)x_{{\mathbf{s}}_{k+1}}^h}}{{\displaystyle \sum _{h^{+}=0}^{20}\sum _{q^{+}=0}^{20}\sum _{l^{+}=0}^{12}{\beta }_{h^{+}{q}^{+}{l}^{+}}^{+}{x}_{{\mathbf{s}}_{k+1}}^{h^{+}}{y}_{{\mathbf{s}}_{k+1}}^{q^{+}}{z}_{{\mathbf{s}}_{k+1}}^{l^{+}}}},$$

(81)

$$\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial y_{{\mathbf{s}}_{k+1}}}=\frac{{\displaystyle \sum _{h=0}^{14}\sum _{q=0}^{13}\sum _{l=0}^8{\alpha }_{hql}^2(y_{{\mathbf{s}}_{k+1}}^q,z_{{\mathbf{s}}_{k+1}}^l)x_{{\mathbf{s}}_{k+1}}^h}}{{\displaystyle \sum _{h^{+}=0}^{20}\sum _{q^{+}=0}^{20}\sum _{l^{+}=0}^{12}{\beta }_{h^{+}{q}^{+}{l}^{+}}^{+}{x}_{{\mathbf{s}}_{k+1}}^{h^{+}}{y}_{{\mathbf{s}}_{k+1}}^{q^{+}}{z}_{{\mathbf{s}}_{k+1}}^{l^{+}}}},$$

(82)

$$\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial z_{{\mathbf{s}}_{k+1}}}=\frac{{\displaystyle \sum _{h=0}^{14}\sum _{q=0}^{14}\sum _{l=0}^7{\alpha }_{hql}^3(y_{{\mathbf{s}}_{k+1}}^q,z_{{\mathbf{s}}_{k+1}}^l)x_{{\mathbf{s}}_{k+1}}^h}}{{\displaystyle \sum _{h^{+}=0}^{20}\sum _{q^{+}=0}^{20}\sum _{l^{+}=0}^{12}{\beta }_{h^{+}{q}^{+}{l}^{+}}^{+}{x}_{{\mathbf{s}}_{k+1}}^{h^{+}}{y}_{{\mathbf{s}}_{k+1}}^{q^{+}}{z}_{{\mathbf{s}}_{k+1}}^{l^{+}}}},$$

(83)

where the coefficient ${\alpha }_{hql}^1$, ${\alpha }_{hql}^2$, ${\alpha }_{hql}^3$ and ${\beta }_{h^{+}{q}^{+}{l}^{+}}^{+}$ can be zero. From Proposition 1, the feasible region always lies on the area boundary. Take the case when the two areas are separated as an example. ${\left[\begin{array}{ccc}\partial f\left({\mathbf{S}}_{k+1}\right)/\partial x_{{\mathbf{s}}_{k+1}}& \partial f\left({\mathbf{S}}_{k+1}\right)/\partial y_{{\mathbf{s}}_{k+1}}& \partial f\left({\mathbf{S}}_{k+1}\right)/\partial z_{{\mathbf{s}}_{k+1}}\end{array}\right]}^{\prime }$ should be colinear with the normal vector ${\left[\begin{array}{ccc}{x}_{{\mathbf{s}}_{k+1}}-x_{{\mathbf{s}}_k}& y_{{\mathbf{s}}_{k+1}}-y_{{\mathbf{s}}_k}& z_{{\mathbf{s}}_{k+1}}-z_{{\mathbf{s}}_k}\end{array}\right]}^{\prime }$ at the optimal, i.e.,

$$\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial x_{{\mathbf{s}}_{k+1}}}(y_{{\mathbf{s}}_{k+1}}-y_{{\mathbf{s}}_k})-\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial y_{{\mathbf{s}}_{k+1}}}(x_{{\mathbf{s}}_{k+1}}-x_{{\mathbf{s}}_k})=0,$$

(84)

$$\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial x_{{\mathbf{s}}_{k+1}}}(z_{{\mathbf{s}}_{k+1}}-z_{{\mathbf{s}}_k})-\frac{\partial f\left({\mathbf{S}}_{k+1}\right)}{\partial z_{{\mathbf{s}}_{k+1}}}(x_{{\mathbf{s}}_{k+1}}-x_{{\mathbf{s}}_k})=0.$$

(85)

Otherwise, any infinitesimal move along the tangent vector that constructs an obtuse angle with $-{\left[\begin{array}{ccc}\partial f\left({\mathbf{S}}_{k+1}\right)/\partial x_{{\mathbf{s}}_{k+1}}& \partial f\left({\mathbf{S}}_{k+1}\right)/\partial y_{{\mathbf{S}}_{k+1}}& \partial f\left({\mathbf{S}}_{k+1}\right)/\partial z_{{\mathbf{S}}_{k+1}}\end{array}\right]}^{\prime }$ increases the value of $f\left({\mathbf{S}}_{k+1}\right)$, (84) is equivalent to:

$$\sum _{h=0}^{15}{g}_h(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})x_{{\mathbf{s}}_{k+1}}^h=0,$$

(86)

where $g_h(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})$ is the polynomial of $y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}}$. Define $\eta (y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})={(y_{{\mathbf{s}}_{k+1}}-y_{{\mathbf{s}}_k})}^2+{(z_{{\mathbf{s}}_{k+1}}-z_{{\mathbf{s}}_k})}^2+{x_{{\mathbf{s}}_k}}^2-r^2$. The movement boundary condition is rewritten into:

$$\eta (y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})-2x_{{\mathbf{s}}_k}{x}_{{\mathbf{s}}_{k+1}}+x_{{\mathbf{s}}_{k+1}}^2=0.$$

(87)

The joint solution of (84) and (87) can be developed as follows. The $(15+2)\times (15+2)$ Sylvester matrix of (84) and (87) with respect to $y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}}$ is:

$$\begin{array}{cc}\hfill & \mathbf{S}\mathbf{y}{\mathbf{l}}_{\mathbf{1}}(\left(84\right),\left(87\right);y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})=\hfill \\ & \left(\begin{array}{cccccc}{g}_{15}(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& 0& 1& 0& ...& 0\\ g_{14}(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& g_{15}(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& -2x_{{\mathbf{s}}_k}& 1& ...& 0\\ g_{13}(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& g_{14}(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& \eta (y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& -2x_{{\mathbf{s}}_k}& ...& 0\\ ⋮& ⋮& ⋮& ⋮& ⋮& ⋮\\ g_1(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& g_2(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& 0& 0& ...& 1\\ g_0(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& g_1(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& 0& 0& ...& -2x_{{\mathbf{s}}_k}\\ 0& g_0(y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})& 0& 0& ...& \eta (y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})\end{array}\right).\hfill \end{array}$$

(88)

The Sylvester resultant $det\left(\mathbf{S}\mathbf{y}{\mathbf{l}}_{\mathbf{1}}(\left(84\right),\left(87\right);y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})\right)$ is a bivariate polynomial with an upper bound of the 58th order for $y_{{\mathbf{s}}_{k+1}}$ and the 46th order for $z_{{\mathbf{s}}_{k+1}}$.

Similarly, the joint solution of (85) and (87) can be developed as the $(15+2)\times (15+2)$ Sylvester matrix of (85) and (87) is similar to (88). $det\left(\mathbf{S}\mathbf{y}{\mathbf{l}}_{\mathbf{2}}(\left(85\right),\left(87\right);y_{{\mathbf{s}}_{k+1}},z_{{\mathbf{s}}_{k+1}})\right)$ is also a bivariate polynomial with an upper bound of the 58th order for $y_{{\mathbf{s}}_{k+1}}$ and the 46th order for $z_{{\mathbf{s}}_{k+1}}$. The real solution can be obtained by solving the equations given by the determinants of the two Sylvester matrices.

Comparing only the real solutions lying on surface 1 in terms of $f\left({\mathbf{S}}_{k+1}\right)$ gives the global maximizer. For surface 2, the real solutions of the surface equation and (86) are compared with the two intersection points to generate the global maximizer. For surfaces 3 and 4, the real solutions of the surface equation and (86) on the surface are compared with the two intersection points to provide ${\mathbf{S}}_{k+1}$. When the movement area contains the safety area, the real solutions of the safety constraint and the (86) that has the biggest $f\left({\mathbf{S}}_{k+1}\right)$ are the global maximizer.

Remark 1.

The position of a sensor should not be collinear with the target position in the z-axis direction so that the target is observable. It is necessary to set the cylindrical as a non-flying area in Figure 4. It means to insert a cylinder centering at the target position with the radius of ρ in the target safety area, and the z-axis direction extends to infinity. The constraint is formulated as:

$${(x_{k+1}-{\widehat{\mathbf{T}}}^k\left(1\right))}^2+{(y_{k+1}-{\widehat{\mathbf{T}}}^k\left(2\right))}^2⩾{\rho }^2.$$

(89)

If the maximum solution of f ($\mathbf{S}$${}_{k+1}$) falls into the section where the surface intersects the cylinder, we choose the other one which leads to the second largest f ($\mathbf{S}$${}_{k+1}$), and so forth. If all the maximum solutions of f ($\mathbf{S}$${}_{k+1}$) fall within the intersection section, a point is randomly selected from the feasible region as the solution of f ($\mathbf{S}$${}_{k+1}$).

4.5. Single Sensor Trajectory Design Procedure

We summarize the sensor trajectory design procedure in the following algorithm.

Trajectory design based localization algorithm:

Initialization: Use the initial position of the sensor to generate the initial estimator parameter by localization algorithm, e.g., Kalman-related methods.

Repeat until n is reached:

• Generate ${\mathbf{S}}_{k+1}$ through the maximization of (69) or (72). The maximizer of (69) or (72) can be produced in an analytical way described in Section 4.4;
• Gather the measurement information ${\widehat{\varphi }}_{k+1}$, ${\widehat{\theta }}_{k+1}$, ${\widehat{\sigma }}^2$ and ${\widehat{\xi }}^2$ from the sensor at ${\mathbf{S}}_{k+1}$;
• Use localization technique to generate ${\widehat{\mathbf{T}}}^{k+1}$;
• $k=k+1$.

Output the target location.

In many applications, various location-based services need to determine accurate positioning information, such as for public safety services, field rescue and other fields. It is necessary to track the target under motion constraints. Unlike static sensors, which have a fixed density and perception range, mobile sensors can cover a larger area over time without increasing their number. In addition, their spatial distribution can change dynamically to accommodate the movement of the target, thus providing measurements of information about its position. Choosing the best sensing location is particularly important, especially given time-constrained applications such as tracking hostile targets. For example, autonomous underwater vehicles (AUV) are underwater unmanned autonomous vehicle platforms without a cable connection that can be applied for underwater environment monitoring, offshore oil engineering operations, underwater search and mapping after loading appropriate sensors. In the main-slave UUVS (Unmanned Underwater Vehicles) system, the slave-UUV only carries the azimuth information of the main-UUV under a cluttered environment and realizes the convergence with the main-UUV as soon as possible through autonomous optimization of its movement trajectory. The localization method and trajectory planning method proposed in this paper can be applied to the above scenarios.

5. Simulation

In this section, the bias compensation method and sensor trajectory planning algorithm are compared with other benchmarks through MATLAB R2020a simulation. The location unit used for the following experiments is a metre.

5.1. Example 1

We now explore the positioning performance of the proposed method and estimate the deviation through simulation. The simulation is performed in 3D space, where the target is assumed to be at position $\mathbf{T}={\left[\begin{array}{ccc}30& 40& 50\end{array}\right]}^{\prime }$ m. The sensor collects $N=100$ azimuth/elevation angle measurements, which ensure the target is observable. The initial sensor is located at ${\mathbf{S}}_1={\left[\begin{array}{ccc}0& 0& 0\end{array}\right]}^{\prime }$ m, the motion velocity components of the sensor in x, y and z directions are 0.5 m/s, 0.8 m/s, 1 m/s, respectively. The bearing angle measurement noise standard deviations $\sigma =\xi$ are set in between $\sqrt{0.01}$ and $\sqrt{0.08}$ radian with the variance difference of $0.01$ radian${}^2$. One thousand independent Monte Carlo experiments are run. We use the mean square error (MSE) and bias norm as evaluation indicators. The MSE and bias norm are defined as:

$$MSE=\frac{{\displaystyle \sum _{i=1}^{1000}\sum _{j=1}^3{({\widehat{\mathbf{T}}}_i\left(j\right)-\mathbf{T}\left(j\right))}^2}}{1000},$$

(90)

$$bias=\frac{{\displaystyle \sum _{i=1}^{1000}\sum _{j=1}^3|{\widehat{\mathbf{T}}}_i\left(j\right)-\mathbf{T}\left(j\right)|}}{1000},$$

(91)

respectively. The first simulation is about the MSE and bias norm comparison between the proposed BC method and the original 3D localization method (PLE) in [15]. As shown in Figure 5 and Figure 6, the BC method has lower MSE and bias norm compared to the PLE method. In addition, the MSE and bias norm increase as measurement noise level increases. The positioning accuracy of the two methods is significantly improved by using the weighted instrumental variable method. The PLE-WIV estimator, as expected, suffers from more serious bias than the BC-WIV estimator.

The second simulation based on positioning methods is carried out with the increase of sensor number. The bearing angle measurement noise standard deviation is assumed to be $\sqrt{0.001}$ radian. The number of known sensors is changed from 60 to 260 with a difference of 40. The position of sensors is set as $x_{{\mathbf{s}}_i}\in \left(050\right)$ m, $y_{{\mathbf{s}}_i}\in \left(050\right)$ m, $z_{{\mathbf{s}}_i}\in \left(050\right)$ m $(i=1,\dots ,n)$.

Under the same conditions of each experiment, as shown in Figure 7 and Figure 8, the BC and the BC-WIV methods still show the best positioning performance. The MSE and bias norm decrease when the number of sensors increases. However, there can be low positioning accuracy occasionally when the number of sensors is large. This is because in the process of the random generation of sensors, the position of some sensors makes the target unobservable, resulting in low positioning accuracy.

5.2. Example 2

The third simulation is about sensor trajectory planning. For fairness and completeness, we use the pseudolinear Kalman filter method in [30] for target localization as the platform to compare the performance of trajectories. One fitness function,

$$L\left(\widehat{\mathbf{T}}\right)=100\ast (1-\frac{\left|\right|\mathbf{T}-\widehat{\mathbf{T}}{\left|\right|}_2}{{\left|\right|\mathbf{T}\left|\right|}_2}),$$

(92)

in [31] is used to demonstrate the performance of each estimator over n runs. It could even be much less than zero when the estimators deviate far from the real values. To downplay the effect of the extremum points, $L\left(\widehat{\mathbf{T}}\right)$ is modified into:

$$\acute{L}\left(\widehat{\mathbf{T}}\right)=max(0,100\ast (1-\frac{\left|\right|\mathbf{T}-\widehat{\mathbf{T}}{\left|\right|}_2}{{\left|\right|\mathbf{T}\left|\right|}_2})).$$

(93)

Trajectory 3.

The location$\mathbf{S}$${}_{k+1}$on the trajectory is generated through the maximization of det($\mathit{FIM}$${}_{k+1}$)-det($\mathit{FIM}$${}_{k-2}$) with respect to ${\mathbf{S}}_{\mathbf{k}+\mathbf{1}}$recursively. The design of$\mathbf{S}$${}_{k+1}$ is given by the $(k+1)$th measurement jointly with its two temporal nearest predecessors.

Trajectory 4.

The location$\mathbf{S}$${}_{k+1}$ on the trajectory is generated through the minimization of the state estimate covariance.

The four trajectories planning methods are simulated and compared. The positions of the target and sensors are located at $\mathbf{T}={\left[\begin{array}{ccc}20& 35& 55\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_1={\left[\begin{array}{ccc}10& 25& 20\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_2={\left[\begin{array}{ccc}30& 20& 40\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_3={\left[\begin{array}{ccc}70& 50& 15\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_4={\left[\begin{array}{ccc}12& 80& 70\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_5={\left[\begin{array}{ccc}59& 22& 18\end{array}\right]}^{\prime }$ m, ${\mathbf{S}}_6={\left[\begin{array}{ccc}120& 150& 90\end{array}\right]}^{\prime }$ m, respectively. For each test, the four trajectories share the same six initial sensor locations. The noise of the angle measurements are Gaussian independently distributed with zero mean and the standard deviation is $\sqrt{0.1}$ radian. The anti-collision distance and the maximum distance of sensor movement are set to $R=5\mathrm{m}$, $r=10\mathrm{m}$, respectively. The radius of the cylindrical non-flying area is set to $\rho =0.5R$. One hundred independent Monte Carlo experiments are run.

Performance of the four trajectories is measured and displayed at two observation points when $k=10$ and $k=15$.

Table 1. Statistics that sum up the number of tests when $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}^{\mathbf{i}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}^{\mathbf{4}}\right)>0(i=1,2,3)$ for trajectory i at two observations.

Observation Point	$det\left({\mathbf{FIM}}_{\mathbf{k}}^1\right)-det\left({\mathbf{FIM}}_{\mathbf{k}}^4\right)>0$	$det\left({\mathbf{FIM}}_{\mathbf{k}}^2\right)-det\left({\mathbf{FIM}}_{\mathbf{k}}^4\right)>0$	$det\left({\mathbf{FIM}}_{\mathbf{k}}^3\right)-det\left({\mathbf{FIM}}_{\mathbf{k}}^4\right)>0$
k = 10	98/100	96/100	3/100
k = 15	99/100	96/100	2/100

shows that Trajectory 1 and 2 contribute to greater $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$ in over $90\%$ of tests where trajectory 4 is used as a benchmark. This is consistent with the numerical results given in

Table 2. Mean values of $\acute{L}\left(\widehat{\mathbf{T}}\right)$ for four trajectories at two observation points.

	Trajectory 1	Trajectory 2	Trajectory 3	Trajectory 4
mean of $\acute{L}\left(\widehat{\mathbf{T}}\right)$$(k=10)$	91.16	90.58	89.62	90.49
mean of $\acute{L}\left(\widehat{\mathbf{T}}\right)$$(k=15)$	93.42	92.86	88.85	90.99

where Trajectory 1 has the highest $\acute{L}\left(\widehat{\mathbf{T}}\right)$ at $k=10$ and $k=15$. Compared with Trajectory 3 and 4, the target estimations are more accurate under Trajectory 1 and 2 as the corresponding $\acute{L}\left(\widehat{\mathbf{T}}\right)$ values are closer to 100. The accuracy of target localization is related to $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$. The larger the $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$, the higher the positioning accuracy.

From Theorem 3, $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$ for trajectory 1 and $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}-\mathbf{2}}\right)$ for Trajectory 3 both are parts of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$. Since Trajectory 3 only utilizes the sensor location of the past two moments, $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}-\mathbf{2}}\right)$ weighs less in $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$ than $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$. The maximizer of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)-det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}-\mathbf{2}}\right)$ is less likely to be close to the global optimizer of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}+\mathbf{1}}\right)$. Under such circumstances, Trajectory 3 contributes a smaller information increment than Trajectories 1 and 2. Trajectory 4 is achieved by minimizing the trace of the state estimation covariance matrix. Due to the inverse relation between the covariance and FIM, the generation of Trajectory 4 also utilizes more past information than Trajectory 3. However, compared with Trajectories 1 and 2, not all the information is used.

Comparison of the mean of $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$ for four trajectories can be seen intuitively in Figure 9. When the sampling step increases, $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$ of the proposed method increases more. In addition, more $det\left(\mathbf{F}\mathbf{I}{\mathbf{M}}_{\mathbf{k}}\right)$ can be obtained when the angles are updated in Trajectory 1 at the cost of increasing the calculation amount. Less computation time is consumed by generating Trajectory 2. When the measurement noise is small, the difference of the determinant obtained by Trajectories 1 and 2 is subtle as seen in the inner plot of Figure 9.

The trajectories simulation results are shown in Figure 10. In the process of trajectory planning, the predicted sensor position keeps getting closer to the target. When the sensor motion area is separated from the target safety area, ${\mathbf{S}}_{k+1}$ sits on the movement boundary sphere centering at ${\mathbf{S}}_k$.

As shown in Figure 10, Trajectories 1 and 2 approach the target much faster because the trajectory planning method proposed in this paper takes into account sensor position information at all past moments and have higher FIM determinant. Since Trajectories 3 and 4 correspond to lower and mild FIM determinants, their curves reach the target in a slow and mild speed, respectively, amongst four trajectories; although it seems that Trajectory 3 distances itself from the target.

For each scenario, the whole computation complexity includes the following parts: initialization, Kalman-related localization, objective function construction and optimization. The average runtime for each trajectory is given in

Table 3. Averaged runtimes for four trajectories.

	Trajectory 1	Trajectory 2	Trajectory 3	Trajectory 4
runtime(s)	0.5558	0.5240	0.6996	0.5899

to describe the complexity.

In Table 3, it can be seen that the time consumed for Trajectory 2 is the shortest. The generation of Trajectory 2 can effectively reduce the computational complexity in the optimization of (62) in return for no update of the bearing angles. The generation of Trajectory 3 takes a longer time since the objective function is different from Trajectories 1 and 2. For Trajectory 4, inverse operation of Fisher information matrix results in a longer calculation time.

6. Conclusions and Future Works

This paper studies the bearing-only localization from the improvement of target localization method and the reasonable planning of a sensor trajectory strategy. Under the condition of Gaussian noise, a potential relationship exists between the determinant of the extended coefficient matrix and the noise variance in the angle measurement equation. When the determinant of the extended coefficient matrix is zero, the variance of zero-mean Gaussian noise can be derived. Based on this estimated variance, the bias compensation localization of the target on the 3D space is realized. The estimator can be further refined by the BC-WIV method. In addition, a trajectory planning algorithm encapsulating two trajectory planning strategies are proposed to improve localization accuracy under several constraints. The next moment sensor position is given by optimizing quantified FIM determinant increment as the objective function that evaluates all the historical measurements. The optimal solution always lies on the constraint boundary. Simulation results show that Trajectory 1 leads to a higher localization accuracy at the cost of more computation time while Trajectory 2 has a comparable result with a mild computation burden.

In the future, we plan to extend our current approach to multiple sensors localization. The research work in this paper is only carried out at the theoretical level, the next research will test the method proposed in the practical communication environment. A team of three Pioneer robots is expected to be deployed in a rectangular area, with one Pioneer robot as the target and the other two Pioneers as tracking sensors to locate the target by the obtained bearing information.

7. Patents

Yiqun Zou, Bilu Gao, Xiafei Tang. 3D trajectory planning method, system, equipment and medium based on bearing-only measurements: 202111562286.9[P].2021.12.20.