A Closed-Form Method for Simultaneous Target Localization and UAV Trajectory Optimization

Featured Application

This work can be applied to target tracking and localization applications with UAV.

Abstract

Unmanned aerial vehicles (UAVs) play a key role in modern surveillance-related missions. A major task for performing these missions is to find the precise location of a moving target in real-time, for which the main challenge is to estimate the target position to high precision using the noisy measurements from the airborne sensors. In this paper, we present a closed-form on-line simultaneous target localization and UAV trajectory optimization method based on the visual platform, which can effectively minimize the localization uncertainty to the target. The proposed method can be elucidated explicitly using two phases, of which, in the target localization phase, the expended information filtering (EIF) is exploited, which can express the predicted Fisher information matrix (FIM) of the target explicitly and iteratively, and in the UAV trajectory optimization phase, the property of the predicted FIM is exploited to establish the UAV waypoint optimization objective by taking account of the UAV motion limit. Compared with existing methods of the same class, the proposed method not only estimates the next target position more correctly, but also takes the error of the target motion into consideration, thus improving the effectiveness of the optimized UAV trajectory. Both simulations and field experiments were conducted, which show that the proposed method outperformed the existing methods.

1. Introduction

Target localization based on unmanned aerial vehicles (UAVs) has played an important role in many modern applications such as intelligence, reconnaissance, and surveillance missions (ISR) [1,2]. Equipped with onboard infrared and optical cameras as well as communication and navigation hardware, modern UAVs are capable of tracking an anonymous target, surveying the environment and providing the operator with real-time visual information. A common ISR mission for UAV is to track and localize a target detected by airborne sensors. Exploiting the image data of the target from different observation stations of the UAV and the knowledge of the UAV position and orientation, the target position can be estimated by establishing the collinear equation of the optical center, imaged point, and the true position of the target. However, due to the noisy measurements, the obtained estimates always deviate from the true positions. In order to position the target with a high accuracy given the fixed noise levels, a feasible and effective solution is to choose the observation stations so that the optimal sensor-target geometry can be formed, which can minimize the estimate uncertainty. As the task is to localize a moving target, the optimized sequential UAV observation stations form a desired UAV trajectory through which the target can be observed with the minimized uncertainty.

An off-line planned trajectory cannot well predict the behavior of the target, and it is also difficult for a human operator to fly the vehicle so that the target position can be estimated with the minimized error. Hence, an autonomous and on-line UAV path generation is important to target localization. The purpose of this paper was to provide a method for UAV trajectory generation that enables the optimized localization performance.

Relevant works on target localization are primarily about the localization of a stationary target [3,4,5], for instance, Kutluyil Doğançay [6] developed a total least square algorithm for two-dimensional localization of a stationary target using a bearing only measurement; Bishop [7] exploited the geometric constraint method to implement target position estimation, which can obtain a geometrically consistent estimation; Barber [3] used the recursive least square (RLS) filtering and wind velocity estimation method to improve the target localization performance, showing that the target localization bias caused by measurement biases can be eliminated using an on-line learning algorithm; Sohn [4] studied how to improve localization performance using wind velocity estimation and pitch estimation method; Redding [5] presented a method for localizing a stationary target using the pixel location of the target in the aerial image, with the measurement of UAV position and attitude, and camera pose angles. The obtained estimate of target position in the world coordinate has been demonstrated with an error less than 11 m. Wang [8] studied the use of electro-optical stabilized imaging systems to localize multiple targets in real time, and the RLS method was used to estimate the positions of targets. However, the above works mainly focused on methods on the estimation or bias elimination. Apart from establishing a proper estimation method that can improve the target localization accuracy, another important factor that plays an important role in the localization performance should also be taken into account, namely, the UAV-target geometry.

There have been many works on optimizing the target localization geometry. Kutluyil Doğançay [9] established the angular separation requirement for the angle of arrival (AOA) sensors to achieve the best mean-squared-error (MSE) localization performance for arbitrary but fixed sensor ranges. Bishop [10] studied the influence of the geometric configuration between the multiple stationary observation stations and the stationary target on target localization performance; Xu sheng [11] investigated the optimal sensor placement strategies for AOA localization in 3-dimensional space by using a resistor network analogy to determine the sensor–target geometry. The metric for optimizing the sensor–target geometry in the above work is mainly the Fisher information matrix (FIM). Furthermore, Bai [12] proposed a two-UAV intersection target localization system for which the UAVs were equipped with airborne optoelectronic platforms. The optimal positions for the two UAVs were considered by changing the baseline between them. Compared with the sensor–target optimization problem for a stationary target as above, the counterpart problem for the moving target is a different situation, which is a problem on the UAV trajectory optimization. Ref. [13] presented an early study that tried to optimize the observer’s path to increase the localization accuracy for a moving target. Afterward, the problem on the localization of moving targets with measurements from different mobile sensors has attracted many researchers such as [14], who used using time differences of arrival (TDOA) and frequency differences of arrival (FDOA). In [15], the moving observer was illustrated to make a spiral motion around the moving target. Ref. [16] studied the vehicle planning problem for optimal target localization with range measurement by simultaneously considering the collision avoidance and vehicle maneuvering constraints. Ref. [17] proposed a non-causal trajectory optimization method for optimally localizing a moving target, which could achieve a better localization performance in some external constraint conditions than the causal trajectory optimization strategy, for example, the UAV travel length or the total travel time is restricted. Optimal vehicle trajectory that maximizes the observability for maneuvering target is also generated in [18]. The authors dealt with range observability for the target with a bearing-only measurement, which is also equivalent to the localization observability of the target as together with the bearing measurement, the target can be localized with the observable range. Ref. [19] utilized a geometry metric to measure the target localization performance, with which a 2-dimensional constrained robot trajectory optimization problem was addressed to increase the localization performance using bearing-only measurement.

Furthermore, trajectory optimization also involves the stationary target localization problem. In [20], the problem of trajectory optimization for multiple UAVs with investigated with heterogeneous sensors, which can cooperatively localize a target to achieve a better performance. Ref. [21] investigated the UAV path planning problem to increase the localization performance to a signal emitter. The optimal observer trajectory that maximizes the observability of the target of interest was also studied in [22,23,24,25].

However, the above work mainly deals with the stationary target or the moving target with an ideal motion model such as the constant velocity. In practical applications, the target motion model is not exactly known, and this can usually be modeled by the process noises of the dynamic model. If the error from the motion model is neglected, the resultant optimized trajectory will become unstable. Under the vehicle constraints, this will result in a different trajectory, which will degenerate the target localization performance. In this paper, we present a closed-form solution for simultaneous target localization and UAV trajectory optimization problem. With this method, the extended information filtering (EIF) [26,27] is exploited to estimate the target state, which can compute the predicted information matrix required by the trajectory optimization explicitly and iteratively. Each time after the state estimation, the next optimized waypoint is solved and becomes an index to the control algorithm so that the UAV can reach the desired position and minimize the target localization uncertainty.

The main contributions of this paper can be enumerated as follows.

(i)

It is derived that the target localization using the UAV with a visual platform is a 3D bearing-only target localization problem.

(ii)

A closed-form method based on EIF for the simultaneous target localization and UAV trajectory optimization problem is presented, which well decouples the target localization from the UAV trajectory optimization. By using the predicted estimate of the target estimate and taking account of the process noise in the trajectory optimization phase, the optimized next waypoint will become more effective than existing methods.

(iii)

By analyzing the valid information of the information matrix, the position-related information matrix is extracted, which can facilitate the numerical computation for the optimization objective of the next waypoint.

The remainder of this paper is outlined as follows. Section 2 introduces the system and principle of target localization, and derives that the target localization problem using the UAV with a visual platform is a 3D bearing-only target localization problem; Section 3 presents the simultaneous target localization and UAV trajectory optimization method based on EIF; Section 4 gives the simulations, field experiments and relevant discussions; and Section 5 concludes this paper and introduces the future work.

2. Target Localization Based on Visual Platform

2.1. System Structure and Preliminaries

The referred UAVs in our project were equipped with an inertial measurement unit (IMU), a high precision global positioning system (GPS) with real-time kinematic (RTK) technique, and a visual tracking platform. The target localization workflow is shown in Figure 1. In the target localization process, the target is first detected and recognized by the video processing unit once the target is captured by the camera, and the video algorithm localizes the pixel centroid of the target and outputs the pixel position as the control block’s input. The camera control module drives the pan/tilt servo motor to change the camera’s azimuth and elevation angle through pulse width modulation, so as to lock the target image in the center of the camera’s field of view (FOV). Meanwhile, the camera attitude sensors output the relative angles between the UAV and the camera. Combining the synchronized measurement data (MEA) from the GPS, IMU, and camera attitude sensors, the target can be localized in real time. Note that in the localization process, some prior information may be available such as the known height of the target to be localized. To introduce the proposed method concisely, we avoid discussing the observability of the moving target by guaranteeing the observability of the target (i.e., it is assumed that the target height is known while the related angle measurements are achieved). In many applications, the target usually moves in a limited area so that the ground can be considered as flat [3]. To illustrate the target localization procedure in detail, several coordinate frames should be introduced.

(1) Global coordinate frame, notated as $C^g$, where, the axes $X^g$,$Y^g$ point to the east and north directions, respectively, which together with axis $Z^g$ form a right hand Cartesian frame and the origin $O^g$ is set at a fixed point, as shown in Figure 2.

(2) UAV geographical coordinate frame, notated as $C^u$, which has the same directions as $C^g$ over three axes except that the origin of the $C^u$ is at the center position of mass of the UAV.

(3) UAV body fixed coordinate frame, notated as $C^b$, where, the axes $X^b$,${\mathbf{Y}}^b$ point to the front and left directions of the UAV, respectively, which together with axis $Z^b$ form a right hand Cartesian frame, with the origin coinciding with $O^b$.

(4) Camera coordinate frame, represented by $C^c$, which coincides with $C^b$ when the attitude angles of the camera are kept at the initial positions (i.e., the azimuth angle and elevation angle are both zero). Note that using the control block, as shown in Figure 1, the positive direction of the $Z^c$-axis always points to the UAV from the target, as shown in Figure 2. As the point of mass of the UAV is very close to the optical center of the camera, the origins of $C^u$ and $C^b$ can be assumed to be at the optical center.

2.2. Target Localization Principle

The fundamental principle of the target localization using the above hardware setting can be expressed as follows.

First, the target position in the camera coordinate frame is transformed into the global coordinate frame,

$${\mathit{x}}_g=\mathit{R}\mathit{o}{\mathit{t}}_g^b\left(\varphi ,\theta ,\phi \right)\mathit{R}\mathit{o}{\mathit{t}}_b^c\left(\alpha ,\beta \right){\mathit{x}}_c+\mathit{y}$$

(1)

where ${\mathit{x}}_g$,${\mathit{x}}_c$ denote the target position in the $C^g$ frame and $C^c$ frame, respectively, and ${\mathit{x}}_c={[0,0,-l]}^{\top }$ where $l$ denotes the distance between UAV and the target and $\top$ is the transpose operation; $y$ represents the UAV position; $\mathit{R}\mathit{o}{\mathit{t}}_b^c\left(\alpha ,\beta \right)$ and $\mathit{R}\mathit{o}{\mathit{t}}_g^b\left(\varphi ,\theta ,\phi \right)$ denote the transformation matrices from ${\mathit{C}}_c$ to ${\mathit{C}}_b$ and ${\mathit{C}}_b$ to ${\mathit{C}}_g$, respectively, which can be expressed as:

$$\left\{\begin{array}{l}\mathit{R}\mathit{o}{\mathit{t}}_b^c\left(\alpha ,\beta \right)={\mathit{R}}_y(\beta ){\mathit{R}}_z(\alpha )\\ \mathit{R}\mathit{o}{\mathit{t}}_g^b\left(\varphi ,\theta ,\phi \right)={\mathit{R}}_{\mathbf{x}}(\varphi ){\mathit{R}}_y(\theta ){\mathit{R}}_z(\phi )\end{array}\right.$$

(2)

where $\alpha$,$\beta$ denote the azimuth and elevation angle, respectively, and $\varphi$,$\theta$,$\phi$ represent the roll, pitch, and yaw angle, respectively; and

$${\mathit{R}}_x(\gamma )=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \gamma & -\sin \gamma \\ 0& \sin \gamma & \cos \gamma \end{array}\right),{\mathit{R}}_y(\gamma )=\left(\begin{array}{ccc}\cos \gamma & 0& \sin \gamma \\ 0& 1& 0\\ -\sin \gamma & 0& \cos \gamma \end{array}\right),{\mathit{R}}_z(\gamma )=\left(\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right)$$

(3)

As in (1), both ${\mathit{x}}_g$ and ${\mathit{x}}_c$ are unknown, and (1) is linear about these variables, so (1) can be considered as the Line of Sight (LoS) representation. For a stationary target, if there is no other prior information such as the distance from UAV to the target, the target position can be determined by two or more LoS equations as above from different observation stations; for the moving target, if there is no prior information available, multiple LoS equations cannot make up the sufficient condition to determine the target position because the moving target may be unobservable with the improper multiple measurements [28]. However, if there is prior information such as distance information, the target position can be determined uniquely using only one of the above LoS equations. In this paper, to concisely present the proposed method, we circumvented the observability problem, and instead, we assumed that the moving target had a prior known height, which can guarantee that after each measurement, an estimate of the target position can be achieved, and in most of the practical applications, this assumption is reasonable. Importantly, the proposed method can be easily generalized to other measurement models, for example, the bearing-only measurement model and the bearing-distance model.

In practical applications, the measurements are obtained with noise; to reduce the influence of noise on the localization results, classical estimation methods such as maximum likelihood estimator (MLE) and least square estimator (LSE) can be used. For the moving target localization problem, the most used estimation methods belong to the class of filtering methods, among which the extended Kalman filtering (EKF) is the most common one [1]. In the following paper, we use a variant of EKF to iteratively estimate the target position and simultaneously optimize the UAV trajectory so that the localization uncertainty for the moving target can be minimized.

3. Simultaneous Target Localization and Unmanned Aerial Vehicle (UAV) Trajectory Optimization

3.1. Target Localization with the Attitude Measurement of Line of Sight (LoS)

To simplify the above problem, we now treated the target and UAV as mass points. Given the measurement at one observation station, we achieved a LoS as shown in Figure 3. Obviously, in this setting, the problem becomes a 3D bearing-only target localization problem. Denote the azimuth and elevation angle of the LoS by $\vartheta$ and $\gamma$, respectively. We have

$$\left\{\begin{array}{l}\gamma =\arctan 2\left(\frac{\mathit{r}(3)}{\sqrt{\mathit{r}{(1)}^2+\mathit{r}{(2)}^2}}\right)\\ \vartheta =\arctan \left(\frac{\mathit{r}(2)}{\mathit{r}(1)}\right)\end{array}\right.$$

(4)

where $\mathit{r}:={\mathit{x}}_g-\mathit{y}$ denotes the pointing vector from the UAV to the target; $\mathit{r}(1)$,$\mathit{r}(2)$, and $\mathit{r}(3)$ denote the components of $\mathit{r}$ in $X^g$,$Y^g$, and $Z^g$ axes in $C^g$, respectively; $\arctan 2(\cdot )$ denotes the 2-argument arctangent, which makes the azimuth angle range from $-\pi$ to $\pi$. We now use the direct measurement to represent the attitude of the LoS. According to the transformation relation in the previous section, $\mathit{r}$ can be obtained by transforming the pointing vector on $Z^c$ in $C^c$ into $C^u$ using the direct measurement, in other words,

$$\mathit{r}=\mathit{R}\mathit{o}{\mathit{t}}_g^b\left(\varphi ,\theta ,\phi \right)\mathit{R}\mathit{o}{\mathit{t}}_b^c\left(\alpha ,\beta \right)\left(\begin{array}{c}0\\ 0\\ -l\end{array}\right)$$

(5)

Therefore, the target localization problem can be considered as a 3D bearing-only target localization problem combining (4) with (5). Furthermore, the measurement noises for the direct measurement can be transformed into errors of the azimuth and elevation angles using (4) and (5). Assuming the error of the direct measurement is denoted by $\mathit{n}={[\delta \alpha ,\delta \beta ,\delta \varphi ,\delta \theta ,\delta \phi ]}^{\top }$(the UAV position error is ignored because the GPS-RTK data have a high precision to the centimeter level), the azimuth and elevation angle errors denoted by $\mathit{w}={[\delta \gamma ,\delta \vartheta ]}^{\top }$ can be approximately expressed as:

$$\mathit{w}=\mathit{J}\mathit{n}$$

(6)

where,

$$\mathit{J}={\left(\begin{array}{ccccc}\frac{\partial \gamma }{\partial \alpha }& \frac{\partial \gamma }{\partial \beta }& \frac{\partial \gamma }{\partial \varphi }& \frac{\partial \gamma }{\partial \theta }& \frac{\partial \gamma }{\partial \phi }\\ \frac{\partial \vartheta }{\partial \alpha }& \frac{\partial \vartheta }{\partial \beta }& \frac{\partial \vartheta }{\partial \varphi }& \frac{\partial \vartheta }{\partial \theta }& \frac{\partial \vartheta }{\partial \phi }\end{array}\right)}_{\left|\alpha ,\beta ,\varphi ,\theta ,\phi \right.}$$

(7)

denotes the Jacobian matrix at the current measurement. If the direct measurement is assumed with Gaussian-distributed noise, the errors of the attitude angles of the LoS can also be treated as Gaussian, and their expectation and variance can be computed using (7).

With the above analysis, the target localization problem can be depicted as a 3D bearing-only target localization problem, which corresponds to the direct measurement using (5). For the ease of expressing our idea, the following paper will treat the attitude angles of LoS as the direct measurement.

As stated in the previous section, if there is no prior information available, multiple LoS equations still cannot make up the sufficient condition to determine the position of a moving target because the moving target may be unobservable with the achieved angle measurements. To make the moving target observable using arbitrary measurement, one more dimensional measurement is usually needed such as the real-time distance or the relative height between the target and UAV. In our project, the height of the target was assumed to be known because the height of the ground plane, which can usually be treated as flat, can be measured using GPS before localizing the target. Therefore, the measurement model for the moving target can be expressed as:

$$\mathit{z}=\mathit{h}(\mathbf{x})+\mathit{w}$$

(8)

where $\mathbf{x}={[x_g,y_g]}^{\top }$ denotes the horizontal position vector of the target; $\mathit{z}={[\gamma ,\vartheta ]}^{\top }$ represents the measurement vector; and the expression of $\mathit{h}(\mathbf{x})$ is exactly the equation in (4). As $z_g$ is known, there are only two variables to be estimated, and hence the target position can be estimated by (4). Thus, the target is observable with the attitude measurement of the LoS.

3.2. Simultaneous Target Localization and UAV Trajectory Optimization

The attitude angles of LoS are measured with random errors, and thus there is unavoidable error in the localization result. However, the localization performance to the moving target is not only influenced by the measurement noises, but also significantly influenced by the trajectory of the UAV, which determines the sensor–target geometry at each time instant. Therefore, to best localize the target position, an optimized trajectory should be generated automatically according to the desired Sensor–target geometry. As the true position of the target is unknown, the localization uncertainty minimization objective of the moving target relies closely on the estimate of the target. In other words, the position estimation for the target and trajectory optimization for the UAV are closely coupled, which makes the simultaneous target localization and UAV trajectory optimization problem complicated, as shown in Figure 4a.

For the discrete measurement case, one solution to decoupling the problem is to optimize the next waypoint of the UAV immediately each time the estimation task of the target position is completed, as shown in Figure 4b. The existing methods use the current estimate of the target position to optimize the next waypoint of the observer. However, when the measurement time interval is large, the current estimate of the target position deviates largely from the actual position at the next time instant. This will make the next optimized waypoint of the UAV invalid. To overcome this problem, the predicted estimate of the target position was utilized in this paper. This can be expressed as follows. In the estimation phase of time step $k$, the target position is estimated using the optimized waypoint of UAV at time step $k-1$, and then, in the trajectory optimization phase, the predicted position of the target given the obtained estimate is exploited to optimize the waypoint of UAV at time step $k+1$. In this way, the target localization and UAV trajectory optimization are implemented concurrently, leading to the localization performance being maximized and the UAV trajectory is also generated automatically.

To facilitate the simultaneous target localization and UAV trajectory optimization, the EIF target state estimation method was exploited, which can express the predicted FIM information of the target state explicitly and iteratively. Using the predicted FIM obtained from the target motion model and the current estimate, the UAV waypoint optimization objective can be established. The sequel will first introduce the EIF method for moving target localization, and then present the UAV trajectory optimization algorithm.

3.2.1. Target Localization Using EIF

Assume the target has a constant velocity motion model as follows:

$${\mathbf{x}}_k={\mathsf{\Phi }}_{k|k-1}{\mathbf{x}}_{k-1}+{\mathsf{\Gamma }}_{k|k-1}{\mathit{v}}_{k-1}$$

(9)

where ${\mathbf{x}}_k={[x_{g,k},y_{g,k},v_{x,k},v_{y,k}]}^{\top }$ denotes the target state at time step $k$, of which the first two components represent the horizontal position of the target and the last two components are the velocities in the horizontal plane; and

$${\mathsf{\Phi }}_{k|k-1}=\left(\begin{array}{cc}{\mathit{I}}_{2\times 2}& \Delta t{\mathit{I}}_{2\times 2}\\ {\mathbf{0}}_{2\times 2}& {\mathit{I}}_{2\times 2}\end{array}\right)$$

(10)

denotes the state transition matrix of the target from time $k-1$ to time $k$, where ${\mathit{I}}_{2\times 2}$,${\mathbf{0}}_{2\times 2}$ denote the identity matrix and zero matrix of order 2, respectively; $\Delta t$ denotes the time interval of measurements; and

$${\mathsf{\Gamma }}_{k|k-1}=\left(\begin{array}{c}\Delta t{\mathit{I}}_{2\times 2}\\ {\mathit{I}}_{2\times 2}\end{array}\right)$$

(11)

denotes the noises input matrix; ${\mathit{v}}_{k-1}={(n_x,n_y)}^{\top }$ denotes the process noise assumed as zero mean Gaussian with covariance matrix ${\mathit{Q}}_{k-1}$. Based on (8), in the previous section, we have the measurement model of the form:

$${\mathbf{z}}_k=\mathit{h}({\mathbf{x}}_k)+{\mathit{w}}_k$$

(12)

where ${\mathbf{z}}_k$,${\mathit{w}}_k$ denote the measurement vector and measurement noise vector at time $k$, respectively; and ${\mathit{w}}_k$ is assumed as zero mean Gaussian with covariance matrix ${\mathit{R}}_k$ and independent from ${\mathit{v}}_{k-1}$.

Given the initial state ${\widehat{\mathbf{x}}}_0$ and covariance matrix ${\mathbf{P}}_0$, the filtering methods such as EKF and uscent Kalman filtering (UKF) can be used to iteratively and consistently estimate target state, performing the real-time localization to the moving target. However, to optimize the UAV trajectory so that the target localization uncertainty can be minimized, the predicted FIM is needed in the trajectory optimization phase. To circumvent the inverse computation of predicted covariance matrix in each iteration, the information filtering corresponding to EKF is used, which can express the predicted FIM of the target state explicitly in each iteration. Furthermore, compared with the existing methods, using the predicted FIM, the process noise of the target model was also taken into consideration, which can optimize the next waypoint of UAV more effectively.

EIF is an information form of the extended Kalman filtering, of which the target state estimation and covariance matrix are replaced with the information state vector and information matrix, respectively. The relation between them can be expressed as follows:

$$\left\{\begin{array}{l}{\mathbf{Y}}_k={\mathbf{P}}_k^{-1}\\ {\widehat{\mathbf{y}}}_k={\mathbf{P}}_k^{-1}{\widehat{\mathbf{x}}}_k\end{array}\right.$$

(13)

where ${\mathbf{Y}}_k$,${\widehat{\mathbf{y}}}_k$ denote the information matrix and information state vector at time $k$, respectively; ${\mathbf{P}}_k$,${\widehat{\mathbf{x}}}_k$ denote the covariance matrix and target state estimation at time step $k$, respectively. Using (13), the initial information matrix ${\mathbf{Y}}_0$ and the information state vector ${\widehat{\mathbf{y}}}_0$ can be obtained. Given the information matrix ${\mathbf{Y}}_{k-1}$ and information state vector ${\widehat{\mathbf{y}}}_{k-1}$ at time instant $k-1$, the EIF advances with time as follows:

• Prediction update:

  $${\mathbf{Y}}_{k\left|k-1\right.}={\left({\mathsf{\Phi }}_{k|k-1}{\mathbf{Y}}_k^{-1}{\mathsf{\Phi }}_{k|k-1}^{\top }+{\mathsf{\Gamma }}_{\mathbf{k}|\mathbf{k}-1}{\mathit{Q}}_{k-1}{\mathsf{\Gamma }}_{{}_{\mathbf{k}|\mathbf{k}-1}}^{\top }\right)}^{-1}$$

  (14)

  $${\mathbf{B}}_{k\left|k-1\right.}={\mathbf{Y}}_{k\left|k-1\right.}{\mathsf{\Phi }}_{k|k-1}{\mathbf{Y}}_k^{-1}$$

  (15)

  $${\widehat{\mathbf{y}}}_{k\left|k-1\right.}={\mathbf{B}}_{k\left|k-1\right.}{\widehat{\mathbf{y}}}_{k-1}$$

  (16)
• Measurement update:

  $${\mathbf{i}}_k={\mathbf{H}}_k^{\top }{\mathit{R}}_k^{-1}{\mathbf{z}}_k$$

  (17)

  $${\mathbf{I}}_k={\mathbf{H}}_k^{\top }{\mathit{R}}_k^{-1}{\mathbf{H}}_k$$

  (18)

  $${\widehat{\mathbf{y}}}_k={\widehat{\mathbf{y}}}_{k\left|k-1\right.}+{\mathbf{i}}_k$$

  (19)

  $${\mathbf{Y}}_k={\mathbf{Y}}_{k\left|k-1\right.}+{\mathbf{I}}_k$$

  (20)

  where

  $${\mathbf{H}}_k=\left(\begin{array}{cc}\frac{\partial \gamma }{\partial x_g}& \frac{\partial \gamma }{\partial y_g}\\ \frac{\partial \vartheta }{\partial x_g}& \frac{\partial \vartheta }{\partial y_g}\end{array}\right)$$

  (21)

  denotes the Jacobian matrix of $\mathit{h}({\mathbf{x}}_k)$ with respect to ${\mathbf{x}}_k$ at ${\widehat{\mathbf{x}}}_{k\left|k-1\right.}$, and ${\widehat{\mathbf{x}}}_{k\left|k-1\right.}$ can be computed using the corresponding information matrix and information state vector as follows:

  $${\widehat{\mathbf{x}}}_{k\left|k-1\right.}={\mathbf{Y}}_{k\left|k-1\right.}^{-1}{\widehat{\mathbf{y}}}_{k\left|k-1\right.}$$

  (22)

Using the above procedure, the moving target can be located consistently in real-time.

3.2.2. UAV Trajectory Optimization

Although the EIF can estimate the target position consistently, the sensor-target geometry also significantly influences the performance of target localization. Based on the decoupling structure, the UAV trajectory should be optimized at each waypoint.

Assuming the previous waypoints have existed at time $k$, and the next waypoint at time $k+1$ is to be optimized. According to relevant theory in the literature, making the uncertainty of the target position estimate minimized is equivalent to maximizing the posterior FIM after observation at time $k+1$ in some measure, which can be expressed mathematically as:

$$\max \mathcal{T}\left[{\mathbf{Y}}_{k+1}\right]$$

(23)

where $\mathcal{T}$ denotes some measure of the FIM. On the other hand, the UAV maneuverability constraints should also be taken into account:

$$\left\{\begin{array}{c}{\gamma }_{\mathrm{h}}\le {\gamma }_{\mathrm{h}}{,}_{\max }\\ {\gamma }_{\mathrm{v}}\le {\gamma }_{\mathrm{v}}{,}_{\max }\\ v_{\min }\le ‖{\mathit{v}}_k‖\le v_{\max }\\ z_{u,k+1}-z_0\ge h_{\min }\hfill \end{array}\right.,\forall k=1,2,\cdots$$

(24)

where ${\mathit{v}}_k$ is the velocity vector of the UAV; $v_{\min }$,$v_{\max }$ denote the minimum and maximum velocity values, respectively; ${\gamma }_{\mathrm{h}}$,${\gamma }_{\mathrm{v}}$ denote the horizontal turn rate and vertical turn rate with ${\gamma }_{\mathrm{h}}{,}_{\max }$,${\gamma }_{\mathrm{v}}{,}_{\max }$ as their maximum values, respectively; $z_{u,k+1}$ denotes the third component of ${\mathit{y}}_{k+1}$, the UAV position vector in $C^g$; $z_0$ represents the height of ground plane (i.e., the third component of target position vector). The first constraint is about the maximum turn rate of UAV; the second constraint is about the maximum velocity of UAV; the third equality is to restrict the relative height between the target and the UAV. Combining (23) and (24), for UAV trajectory optimization, we have the following optimization problem:

$$\begin{array}{c}\max \mathcal{T}\left[{\mathbf{Y}}_{k+1}\right]\hfill \\ \mathrm{s}.\mathrm{t}.{\mathbf{Y}}_{k+1}={\mathbf{Y}}_{k+1\left|k\right.}+{\mathbf{I}}_{k+1}\hfill \\ {\gamma }_{\mathrm{h}}\le {\gamma }_{\mathrm{h}}{,}_{\max }\\ {\gamma }_{\mathrm{v}}\le {\gamma }_{\mathrm{v}}{,}_{\max }\\ v_{\min }\le ‖{\mathit{v}}_k‖\le v_{\max }\\ z_{u,k}-z_0\ge h_{\min }\forall k=1,2,\cdots \end{array}$$

(25)

where the predicted information matrix ${\mathbf{Y}}_{k+1\left|k\right.}$ and information gain ${\mathbf{I}}_{k+1}$ at time step $k+1$ can be computed by (14) and (18) using the current information matrix and information state vector ${\mathbf{Y}}_k$,${\widehat{\mathbf{y}}}_k$. Note that in order to optimize the UAV trajectory, ${\mathbf{I}}_{k+1}$ should be expressed in terms of the target position, and as the target position is unknown, we replaced it with the predicted state, ${\widehat{\mathbf{x}}}_{k+1\left|k\right.}$.

Before proceeding to solve the optimization problem of (25), the valid FIM of the target state for the trajectory optimization should be illustrated. As the Fisher information is a way of measuring the amount of information that some measurement carries about an unknown variable, upon which the probability of the measurement depends, the FIM is a way of measuring the information of an unknown vector. Each element of the FIM corresponds to the amount of information about the corresponding variable element in the vector to be estimated. For example, the element in entry (1,1) of the ${\mathbf{Y}}_{k+1}$ represents the information of the component $x_{g,k+1}$ of the posterior target state estimate vector from the prior estimate and the current measurement, and the element in entry (1,2) of ${\mathbf{Y}}_{k+1}$ represents the information relevant with both component $x_{g,k+1}$ and component $y_{g,k+1}$ of the target state vector. As the measurement model (12) only involves the position variables of the target state vector, the essential variables to be estimated is the position variables, according to which the velocity variables can be inferred. Hence, the information we really care about is the information involved with the position variables. Denote the information matrix involved with the position variables of the target state vector by ${\tilde{\mathbf{Y}}}_{k+1}$, which can be obtained by dropping the columns of order 3, 4, and the rows of order 3, 4 in the information matrix,${\tilde{\mathbf{Y}}}_{k+1}$. Furthermore, the predicted information matrix ${\tilde{\mathbf{Y}}}_{k+1\left|k\right.}$ and information gain ${\tilde{\mathbf{I}}}_{k+1}$ involved with target position variables corresponding to ${\mathbf{Y}}_{k+1\left|k\right.}$ and ${\mathbf{I}}_{k+1}$ can also be achieved in the same way. Hence, we have

$${\tilde{\mathbf{Y}}}_{k+1}={\tilde{\mathbf{Y}}}_{k+1\left|k\right.}+{\tilde{\mathbf{I}}}_{k+1}$$

(26)

As ${\mathbf{Y}}_{k+1\left|k\right.}$ is predicted from ${\mathbf{Y}}_k$, it can be computed directly with the current estimate. Hence, only ${\tilde{\mathbf{I}}}_{k+1}$ is related to ${\mathit{y}}_{k+1}$, the next waypoint of UAV. To directly compute ${\tilde{\mathbf{I}}}_{k+1}$, the Jacobian matrix of the measurement with respect to the predicted target position vector can be computed as:

$$\begin{array}{cc}\hfill {\tilde{\mathbf{H}}}_{k+1}& =\left(\begin{array}{cc}\frac{\partial \gamma }{\partial x_{g,k+1|k}}& \frac{\partial \gamma }{\partial y_{g,k+1|k}}\\ \frac{\partial \vartheta }{\partial x_{g,k+1|k}}& \frac{\partial \vartheta }{\partial y_{g,k+1|k}}\end{array}\right)\hfill \\ & =\left(\begin{array}{cc}-\frac{{\mathit{r}}_k(2)}{{\mathit{r}}_k{(1)}^2+{\mathit{r}}_k{(2)}^2}& \frac{{\mathit{r}}_k(1)}{{\mathit{r}}_k{(1)}^2+{\mathit{r}}_k{(2)}^2}\\ -\frac{{\mathit{r}}_k(3){\mathit{r}}_k(1)}{{\Vert {\mathit{r}}_k\Vert }^2\sqrt{{\mathit{r}}_k{(1)}^2+{\mathit{r}}_k{(2)}^2}}& -\frac{{\mathit{r}}_k(3){\mathit{r}}_k(2)}{{\Vert {\mathit{r}}_k\Vert }^2\sqrt{{\mathit{r}}_k{(1)}^2+{\mathit{r}}_k{(2)}^2}}\end{array}\right)\hfill \end{array}$$

(27)

where ${\mathit{r}}_k={[x_{g,k+1\left|k\right.}-x_{u,k+1},y_{g,k+1\left|k\right.}-y_{u,k+1},z_0-z_{u,k+1}]}^{\top }$ denotes the target pointing vector, and ${\mathit{r}}_k(i)$ denotes the $i$th element of ${\mathit{r}}_k$, $i=1,2,3$; $x_{g,k+1\left|k\right.}$ and $y_{g,k+1\left|k\right.}$ denote the position elements of the predicted state at the time step $k$. Then, we have:

$${\tilde{\mathbf{I}}}_{k+1}={\tilde{\mathbf{H}}}_{k+1}^{\top }{\mathit{R}}_{k+1}^{-1}{\tilde{\mathbf{H}}}_{k+1}$$

(28)

For computation facility, the Cartesian coordinate representation can be transformed into the spherical coordinate form. In the spherical coordinate frame, the origin is selected as the predicted target position; then we have:

$$\left\{\begin{array}{l}{x}_{u,k+1}=\rho \cos \eta \cos \omega +x_{g,k+1\left|k\right.}\\ y_{u,k+1}=\rho \cos \eta \sin \omega +y_{g,k+1\left|k\right.}\\ z_{u,k+1}=\rho \sin \eta +z_0\end{array}\right.$$

(29)

where ${(\rho ,\omega ,\eta )}^{\top }$ denotes the spherical coordinate of the UAV position; $\rho$, $\omega$, and $\eta$ denote the radius, azimuth angle, and elevation angle, respectively. Substitute (29) and (27) into (28), we have

$${\tilde{\mathbf{I}}}_{k+1}=\frac{1}{{\rho }^2}\left(\begin{array}{cc}\frac{{\sin }^2\omega }{{\sigma }_{\gamma }^2{\cos }^2\eta }+\frac{{\sin }^2\eta {\cos }^2\omega }{{\sigma }_{\vartheta }^2}& -\frac{\sin \omega \cos \omega }{{\sigma }_{\gamma }^2{\cos }^2\eta }+\frac{{\sin }^2\eta \cos \omega \sin \omega }{{\sigma }_{\vartheta }^2}\\ -\frac{\sin \omega \cos \omega }{{\sigma }_{\gamma }^2{\cos }^2\eta }+\frac{{\sin }^2\eta \cos \omega \sin \omega }{{\sigma }_{\vartheta }^2}& \frac{{\cos }^2\omega }{{\sigma }_{\gamma }^2{\cos }^2\eta }+\frac{{\sin }^2\eta {\sin }^2\omega }{{\sigma }_{\vartheta }^2}\end{array}\right)$$

(30)

where ${\sigma }_{\gamma }$,${\sigma }_{\vartheta }$ denote the standard deviation of the azimuth and elevation angle, respectively.

To express the problem explicitly, a proper measure $\mathcal{T}$ should be selected. Different measures will lead to different optimization criterion. The optimization criterion includes: D-criterion, A-criterion, and E-criterion. Among them, the D-criterion leads to minimizing the determinant of the inverse of the FIM, which actually minimizes the area of the ellipse of uncertainty; the A-criterion results in minimizing the trace of the inverse of the FIM, which usually minimizes the average radius of the uncertainty ellipse, and the E-criterion, which minimizes the maximum radius of the uncertainty ellipse. Considering the optimization effectiveness and computation complexity [1], the measure that leads to the A-criterion was selected to optimize the next waypoint of UAV in (25).

With the above arrangements and analysis, we can achieve the following optimization problem:

$$\begin{array}{l}\underset{y_{k+1}}{\min } \mathrm{trace}\left({\tilde{\mathbf{Y}}}_{k+1}^{-1}\right)\\ \mathrm{s}.\mathrm{t}.{\tilde{\mathbf{Y}}}_{k+1}={\tilde{\mathbf{Y}}}_{k+1\left|k\right.}+{\tilde{\mathbf{I}}}_{k+1}\\ \left|{\omega }_k-{\omega }_{k+1}\right|\le \Delta {\omega }_{\max }\\ \left|{\eta }_k-{\eta }_{k+1}\right|\le \Delta {\eta }_{\max }\\ \Delta t{v}_{\min }\le ‖y_{k+1}-y_k‖\le \Delta t{v}_{\max }\\ z_{u,k}-z_0\ge h_{\min } \forall k=1,2,\cdots \end{array}$$

(31)

where the constraints have been expressed as feasible forms in the discrete measurement case; the operator $\mathrm{trace}\left(\cdot \right)$ denotes the trace operation of a matrix. The second and third constraints are to restrict the horizontal and vertical turn rate of the UAV; the fourth constraint is to constrain the minimum and maximum velocity of UAV; ${\mathit{y}}_k$ is the current position of the UAV. The optimization problem is nonconvex in general, which makes the optimization quite sensitive to initial positions of UAV and may cause the optimizer to get stuck in local minima if not initialized properly. However, if the initial position of UAV is initialized with the current position ${\mathit{y}}_k$ and the interior point method is utilized to solve the above problem, it is easy to find the global minimum solution.

Combined with the target position estimation with the UAV trajectory optimization phases together, a simultaneous target localization and UAV trajectory optimization procedure is presented as follows:

Step 1: Predict the information state and information matrix based on the initial state ${\widehat{\mathbf{x}}}_0$ and covariance matrix ${\mathbf{P}}_0$ using the target motion model (9), achieving ${\widehat{\mathbf{y}}}_{1\left|0\right.}$,${\mathbf{Y}}_{1\left|0\right.}$.

Step 2: Solve the optimization problem using the interior point method based on current UAV position ${\mathit{y}}_0$, obtaining ${\mathit{y}}_1$, and control the UAV to reach the indexed waypoint ${\mathit{y}}_1$ using the onboard control algorithm.

Step 3: Conduct observation to the target. Utilize the new measurement to correct the predict information state and information matrix, achieving ${\widehat{\mathbf{y}}}_1$,${\mathbf{Y}}_1$.

Step 4: Predict the information state and information matrix based on the initial state ${\widehat{\mathbf{x}}}_k$ and covariance matrix ${\mathbf{P}}_k$ using the target motion model (9), achieving ${\widehat{\mathbf{y}}}_{k+1\left|k\right.}$,${\mathbf{Y}}_{k+1\left|k\right.}$.

Step 5: Solve the optimization problem using the interior point method based on current UAV position ${\mathit{y}}_k$, obtaining ${\mathit{y}}_{k+1}$, and controlling the UAV to reach the indexed waypoint ${\mathit{y}}_{k+1}$.

Step 6: Conduct observations on the target. Utilize the new measurement to correct the predict information state and information matrix, achieving ${\widehat{\mathbf{y}}}_{k+1}$,${\mathbf{Y}}_{k+1}$.

Step 7: Repeat Step 4~Step 6.

The pseudocode for the algorithm of simultaneous target localization and UAV trajectory optimization algorithm is shown as Algorithm 1.

Algorithm 1: Simultaneous target localization and UAV trajectory optimization.
Input: Initial state ${\widehat{\mathbf{x}}}_0$ and covariance matrix ${\mathbf{P}}_0$; initial position of UAV ${\mathit{y}}_0$; Process noise covariance: ${\mathit{Q}}_k$; Measurement noise covariance: ${\mathit{R}}_k$,$\forall k=0,1,2\dots$
Output: UAV waypoints ${\mathit{y}}_k$, and target position estimate ${\widehat{\mathbf{x}}}_k$; $\forall k=0,1,2\dots$
1:	${\widehat{\mathbf{y}}}_0\leftarrow {\mathbf{P}}_0^{-1}{\widehat{\mathbf{x}}}_0$;${\mathbf{Y}}_0\leftarrow {\mathbf{P}}_0^{-1}$;
2:	for$k=1;k\le T$ do
3:	${\mathbf{Y}}_{k\left|k-1\right.}\leftarrow {\left({\mathsf{\Phi }}_{k\left|k-1\right.}{\mathbf{Y}}_{k-1}^{-1}{\mathsf{\Phi }}_{k\left|k-1\right.}^{\top }+{\mathsf{\Gamma }}_{k\left|k-1\right.}{Q}_k{\mathsf{\Gamma }}_{k\left|k-1\right.}^{\top }\right)}^{-1}$;
4:	${\tilde{\mathbf{Y}}}_{k\left|k-1\right.}\leftarrow {\mathbf{Y}}_{k\left|k-1\right.}$;
5:	Establish the objective: ${\tilde{\mathbf{Y}}}_k\leftarrow {\tilde{\mathbf{Y}}}_{k\left|k-1\right.}+{\tilde{\mathbf{I}}}_k$;
	${\mathit{y}}_k$$\leftarrow$ Solve the optimization problem (31) using interior points method.
6:	${\mathbf{B}}_{k\left|k-1\right.}\leftarrow {\mathbf{Y}}_{k\left|k-1\right.}{\mathsf{\Phi }}_{k|k-1}{\mathbf{Y}}_k^{-1}$;
7:	${\widehat{\mathbf{y}}}_{k\left|k-1\right.}\leftarrow {\mathbf{B}}_{k\left|k-1\right.}{\widehat{\mathbf{y}}}_{k-1}$;
8:	${\mathbf{I}}_k\leftarrow {\mathbf{H}}_k^{\top }{\mathit{R}}_k^{-1}{\mathbf{H}}_k$;
9:	Wait for a new measurement: ${\mathbf{z}}_k$;
10:	${\mathbf{i}}_k\leftarrow {\mathbf{H}}_k^{\top }{\mathit{R}}_k^{-1}{\mathbf{z}}_k$;
11:	${\mathbf{I}}_k\leftarrow {\mathbf{H}}_k^{\top }{\mathit{R}}_k^{-1}{\mathbf{H}}_k$;
12:	${\mathbf{Y}}_k\leftarrow {\mathbf{Y}}_{k\left|k-1\right.}+{\mathbf{I}}_k$;
13:	${\widehat{\mathbf{y}}}_k\leftarrow {\widehat{\mathbf{y}}}_{k\left|k-1\right.}+{\mathbf{i}}_k$;
14:	${\widehat{\mathbf{x}}}_k\leftarrow {\mathbf{Y}}_k^{-1}{\widehat{\mathbf{y}}}_k$;
15:	$k\leftarrow k+1$;
16:	end for

3.2.3. Generalization to Other Motion Models

Note that the above description of the simultaneous target localization and UAV trajectory optimization was based on the assumption that the target has a constant velocity motion mod0el. However, the above algorithm is also applicable to other motion models such as the constant acceleration model and constant turn motion model, which are the most widely used motion models [28].

As different motion models of the moving target have different state transition functions with the different dimensionalities of the target state vector, the target estimation procedures are also different for different motion models. For instance, for the constant acceleration motion model, the state transition function has the same form as (9), except that the transition matrix and noise input matrix have different forms:

$${\mathbf{x}}_k={\mathsf{\Phi }}_{k|k-1}^a{\mathbf{x}}_{k-1}+{\mathsf{\Gamma }}_{k|k-1}^a{\mathit{v}}_{k-1}^a$$

(32)

where

$${\mathsf{\Phi }}_{k|k-1}^a=\left(\begin{array}{ccc}{\mathit{I}}_{2\times 2}& \Delta t{\mathit{I}}_{2\times 2}& \frac{1}{2}\Delta t^2{\mathit{I}}_{2\times 2}\\ {\mathbf{0}}_{2\times 2}& {\mathit{I}}_{2\times 2}& \Delta t{\mathit{I}}_{2\times 2}\\ {\mathbf{0}}_{2\times 2}& {\mathbf{0}}_{2\times 2}& {\mathit{I}}_{2\times 2}\end{array}\right)$$

(33)

and

$${\mathsf{\Gamma }}_{k|k-1}^a=\left(\begin{array}{c}\frac{1}{2}\Delta t^2{\mathit{I}}_{2\times 2}\\ \Delta t{\mathit{I}}_{2\times 2}\\ {\mathit{I}}_{2\times 2}\end{array}\right)$$

(34)

denote the transition matrix and noise input matrix, respectively. Additionally, note that the target state vector is of six dimensions, as ${\mathbf{x}}_k={[x_{g,k},y_{g,k},v_{x,k},v_{y,k},a_{x,k},a_{y,k}]}^{\top }$, where the last two elements denote the acceleration variables; and ${\mathit{v}}_{k-1}^a$ is noise vector of the acceleration. Corresponding to this target motion model, the predicted information matrix involved with the position variables can be obtained by dropping the columns and rows of order from 3 to 6 of the full information matrix.

On the other hand, the transition matrix will be replaced with the Jacobian matrix obtained at the previous state if the target has a constant turn motion model because the state transition function is nonlinear. The target state also contains the turn rate of the target compared with the constant velocity model. Hence, the position-related predicted information matrix can be acquired by dropping the columns and rows of order from 3 to 5. The corresponding noise input matrix and noise vector is also changed according to the constant turn model. For more details, we refer the reader to [28].

4. Simulations, Experiments and Results Analysis

To validate the localization performance obtained from the UAV trajectory optimized by the proposed algorithm, simulations and field experiments were conducted.

4.1. Simulations

The simulations were conducted based on the full measurement of the UAV target localization system (i.e., the measurement contains the attitude angles of the UAV and camera). Utilizing (6), the required measurement errors of the azimuth and elevation angle of LoS were computed. The measurement noises were both set as zero mean Gaussian, of which the symbol (SYM) and standard deviations (STD) are listed in

Table 1. Measurement noise setting.

	Roll Angle	Pitch Angle	Yaw Angle	Azimuth	Elevation
SYM	$\varphi$	$\theta$	$\phi$	$\alpha$	$\beta$
STD (degree)	0.8	0.8	1.2	1	1

.

For the simulations, three scenarios were designed: the first scenario had with a moving target with a constant velocity model; the second scenario had the constant turn rate model; and the third had a constant acceleration motion model. For simplicity, the flight heights were all fixed at 600 m.

In the scenario with the constant velocity model, the initial point of the target was set as the origin of $C^g$, the velocity was set as 25 m/s and 12 m/s in the two axes of the horizontal plane; the initial position of the UAV was set as (−1000 m, 0 m, 600 m), and the minimum and maximum velocity were set as 70 m/s and 100 m/s, respectively, and the maximum turn rate was 12 degree/s. The time interval of measurement was one second and the tracking lasts 100 s. Using the above trajectory optimization algorithm, the trajectory result is obtained as Figure 5. To learn the relation between the target and the optimized trajectory, the trajectory optimization result relative to the target position is shown in Figure 5 (i.e., the optimized trajectory when treating the target position as the origin). From the trajectory result, we can see that the optimal trajectory tends to form circles around the target, where the target position is the circle center.

To illustrate the localization performance, 100 Monte Carlo trails were conducted. The root mean squared error (RMSE) results are shown in Figure 6, where the localization method in [1] (EKF based on stochastic model) and the method similar to the referenced method in [17] (notated by STLUTO-IDEAL) are also presented. The two referenced methods are also simultaneous target localization and trajectory optimization strategies, among which the first one assumes target motion as a stochastic model, and the second one does not take into account the process noise. In the simulations, for the first method, the process noise covariance was set as $27.7\times 27.7\times {\mathit{I}}_{2\times 2}$ because the maximum velocity value was 27.7m/s; for the second method, the MLE was used to estimate the target position. From the comparison, we could see that the simultaneous target localization and UAV trajectory optimization based EIF (notated by STLUTO-EIF) had overall lower errors than the two referenced methods. The method of EKF based on the stochastic model had an obviously large error than the other two methods, of which the localization error tended to diverge at $t\approx 45$ s. The underlying reason is that the EKF-based method predicts the position badly using the stochastic model. The method without considering the process noise had an obvious better performance than the EKF-based method, but it has a large error than the proposed method. This shows that the consideration of process noise can improve the effectiveness of the optimized trajectory of the UAV.

For the second scenario, the target motion model was changed accordingly. The initializations of the target state and UAV, and the constraints settings were the same as scenario 1; the turn rate of the target was set as 2 degree/s. The trajectory optimization result is shown in Figure 7.

From the optimized trajectory, we can see that similar to the constant velocity scenario, the optimized trajectory of UAV tends to form a circle around the target with the target as the center. The comparison of localization performances for the proposed method and the referenced method is shown in Figure 8. We can see that the optimized trajectory by the proposed method in this paper can still achieve a better localization performance than that of the referenced methods.

In the scenario with the constant acceleration motion model, the initializations and constraints were the same as the above scenarios; and the acceleration of the target were set as 0.135m/s², 0.260 m/s² in the $X^g$, $Y^g$ directions, respectively. The trajectory optimization result is shown in Figure 9. From the result, we can see that the optimal trajectory was similar to the other two scenarios. The localization performance is shown in Figure 10. The RMSE comparisons of the proposed method and the referenced methods showed that using the predicted information matrix could also better predict the target position and improve the effectiveness of the trajectory for minimizing the localization uncertainty of the target with an acceleration motion model.

All three simulation results coincided with the phenomenon that the optimal path for a moving sensor to maximize a stationary target’s observability is a circle around it. Furthermore, how large the circle is determined by the UAV’s limit and the UAV’s allowable turn rate, which is essentially determined by the UAV’s acceleration. In fact, the circle around the target exists because the allowable minimum speed is not zero. According to the simulation setting where the relative UAV height from the target is known, the optimal position to minimize the target position estimate uncertainty is just above the target location. The algorithm always tries to find a waypoint nearest to the target at each optimization step while satisfying the UAV maneuverability requirement.

From the simulation results, it can be seen that the simultaneous target localization and UAV trajectory optimization algorithm based on EIF is applicable to the common motion models, and can achieve an obvious improvement on the localization performance compared with the existing methods. The underlying reason includes two aspects. First, the proposed algorithm exploits the EIF to consistently estimate the target state by predicting the target state based on the target dynamic model with the previous states and correcting it with the latest obtained measurement. Second, the consistent estimate of the target position was utilized to optimize the next UAV’s waypoint, which makes the waypoint more effective for suppressing the target estimate uncertainty. However, the other two methods, which either use the stochastic model or cannot consistently estimate the target state, are likely to degrade the localization performance.

4.2. Field Experiment

To validate the effectiveness of the proposed method in the field applications, field experiments were conducted in this subsection. The platform we used was the DJI M100, a platform for developers as shown in Figure 11, which was modified to output the attitude angles of the camera, but the precision of the camera attitude was low with a standard deviation of about 5 degrees. The onboard IMU can output the UAV attitude angles with error levels ranging from 1 degree to 5 degrees, and we assumed them as zero mean Gaussian noises, for which the standard deviations are shown in

Table 2. Measurement noise setting.

	Roll Angle	Pitch Angle	Yaw Angle	Azimuth	Elevation
SYM	$\varphi$	$\theta$	$\phi$	$\alpha$	$\beta$
STD (degree)	1.5	1.5	2	5	5

.

The target-tracking video algorithm, which can detect and track a pre-specified target was incorporated into the main board of the UAV. The optimized waypoints for UAV were generated by the proposed method, which were then the input index for the internal control algorithm and linear model predictive control (LMPC) algorithm [29]. The control algorithm, together with the information of the current UAV state, was responsible for controlling the UAV to arrive the index point.

We designed the experiment to track and localize an outdoor car, as shown in Figure 12. The car performed an approximately constant velocity motion (5 m/s along the $X^g$ direction), and the covariance for the process noise were set as ${\mathit{I}}_{2\times 2}$ (i.e., the motion uncertainty in each direction was set as 1 m; the flight altitude is set as about 100 m; UAV had the initial position as (0 m, 100 m, 100 m)). According to the constraints of UAV motion, the experiments were conducted in two scenarios. The first scenario was with the UAV minimum and maximum velocities set as 0 m/s and 9 m/s, respectively; the second scenario was with the UAV minimum and maximum velocity set as 5 m/s and 9 m/s, respectively.

In each iteration, after the optimized waypoint is computed, the onboard control algorithm guided the UAV to the indexed waypoint. Therefore, we achieved the actual UAV trajectory result, but not the planned trajectory. The actual trajectory results were obtained as Figure 13a,b. Figure 14 is the relative trajectory result for the scenario with the minimum velocity of 5 m/s, and as the relative trajectory of Figure 13a is trivial, it was neglected. From Figure 13a, we can see that the optimized trajectory nearly coincided with the car trajectory in the horizontal plane in the scenario with the zero minimum velocity constraint. The underlying reason is that the allowable minimum velocity is 0, and the optimal waypoint above the target location can be retained. Figure 13b and Figure 14 show that after being imposed, the nonzero minimum velocity constraint and the relative UAV trajectory still tends to circle the target at a constant height, but the circle was not standard, and instead, the UAV hovered along the $Y$ direction. The difference between the field test results and the simulation results can be explained as follows. In the field test, the resulting UAV waypoints, which are the direct consequence of the LMPC algorithm, are not the output from the waypoint optimization algorithm. Hence, the trajectory result also contains imperfections or the influence of the control algorithm. In the simulation examples, the UAV waypoints were all the direct outputs of the trajectory optimization algorithm, and thus the waypoints are the result under the ideal condition. Note also that the error levels were significantly large in the field experiment, but it did not influence the result much because the flight height was relatively low, and it did not cause a divergence error. Despite the difference, the test results roughly showed the trajectory optimization performance on the whole. The UAV tended to follow the target as quickly as possible and as near as possible to the target, which essentially attempts to minimize the target localization uncertainty.

To validate the localization performance, the actual positions of the car were measured using the RTK-GPS. We repeated the experiment 10 times to compute the RMSE, and the results are shown in Figure 15. In the figures, the localization results of the referenced methods are also presented. From the results, we can see that for the scenario with the minimum zero velocity constraint, the method proposed in this paper was slightly better than the second referenced method, and the EKF-based method was also slightly worse than the other two methods, which was, interestingly, different from the simulation scenario. As the target had a slow speed, the EKF-based method can track and localize it robustly, which is different from the simulation scenarios where the target has a fast speed, which is likely to cause a divergence error. However, for the scenario with a nonzero minimum velocity constraint, the localization performance was obviously better than the referenced methods, which coincided with simulation results. Through the field experiments, we confirmed that the proposed method outperformed the existing similar methods, and are also applicable to engineering applications.

5. Conclusions

This paper presents a closed-form method for simultaneous target localization and the UAV trajectory optimization problem. While decoupling the problem of target localization from the UAV trajectory optimization in the discrete measurement case, the proposed method uses the predicted target state and the corresponding FIM to establish the trajectory optimization objective, which can not only better estimate the target position, but also take the process noise in the target motion model into consideration. Eventually, the optimized trajectory becomes more effective than existing methods of the same class. Solving the objective, the resulting optimized UAV waypoint can be the index point for minimizing the target localization uncertainty in practical engineering applications. In both the simulation examples and field experiments, we have confirmed the analytical findings. Although the field test was conducted on the quadrotor UAV based on the bearing measurement model with the flight height already known, the proposed method was also applicable to the fixed-wing UAV, which often has a nonzero minimum velocity, with other measurement models, for instance, the bearing-distance measurement model and the bearing-only measurement model without any prior information available. However, successful tracking and localizing to the moving target for the bearing-only measurement model require that the UAV has a special maneuverability. Hence, our algorithm has a certain engineering significance. However, the proposed algorithm also has its limitations, as the algorithm can only track and localize the target with a known motion model. Our future research will focus on simultaneous target localization and UAV trajectory optimization for the target with an unknown motion model and multiple targets with known motion models with multiple UAVs to extend this work.