# A Closed-Form Method for Simultaneous Target Localization and UAV Trajectory Optimization
## 3 Simultaneous Target Localization and Unmanned Aerial Vehicle (UAV) Trajectory Optimization  
### 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 localizationproblem. 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{r(3)}{\sqrt{r(1)^{2}+r(2)^{2}}} \right)\\ \vartheta=\arctan\left(\frac{r(2)}{r(1)}\right)\end{array} \right. \tag{4}\]  
where \(r:=x_{g}-y\) denotes the pointing vector from the UAV to the target; \(r(1)\),\(r(2)\), and \(r(3)\) denote the components of \(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, \(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,  
\[r=\mathbf{Rot}_{g}^{b}(\phi,\theta,\varphi)\mathbf{Rot}_{b}^{c}(\alpha,\beta)\left( \begin{array}{c}0\\ 0\\ -l\end{array} \right) \tag{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 \(\mathbf{n}=[\delta\alpha,\delta\beta,\delta\phi,\delta\theta,\delta\varphi]^{\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 \(\mathbf{w}=[\delta\gamma,\delta\theta]^{\top}\) can be approximately expressed as:  
\[\mathbf{w}=\mathbf{In} \tag{6}\]  
where,  
\[\mathbf{J}=\left( \begin{array}{cccc}\frac{\partial\gamma}{\partial\alpha}& \frac{\partial\gamma}{\partial\beta}&\frac{\partial\gamma}{\partial\phi}& \frac{\partial\gamma}{\partial\theta}&\frac{\partial\gamma}{\partial\phi}\\ \frac{\partial\theta}{\partial\alpha}&\frac{\partial\gamma}{\partial\beta}& \frac{\partial\theta}{\partial\phi}&\frac{\partial\theta}{\partial\theta}& \frac{\partial\theta}{\partial\varphi}\\ \end{array} \right)_{[\alpha,\beta,\beta,\phi,\varphi} \tag{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. Figure 3: Schematic for 3D bearing-only target localization. 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.