# Autonomous Target Tracking of UAV Using High-Speed Visual Feedback
## 5 Visual Servoing Controller Design
### Position Hold Controller Design  
We built our visual servoing controller based on the PX4 flight controller architecture [47]. Figure 9 shows the control diagram for the position hold flight. To achieve position hold control, first the current vehicle states \({}^{O}\hat{x}\) and \({}^{O}\hat{\psi}\) are estimated by a vision-based state estimator. Then, the position controller obtains the estimated relative position directly with a known relative attitude setpoint \({}^{B}q_{sp}\) and thrust setpoint \({}^{B}t_{sp}\).  
Figure 8: Coordinate systems and relative attitude. (**a**) Relationship between each coordinate system. (**b**) Relative yaw angle.  
Figure 7: Control loop of relative-position-based high-speed visual servoing.  
Equations for calculating the attitude setpoint \({}^{B}q_{sp}\) and thrust setpoint \({}^{B}t_{sp}\) are as follows:  
The velocity setpoint \({}^{O}v_{sp}\) is calculated by  
\[{}^{O}v_{sp}=K_{px}\left({}^{O}x_{sp}-{}^{O}\hat{x}\right), \tag{6}\]  
where \(K_{px}\) is the position proportional parameter, and \({}^{O}x_{sp}\) and \({}^{O}\hat{x}\) represent the desired position and the estimated relative position. Then, the thrust setpoint \({}^{O}t_{sp}\) is calculated by  
\[{}^{O}t_{sp}=K_{pv}\Delta\mathbf{v}+K_{dv}\frac{d}{dt}\Delta\mathbf{v}+{}^{O }z_{ini}-{}^{O}z_{sp}, \tag{7}\]  
where the velocity error \(\Delta\mathbf{v}={}^{O}v_{sp}-{}^{O}\hat{v}\), and \(K_{pv}\) and \(K_{dv}\) are velocity P and D parameters in the PD controller, \({}^{O}z_{sp}\) is the desired position in the \(z\) direction, and \({}^{O}z_{ini}\) is the initial position in the \(z\) direction, which is calculated by  
\[{}^{O}z_{ini}={}^{O}z_{ini}+\Delta\mathbf{v}K_{ip}dt, \tag{8}\]  
where \(K_{ip}\) is the velocity integrative parameter. With a known thrust setpoint \({}^{O}t_{sp}\) and yaw angle setpoint \({}^{O}\psi_{sp}\), we have the desired 3-axis unit vectors:  
\[\mathfrak{B}_{z}=-{}^{O}_{tsp}, \tag{9}\]  
\[\mathfrak{B}_{x}=\mathfrak{B}_{z}\times\psi_{xy}, \tag{10}\]  
\[\mathfrak{B}_{y}=\mathfrak{B}_{z}\times\mathfrak{B}_{x}, \tag{11}\]  
where \(\mathfrak{B}_{x}\), \(\mathfrak{B}_{y}\), and \(\mathfrak{B}_{z}\) are the 3-axis unit vectors of the UAV's body frame, \(\psi_{xy}=\left(-\sin\psi_{sp},\cos\psi_{sp},0\right)\) is the vector of the desired yaw direction in the body frame's xy plane, and \({}^{O}_{tsp}\) is the normalized vector of \({}^{O}t_{sp}\). By stacking \(\mathfrak{B}_{x}\), \(\mathfrak{B}_{y}\), \(\mathfrak{B}_{z}\) together we obtain the desired rotation matrix \({}^{B}R_{sp}\):  
\[{}^{B}R_{sp}=\begin{bmatrix}\mathfrak{B}_{x}\\ \mathfrak{B}_{y}\\ \mathfrak{B}_{z}\end{bmatrix}. \tag{12}\]  
Figure 9: Design of direct vision-based position controller.  
By transforming \({}^{B}R_{sp}\) to the quaternion form, we obtain attitude setpoints \({}^{B}q_{sp}\). With a known attitude setpoint \({}^{B}q_{sp}\) and estimated attitude \({}^{B}\hat{q}\) from the EKF state estimator, the angular rates setpoints \(\dot{\phi_{sp}}\), \(\dot{\theta_{sp}}\), and \(\dot{\psi_{sp}}\) are calculated with tuned proportional parameters:  
\[\dot{\phi}_{sp}=K_{p}e_{\phi}, \tag{13}\]  
\[\dot{\theta}_{sp}=K_{p}e_{\theta}, \tag{14}\]  
\[\dot{\psi}_{sp}=K_{p}\psi e_{\phi}, \tag{15}\]  
where \(K_{p}\), \(n\in\phi,\theta,\psi\) are tuned proportional parameters of roll, pitch, and yaw angle, respectively, and \(e_{m}=m_{sp}-\hat{m}\), \(m\in\phi,\theta,\psi\) are the angle errors between the desired attitude and the estimated attitude. In the rate controller, simple PID control is used, and its outputs and \({}^{B}t_{sp}\) will be translated to the motor PWM signals in the mixer module. Figure 10 shows the flowchart of the relative-position-based visual servoing system.