# Pose Measurement for Unmanned Aerial Vehicle Based on Rigid Skeleton
## 3 Research Methodology
### Solving Stage
According to the point set \(\{p_{edge_{t}}\}\) fit the space straight line \(L_{edge_{t}}\) on the edge of the left wing:  
\[\frac{x-x_{edge_{t}}}{A_{edge_{t}}}=\frac{y-y_{edge_{t}}}{B_{edge_{t}}}=\frac{ z-z_{edge_{t}}}{C_{edge_{t}}} \tag{9}\]  
Similar to the above solution process, the corresponding depth map \(I_{d_{r}}\) is calculated for the right wing area \(I_{depth_{r}}\), and the right wing edge space point set \(\{p_{edge_{r}}\}\) is obtained. The fitting is the edge space line of the right wing \(L_{edge_{t}}\):  
\[\frac{x-x_{edge_{t}}}{A_{edge_{t}}}=\frac{y-y_{edge_{t}}}{B_{edge_{t}}}=\frac{ z-z_{edge_{t}}}{C_{edge_{t}}} \tag{10}\]  
According to the linear equations of \(L_{edge_{t}}\) and \(L_{edge_{t}}\), the angles \(\varphi_{edge_{t}}\) and \(\varphi_{edge_{t}}\), between their projection lines in \(O_{p}x_{p}y_{p}\) of the aircraft coordinate system (i.e., \(Oxz\) plane of the camera coordinate system) and the axis of \(x\) can be solved. Given the symmetrical properties of the wings, the initial state angle can be defined as \(\varphi_{0}\), and the yaw angle of the aircraft is \(\varphi\). The corresponding relationship is shown in Figure 6:  
Because the angles between the two linear space vectors and the positive direction of the \(x\) axis are acute, the formula for calculating the yaw angle \(\varphi\) of the aircraft is as follows:  
\[\varphi=\left\{ \begin{array}{ll}\frac{\varphi_{edge_{t}}+\varphi_{edge_{t}} }{2}&\text{if }C_{edge_{t}}\cdot C_{edge_{t}}\geqslant 0\\ \frac{\varphi_{edge_{t}}-\varphi_{edge_{t}}}{2}&\text{if }C_{edge_{t}}\cdot C_{edge_{t}}<0 \end{array} \right. \tag{11}\]  
The aircraft pitch angle \(\theta\) is solved by the intersection of the two wing planes. Although the disparity map \(I_{d}\) can be divided into the wing area and the fuselage area, errors may be encountered during the process of fitting the plane because a large number of noise points in the 3D point cloud are calculated in the stereo matching algorithm. Moreover,  
Figure 5: Edge point set projection. Figure 6: Relationship between yaw and straight line projection. because the amount of point cloud data is large, the calculation efficiency of the method may be negatively affected. Therefore, this paper uses the spatial point set formed by the feature points in the wing area to fit the plane and calculate the intersection of the wing plane. Here, the proposed method use the disparity map \(I_{d}\) and its area distinction to filter the matching feature points, constrain the matching of the feature points through the disparity (i.e., depth information) corresponding to the feature points, eliminate the incorrectly matched points on the same horizontal line, as shown in Figure 7. The feature used in the image is ORB feature [29]. The feature points extracted from the corresponding left image through the left wing area \(I_{d_{l}}\) in the disparity map are divided. The feature point \(p^{l}_{fea_{l_{l}}}=(u^{l}_{fea_{l_{l}}},v^{l}_{fea_{l_{l}}})\) in the left wing area and the feature point in the right image that matches this feature point \(p^{r}_{fea_{l_{l}}}=(u^{r}_{fea_{l_{l}}},v^{r}_{fea_{l_{l}}})\) are extracted, where \(u_{fea_{l_{l}}}\) and \(v_{fea_{l_{l}}}\) denotes the column and row coordinates of the feature point \(p_{fea_{l_{l}}}\) respectively. For comparison, the proposed method can filter out the feature point set on the left wing \(\{p_{fea_{l}}\}\). The filter formula is as follows:  
\[f(x)=\left\{ \begin{array}{ll}p^{l}_{fea_{l_{l}}}\in\{p_{fea_{l_{l}}}\}& \text{if }|u^{r}_{fea_{l}}-u^{l}_{fea_{l_{l}}}|<\delta\&\&\&\&p^{l}_{fea_{l}}\in I_{d_ {l}}\\ p^{r}_{fea_{l_{l}}}\
otin\{p_{fea_{l}}\}&\text{else}\end{array} \right.