# Neural PD Controller for an Unmanned Aerial Vehicle Trained with Extended Kalman Filter
## 5 Monocular Visual Odometry  
Multirotors are equipped with inertial sensors which are capable of measuring the attitude and orientation of the system. Unfortunately, most of the quadrotors do not have any onboard sensors to measure position in indoor environments; in contrast, position is usually measured by a GPS sensor which has an error between 1 and 5 meters depending on the quality of the GPS signal. This approach is based on Parallel Tracking and Mapping (PTAM) algorithm [60] to solve the localization problem. The algorithm is named this way because tracking and mapping are separated and run in parallel; it creates a keyframe based map using Bundle Adjustment (BA). This map is initialized from a stereo pair, and new points are initialized with epipolar searches. It is well known that the scale of the environment cannot be determined using only monocular vision [52]. Once the map is initialized, the visual map is rotated such that the \(xy\) plane corresponds to the horizontal plane according to the accelerometer, and it is scaled with an average keypoint depth of 1. During the tracking, the scale of the map \(\lambda\in\mathbb{R}\) must be estimated as in [52]. The quadrotor measures in regular intervals, the distance traveled according to the visual SLAM \(\mathbf{x}_{i}\in\mathbb{R}^{d}\), and uses the metric sensors available, denoted by \(\mathbf{y}_{i}\in\mathbb{R}^{d}\). To each interval, a pair \(\left(\mathbf{x}_{i},\mathbf{y}_{i}\right)\) is given, where \(\mathbf{x}_{i}\) is scaled according to the visual map and \(\mathbf{y}_{i}\) is in metric units. Both \(\mathbf{x}_{i}\) and \(\mathbf{y}_{i}\) measure motion of the UAV, and they are related by \(\mathbf{x}_{i}\approx\lambda\mathbf{y}_{i}\). If Gaussian noise in the measurements is assumed, then a set of sample pairs \(\left\{\left(\mathbf{x}_{1},\mathbf{y}_{1}\right)\cdots\left(\mathbf{x}_{n}, \mathbf{y}_{n}\right)\right\}\) is given with  
\[\mathbf{x}_{i} \sim \mathcal{N}(\lambda\mu_{i},\sigma_{\mathbf{x}}^{2}\mathbf{I}_{dxd}) \tag{21}\] \[\mathbf{y}_{i} \sim \mathcal{N}(\mu_{i},\sigma_{\mathbf{y}}^{2}\mathbf{I}_{dxd}) \tag{22}\]where \(\mu_{1}\cdots\mu_{i}\in\mathbb{R}^{2}\) are the unknown distances, \(\sigma_{x}^{2},\sigma_{y}^{2}\in\mathbb{R}^{+}\) are the variances of the measurements error and \(I\) is the identity matrix of dimension \(d\). To estimate the unknown parameters, maximum-likelihood estimation is used, minimizing the negative log-likelihood. \[\mathcal{L}(\mu_{1}\cdots\mu_{n},\lambda)\propto\frac{1}{2}\sum_{i=1}^{n} \left(\frac{\left\|\mathbf{x}_{i}-\lambda\mu_{i}\right\|^{2}}{\sigma_{x}^{2}}+ \frac{\left\|\mathbf{y}_{i}-\mu_{i}\right\|^{2}}{\sigma_{y}^{2}}\right) \tag{23}\]  
The global minimum of (23) is unique; its derivation can be found in [52; 61], and it is stated as follows  
\[\mu_{i}^{*} =\frac{\lambda^{*}\sigma_{y}^{2}\mathbf{x}_{i}+\sigma_{x}^{2} \mathbf{y}_{i}}{\lambda^{*2}\sigma_{y}^{2}+\sigma_{x}^{2}} \tag{24}\] \[\lambda^{*} =\frac{s_{xx}-s_{yy}+sign(s_{xy})\sqrt{(s_{xx}-s_{yy})^{2}+4s_{xy }^{2}}}{2\sigma_{x}^{-1}\sigma_{y}s_{xy}} \tag{25}\]  
with  
\[s_{xx} :=\sigma_{y}^{2}\sum_{i=1}^{n}\mathbf{x}_{i}^{T}\mathbf{x}_{i} \tag{26}\] \[s_{yy} :=\sigma_{x}^{2}\sum_{i=1}^{n}\mathbf{y}_{i}^{T}\mathbf{y}_{i}\] (27) \[s_{xy} :=\sigma_{x}\sigma_{y}\left(\sum_{i=1}^{n}\mathbf{x}_{i}^{T} \mathbf{y}_{i}\right)^{2} \tag{28}\]  
Assuming \(\sigma_{x}^{2}\) and \(\sigma_{y}^{2}\) are known, (25) gives a closed solution for the estimation of \(\lambda\). For the estimation of the measurement variances, the authors refer to [52]. To generate the sample pairs, for each visual altitude measurement \(a_{v}(t_{i})\in\mathbb{R}\) there is a corresponding metric altitude measurement \(a_{m}(t_{i})\in\mathbb{R}\) over a window of sensor measurements, and then a visual and metric distance traveled within a period is computed as follows.