# 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
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 \(\mathbf{\tilde{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,\(\mathbf{\tilde{Y}}_{k+1}\). Furthermore, the predicted information matrix \(\mathbf{\tilde{Y}}_{k+1|k}\) and information gain \(\mathbf{\tilde{I}}_{k+1}\) involved with target position variables corresponding to \(\mathbf{Y}_{k+1|k}\) and \(\mathbf{I}_{k+1}\) can also be achieved in the same way. Hence, we have  
\[\mathbf{\tilde{Y}}_{k+1}=\mathbf{\tilde{Y}}_{k+1|k}+\mathbf{\tilde{I}}_{k+1} \tag{26}\]As \(\mathbf{Y}_{k+1|k}\) is predicted from \(\mathbf{Y}_{k}\), it can be computed directly with the current estimate. Hence, only \(\widetilde{\mathbf{I}}_{k+1}\) is related to \(\mathbf{y}_{k+1}\), the next waypoint of UAV. To directly compute \(\widetilde{\mathbf{I}}_{k+1}\), the Jacobian matrix of the measurement with respect to the predicted target position vector can be computed as:  
\[\widetilde{\mathbf{H}}_{k+1} = \left( \begin{array}{cc}\frac{\partial\gamma}{\partial x_{k+1|k} }&\frac{\partial\gamma}{\partial y_{k+1|k}}\\ \frac{\partial\theta}{\partial x_{k+1|k}}&\frac{\partial\theta}{\partial y_{k +1|k}}\end{array} \right) \tag{27}\] \[= \left( \begin{array}{cc}-\frac{\frac{\rho_{1}(1)}{\pi_{k}(1)^{2} +\rho_{2}(2)}}{\frac{\rho_{1}(3)\rho_{1}(1)}{\|\pi_{k}\|^{2}\sqrt{n_{(1)}(1)^{ 2}+n_{(2)}}}}&-\frac{\frac{\rho_{1}(1)}{\pi_{k}(1)^{2}+\rho_{2}(2)^{2}}}{\frac {\rho_{1}(1)^{2}+\rho_{2}(2)^{2}}{\|\pi_{k}\|^{2}\sqrt{n_{(1)}(2)^{2}+n_{(2) }}}}\end{array} \right)\]  
where \(\mathbf{r}_{k}=\left[x_{\xi,k+1|k}-x_{u,k+1},y_{\xi,k+1|k}-y_{u,k+1},z_{0}-z_{u,k+ 1}\right]^{\top}\) denotes the target pointing vector, and \(\mathbf{r}_{k}(i)\) denotes the \(i\)th element of \(\mathbf{r}_{k}\), \(i=1,2,3\); \(x_{\xi,k+1|k}\) and \(y_{\xi,k+1|k}\) denote the position elements of the predicted state at the time step \(k\). Then, we have:  
\[\widetilde{\mathbf{I}}_{k+1}=\widetilde{\mathbf{H}}_{k+1}^{\top}\mathbf{R}_{k +1}^{-1}\widetilde{\mathbf{H}}_{k+1} \tag{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}{ll}x_{u,k+1}=\rho\cos\eta\cos\omega+x_{g,k+1|k}\\ y_{u,k+1}=\rho\cos\eta\sin\omega+y_{g,k+1|k}\\ z_{u,k+1}=\rho\sin\eta+z_{0}\end{array} \right. \tag{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  
\[\widetilde{\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\omega ^{2}}{\sigma_{\theta}^{2}}&-\frac{\sin\omega\cos\omega}{\sigma_{\gamma}^{2} \cos^{2}\eta}+\frac{\sin^{2}\eta\cos\omega\sin\omega}{\sigma_{\theta}^{2}}\\ -\frac{\sin\omega\cos\omega}{\sigma_{\gamma}^{2}\cos^{2}\eta}+\frac{\sin^{2} \eta\cos\omega\sin\omega}{\sigma_{\theta}^{2}}&\frac{\cos^{2}\omega}{\sigma_{ \gamma}^{2}\cos^{2}\eta}+\frac{\sin^{2}\eta\sin^{2}\omega}{\sigma_{\theta}^{2} }\end{array} \right) \tag{30}\]  
where \(\sigma_{\gamma}\sigma_{\theta}\) 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.