# A Closed-Form Method for Simultaneous Target Localization and UAV Trajectory Optimization
## 3 Simultaneous Target Localization and Unmanned Aerial Vehicle (UAV) Trajectory Optimization
### Simultaneous Target Localization and UAV Trajectory Optimization
#### 3.2.2 UAV Trajectory Optimization
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}{ll}\min_{y_{k+1}}\text{trace}\left(\widetilde{\mathbf{Y}}_{k +1}^{-1}\right)\\ \text{s.t.}&\widetilde{\mathbf{Y}}_{k+1}=\widetilde{\mathbf{Y}}_{k+1|k}+ \widetilde{\mathbf{I}}_{k+1}\\ |\omega_{k}-\omega_{k+1}|\leq\Delta\omega_{\text{max}}\\ |\eta_{k}-\eta_{k+1}|\leq\Delta\eta_{\text{max}}\\ \Delta tv_{\text{min}}\leq\|y_{k+1}-y_{k}\|\leq\Delta tv_{\text{max}}\\ z_{u,k}-z_{0}\geq h_{\text{min}}\ \ \forall k=1,2,\cdots\end{array}  \tag{31}\]  
where the constraints have been expressed as feasible forms in the discrete measurement case; the operator \(\text{trace}(\cdot)\) 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; \(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 \(\mathbf{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 \(\hat{\mathbf{x}}_{0}\) and covariance matrix \(\mathbf{P}_{0}\) using the target motion model (9), achieving \(\hat{\mathbf{y}}_{1|0}\mathbf{Y}_{1|0}\). Step 2: Solve the optimization problem using the interior point method based on current UAV position \(\mathbf{y}_{0}\), obtaining \(\mathbf{y}_{1}\), and control the UAV to reach the indexed waypoint \(\mathbf{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 \(\hat{\mathbf{y}}_{1}\),\(\mathbf{Y}_{1}\). Step 4: Predict the information state and information matrix based on the initial state \(\hat{\mathbf{x}}_{k}\) and covariance matrix \(\mathbf{P}_{k}\) using the target motion model (9), achieving \(\hat{\mathbf{y}}_{k+1|k}\mathbf{Y}_{k+1|k}\). Step 5: Solve the optimization problem using the interior point method based on current UAV position \(\mathbf{y}_{k}\), obtaining \(\mathbf{y}_{k+1}\), and controlling the UAV to reach the indexed waypoint \(\mathbf{y}_{k+1}\). Step 6: Conduct observations on the target. Utilize the new measurement to correct the predict information state and information matrix, achieving \(\hat{\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.