# A Closed-Form Method for Simultaneous Target Localization and UAV Trajectory Optimization
## 2 Target Localization Based on Visual Platform
### 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,  
\[\mathbf{x}_{g}=\mathbf{Rot}_{g}^{b}(\phi,\theta,\varphi)\mathbf{Rot}_{b}^{c}(\alpha,\beta)\bm {x}_{c}+\mathbf{y} \tag{1}\]  
where \(\mathbf{x}_{g}\mathbf{x}_{c}\) denote the target position in the C\({}^{g}\) frame and C\({}^{c}\) frame, respectively, and \(\mathbf{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; \(\mathbf{Rot}_{b}^{c}(\alpha,\beta)\) and \(\mathbf{Rot}_{b}^{b}(\theta,\theta,\varphi)\) denote the transformation matrices from \(\mathbf{C}_{c}\) to \(\mathbf{C}_{b}\) and \(\mathbf{C}_{b}\) to \(\mathbf{C}_{g}\), respectively, which can be expressed as:  
\[\left\{\begin{array}{l}\mathbf{Rot}_{b}^{c}(\alpha,\beta)=\mathbf{R}_{y}(\beta)\mathbf{ R}_{z}(\alpha)\\ \mathbf{Rot}_{g}^{b}(\phi,\theta,\varphi)=\mathbf{R}_{x}(\phi)\mathbf{R}_{y}(\theta)\mathbf{R}_ {z}(\varphi)\end{array}\right. \tag{2}\]  
where \(\alpha\),\(\beta\) denote the azimuth and elevation angle, respectively, and \(\phi\),\(\theta\),\(\varphi\) represent the roll, pitch, and yaw angle, respectively; and  
\[\mathbf{R}_{x}(\gamma)=\left(\begin{array}{ccc}1&0&0\\ 0&\cos\gamma&-\sin\gamma\\ 0&\sin\gamma&\cos\gamma\end{array}\right),\mathbf{R}_{y}(\gamma)=\left(\begin{array} []{ccc}\cos\gamma&0&\sin\gamma\\ 0&1&0\\ -\sin\gamma&0&\cos\gamma\end{array}\right),\mathbf{R}_{z}(\gamma)=\left(\begin{array} []{ccc}\cos\gamma&-\sin\gamma&0\\ \sin\gamma&\cos\gamma&0\\ 0&0&1\end{array}\right) \tag{3}\]  
As in (1), both \(\mathbf{x}_{g}\) and \(\mathbf{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.