# Visual-Inertial Method for Localizing Aerial Vehicles in GNSS-Denied Environments
## 2 Materials and Methods
\begin{bmatrix}{\rm X}^{\prime\prime}_{p}\\ {\rm Y}^{\prime\prime}_{p}\\ {\rm Z}^{\prime\prime}_{p}\end{bmatrix} =[T_{\rm O-\rm c^{\prime}}]\times \begin{bmatrix}{\rm X}^{\prime}_{p}\\ {\rm Y}^{\prime}_{p}\\ {\rm Z}^{\prime}_{p}\end{bmatrix}  \tag{5}\]  
Equation (5) gives the position of the image point \(p\) in the relative space with the origin in the camera center (\(p^{\prime\prime}\)). As proposed by [21], let us now consider two world points, G and F, in Figure 2a. Their world coordinates \((X_{G_{w}},Y_{G_{w}},Z_{G_{w}})\) and \(\big{(}X_{F_{W}},Y_{F_{W}},Z_{F_{W}})\) can be obtained considering a Digital Elevation Model (DEM) in a coordinate system like UTM. These points can be used to calculate the horizontal distance between them (indicated with the letter l in Figure 2b) and the vertical distance between them (indicated with the letter d in Figure 1b). In the image plane, points G and F are represented by the points g and f, whose coordinates in the relative coordinate systems (origin in the camera) will be (\(\rm x_{g},Y_{g},z_{g}\)) and  
Figure 2: World points F and G are represented by points f and g in the image plane (**a**). Because the world coordinates of points F and G are known, it is possible to calculate their horizontal (l) and vertical (d) distances (**b**). These distances can then be used to determine the coordinates of F and G in the local coordinate system, with the origin at the camera center. (Source: [21] with modifications). (x\({}_{t}\), y\({}_{t}\), z\({}_{t}\)); therefore, the coordinates of the two points in the relative space (indicated as G\({}_{R}\) and F\({}_{R}\)) are given as follow, where t\({}_{1}\) and t\({}_{2}\) are two scalars:  
\[(X_{G_{R}},Y_{G_{R}},Z_{G_{R}})=\left(t_{1}x_{g},t_{1}y_{g},t_{1}z_{g}\right) \tag{6}\]  
\[\left(X_{F_{R}},Y_{F_{R}},Z_{F_{R}}\right)=\left(t_{2}x_{f},t_{2}y_{f},t_{2}z_{ f}\right) \tag{7}\]  
In particular, we underline that \(Z_{F_{R}}\) is given by the vertical distance between the origin of the camera and the position of point F (see Figure 2b; the same concept applies to \(Z_{G_{R}}\)). This element would be relevant for the next steps. Considering that the horizontal distance (l) and the vertical distance (d) are known, the following equations can be formulated:  
\[t_{1}z_{g}-t_{2}z_{f}-d=0 \tag{8}\]  
\[\sqrt{\left(t_{1}x_{g}-t_{2}x_{f}\right)^{2}+\left(t_{1}y_{g}-t_{2}y_{f} \right)^{2}}-l=0 \tag{9}\]  
This system of equations is solvable, and it admits two couples of variables (t\({}_{1}\) and t\({}_{2}\)). Considering the origin of the relative coordinate system (see Figure 1), one couple will point toward the negative sector, while the other will be oriented toward the opposite positive sector. The couple in the negative sector is the one of relevance for our analysis (the aerial camera, which is the origin of the coordinates, is usually looking down towards the ground). Until now, the described method was taken from [20,21]. The next steps are novel and fully focused on how to retrieve the world coordinates of the camera center O\({}_{W}\). Because t\({}_{1}\) and t\({}_{2}\) are known, they can be used to define the coordinates of the points G\({}_{R}\) and F\({}_{R}\) in the relative coordinate system, which are \(\left(X_{G_{R}},Y_{G_{R}},Z_{G_{R}}\right)\) and \(\left(X_{F_{R}},Y_{F_{R}},Z_{F_{R}}\right)\), respectively. Let us underline that the system composed of the three points O\({}_{R}\), G\({}_{R}\), and F\({}_{R}\) is the same as the system composed of O\({}_{W}\), G\({}_{W}\), and F\({}_{W}\) although localized in a different coordinate system. In other words, \(\overrightarrow{|O_{R}G_{R}|}\) and \(\overrightarrow{|O_{W}G_{W}|}\) are the same and \(\overrightarrow{|O_{R}F_{R}|}\) and \(\overrightarrow{|O_{W}F_{W}|}\) are also the same. All the relative angles between them are the same as well. Taking this into account, let us apply the following transformations. First, the points O\({}_{R}\), G\({}_{R}\), and F\({}_{R}\) need to be converted into the world coordinates system.