# Pose Measurement for Unmanned Aerial Vehicle Based on Rigid Skeleton
## 3 Research Methodology
### Solving Stage
\tag{12}\]  
The 3D coordinates of each point in the feature set \(\{p_{fea_{l}}\}\) are calculated, and the plane equation of the left wing is fitted as follows:  
\[A_{wing}x+B_{wing}y+C_{wing_{l}}=z \tag{13}\]  
In the same way, the feature point set on the right wing \(\{p_{fea_{l}}\}\) can be extracted through the above process, and the right wing plane equation can be fitted as shown below:  
\[A_{wing}x+B_{wing}y+C_{wing},=z \tag{14}\]  
The normal vectors \(\overrightarrow{M}_{wing_{l}}\) and \(\overrightarrow{M}_{wing_{l}}\) of the plane where the two wings are located are obtained, and the cross product are used to calculate the direction vector \(\overrightarrow{M}_{l_{3}}\) of the plane intersection \(l_{3}\). \[\overrightarrow{M}_{l_{3}}=\overrightarrow{M}_{wing_{l}}\times\overrightarrow {M}_{wing_{l}}. \tag{15}\]  
The angle between the projection of the direction vector \(\overrightarrow{M}_{l_{3}}\) of the plane intersection line \(l_{3}\) in \(O_{p}y_{p}z_{p}\) plane of the aircraft coordinate system (i.e., \(Oyz\) plane of the camera coordinate system) and the \(y\) axis of the aircraft coordinate system (i.e., the \(z\) axis of the camera coordinate system) is the pitch angle \(\theta\) of the aircraft. According to the conventional aircraft shape, the position of the aircraft can be spatially located through the tail of the aircraft. The point with the smallest depth information is found in the fuselage area \(I_{d_{c}}\) in the disparity map. The difference between the disparity of the point in the adjacent area is used to eliminate the noise points, and the position \(\mathbf{p}_{tail}(x_{tail},y_{tail},z_{tail})\) of the tail of the aircraft is obtained. The proposed method is presented in Algorithm 1. After calculating the aircraft's space pose, its position information will be adopted to update the confidence of the tracking stage. Figure 7: Feature points after filtering. ```
Input: Frame Sequence Output: Location \(\mathbf{p}_{tail}\), Pitch \(\theta\), Roll \(\gamma\) and Yaw \(\varphi\) whilenot end of sequencedo  read current image pair \(I_{I},I_{r}\) ; if\(I_{I},I_{r}\) is initializedthen \(\{x_{ld},c_{ld}\}=\text{detect}(\text{ROI}_{ld})\), \(\{x_{rd},c_{rd}\}=\text{detect}(\text{ROI}_{rd})\); \(\{x_{ld_{r}},c_{lo}\}=\text{trackupdate}(I_{r})\), \(\{x_{ro},c_{ro}\}=\text{trackupdate}(I_{r})\); if\(c_{id}>c_{jo}(j=l,r)\)then \(\{x_{j},c_{j}\}=\left\{x_{jd},c_{id}\right\}\); else  vice versa;  end if else \(x_{l}=\text{Inidet}(I_{l})\), \(x_{r}=\text{Inidet}(I_{r})\); \(\text{trackInit}(I_{l},x_{l})\), \(\text{trackInit}(I_{r},x_{r})\); \(\text{ROI}_{l}=\text{CalROI}(x_{l})\), \(\text{ROI}_{r}=\text{CalROI}(x_{r})\) ;  end if if\(x_{l}\&\&x_{r}\
eq 0\)then \(I_{depth}=\text{disparity}(x_{l},x_{r})\) ; \(\left[I_{d_{l}},I_{d_{r}},I_{d_{r}}\right]=\text{Divide}\left(I_{depth}\right)\); \(\{p_{edge_{l}}\}=\text{CalP}\Big{(}I_{edge_{l}},I_{d_{l}},\text{SelDP}\big{(} I_{d_{l}}\big{)}\Big{)}\), \(\{p_{edge_{r}}\}=\text{CalP}\Big{(}I_{edge_{r}},I_{d_{r}},\text{SelDP}(I_{d_{r}} )\Big{)}\); \(\varphi_{edge_{l}}=\text{CalinterAngel}\Big{(}\text{FitLine}\{p_{edge_{l}} \}\Big{)}\), \(\varphi_{edge_{r}}=\text{CalinterAngel}\Big{(}\text{FitLine}\{p_{edge_{r}} \}\Big{)}\) ; \(\varphi=\text{CalAngel}\Big{(}\varphi_{edge_{l}},\varphi_{edge_{r}}\Big{)}\);  Similarly, it can be obtained: \(\gamma=\text{CalAngel}\Big{(}\gamma_{edge_{l}},\gamma_{edge_{r}}\Big{)}\); if\(num\{p_{fea_{l}}\}>5||num\{p_{fea_{l}}\}>5\)then \(\theta=\text{CalinterLineAngle}\Big{(}\text{RANSAC}\{p_{fea_{l}}\},\text{ RANSAC}\{p_{fea_{r}}\}\Big{)}\); else \(\theta=0\) ;  end if \(p_{tail}=\text{CalMaxP}\big{(}I_{d_{c}},\{p_{tN}|p_{i}\in Aera_{p_{tail}}(r=2) \}\big{)}\); else \(\{p_{tail},\theta,\gamma,\varphi\}=\{(0,0,0),0,0,0\}\) ;  go back to initialization ;  end if  end if  end if
```  
**Algorithm 1**Measuring Position and Orientation of Aircraft.