Neural PD Controller for an Unmanned Aerial Vehicle Trained with Extended Kalman Filter

Abstract

Flying robots have gained great interest because of their numerous applications. For this reason, the control of Unmanned Aerial Vehicles (UAVs) is one of the most important challenges in mobile robotics. These kinds of robots are commonly controlled with Proportional-Integral-Derivative (PID) controllers; however, traditional linear controllers have limitations when controlling highly nonlinear and uncertain systems such as UAVs. In this paper, a control scheme for the pose of a quadrotor is presented. The scheme presented has the behavior of a PD controller and it is based on a Multilayer Perceptron trained with an Extended Kalman Filter. The Neural Network is trained online in order to ensure adaptation to changes in the presence of dynamics and uncertainties. The control scheme is tested in real time experiments in order to show its effectiveness.

1. Introduction

Unmanned Aerial Vehicles (UAVs) have become very popular thanks to recent progress in propulsion technologies and small sensors with low power consumption [1]. These kinds of vehicles surpass other types of robotics platforms in both military [2] and civilian applications [3], including surveillance, agriculture, traffic monitoring, fire detection and high social impact activities, such as search and rescue in disaster zones. Figure 1 shows the classification of several types of aerial vehicles based on [4]. This paper is focused on Vertical Take-Off and Landing (VTOL) vehicles, such as multirotors.

VTOL vehicles are more cost-efficient than bigger drones like Medium Altitude Long Endurance (MALE) or High Altitude Long Endurance (HALE) and present the ability to hover above a reference position [5]. In contrast, multirotors have limited energy consumption and payload; consequently, light and compact sensors are required for the navigation, and in most of the cases, inertial sensors are not enough to obtain some states of the system, such as its position. Usually, multirotors are teleoperated by a ground station with a limited operational range, but when autonomous tasks are required, the positional feedback is crucial to control the UAV [5].

Drones are equipped with a Global Positioning System (GPS) to solve the problem of the estimation of the position. However, depending on the accuracy of the assignment, a GPS sensor may not be suitable; besides, the GPS signal is lost when working in indoor environments. For indoor flight control, the positional feedback is commonly carried out by motion capture systems, which each consist of a set of fixed cameras in a room. Unfortunately, approaches like this require previous knowledge of the scene and assembly and calibration of the motion capture system, which, in practice, would not be possible in search and rescue tasks. Generally, a combination of visual and inertial information is used to solve the problem of Simultaneous Localization and Mapping (SLAM) [6]. In this paper, a vision sensor is used; cameras are compact and lightweight sensors with low power consumption. These characteristics make them suitable for drones flying in unknown, GPS-denied environments.

Once the SLAM problem has been solved, it is possible to control the position of the vehicle. Proportional-Integral-Derivative (PID) controllers are widely applied in industry because of their simplicity, and they are usually used to control these kinds of UAVs. Nevertheless, multirotors are highly nonlinear, underactuated systems with six Degrees of Freedom (Dof) and four control inputs—torque in x, y and z, and thrust—and therefore, they are difficult to control with conventional methods [7,8]. To overcome the limitations of conventional controllers, direct control using a neural network is proposed. In this work, a Multilayer Perceptron (MLP) is implemented to adapt the gains of a PD controller. As reported in [9], Artificial Neural Networks (ANN) have shown satisfactory results when controlling nonlinear systems, and despite the limitations of conventional controllers, the approach presented in this paper can cover most of the disadvantages of PID, even if the multirotor changes its parameters during the flight.

The MLP is trained with the Extended Kalman Filter (EKF). The Kalman filter is an optimal estimator which deduces parameters based on noisy measurements. Its solution is recursive; therefore, the filter processes data as soon as it arrives and predicts the next value without the need for having the complete data set of observations [10]. This feature makes it faster and convenient for online applications, in contrast with other methods such as batch processing [11]. The idea behind the use of the EKF to train the ANN is that other training algorithms, such as gradient descent, recursive least squares and backpropagation, are particular cases of the Kalman filter; for this reason, the EKF is suitable for training [12,13]. When using the EKF for training, the weights of the neural network are the states that the filter estimates, and the output of the neural network is the measurement used by the Kalman filter. Then, the training of the ANN can be seen as an optimal filtering problem. The EKF has been successfully applied to estimate parameters of an ANN in [14,15,16].

Although several PID tuning algorithms have been proposed, those approaches are mostly offline applications [17,18] or simulation-only experiments [19]. The main contribution of this work is the control of both position and orientation of a quadrotor in real-time experimental tests using direct control; i.e., the output of the neural network is the control action for the UAV. The neural networks are trained online in order to adapt to changes in the dynamics and uncertainties. The objective is to find a robust solution to real applications [20] in which UAVs are capable of grabbing or deliver objects. This approach also uses a solution for the SLAM problem, to control the position using only onboard sensors, making it able to fly in unknown environments.

The remainder of this paper is organized as follows: in Section 2, some previous and related works are presented. The dynamic model of the platform used is described in Section 3. Section 4 presents the architecture of the neural network trained with EKF. In Section 5, the algorithm of localization is described. The quadrotor control scheme is shown in Section 6. Finally, simulation and experimental results are shown in Section 7. The conclusions of this work are discussed in Section 8.

2. Related work

Multirotors are commonly controlled by PID [21]. PID controllers have been widely used in industry, mainly because of the trade-off between efficiency and simplicity [22], and numerous offline tuning techniques have been reported in the literature [19]. The main problem with conventional approaches is that physical systems present parametric changes and uncertainties, and they are perturbed by external disturbances; consequently, an online, continuous tuning approach is needed.

Another common approach is the Linear Quadratic Regulator (LQR) for attitude stabilization [23,24,25]. While this may be applicable for some configurations, multirotors are non-linear systems that present uncertainties, such as unknown physical parameters, actuator degradation, unknown delays in-process communication and unmodeled dynamics [26]. Hence, an approach considering these characteristics must be used [27,28]. Nonlinear control techniques provide better performance [29], and one of the most applied methods is feedback linearization, which relies on cancellation of all nonlinearities to convert the non-linear system into a system with linear dynamics [30,31,32]. In [33,34,35], the authors use a backstepping control, which is designed to stabilize the whole system based on the Lyapunov stability theory, showing good results. However, backstepping requires the analytic calculation of the partial derivatives of the stabilizing functions, which becomes impractical as the order of the system grows [36], and in general, most of the nonlinear techniques require complete knowledge of the nonlinearities present in the plant and are vulnerable to modeling errors or parametric uncertainty [29]. In this paper, an adaptive controller based on an Artificial Neural Network (ANN) is proposed. The ANN has been used to control complex nonlinear systems [9,37,38,39]. The controller used in this scheme has the same simple structure and easy implementation of a PD controller but with the adaptability and learning capabilities of a neural network [9]. The system will be able to adapt to actuator faults, such as loss of effectiveness [40] and solve the principal disadvantages of traditional PID [41].

On the other hand, there is no onboard sensor to read the absolute position of the UAV. There are two common solutions to solve this; the first one consists of an external motion detection system, which has to be mounted, thereby limiting the applications to known indoor environments.

The second approach is based on solving the SLAM problem. For this, different sensors can be used, such as laser range scanners [42], stereo vision systems [43,44], RGB-Depth sensors [45,46,47] and monocular cameras [48,49,50]. In this work, a monocular vision system is preferred because of its lower power consumption and compact size, compared to the amount of information it delivers. The advantage is that the range of a camera is virtually unlimited [6], making it possible flying in both small and large environments. Despite the advantages of monocular vision, it is impossible to determine the scale of the environment using only one view, and it is necessary to fuse this information with inertial information provided by an Inertial Measurement Unit (IMU). To solve this, Parallel Tracking and Mapping (PTAM) for robot localization (introduced in [6,51,52]) will be implemented.

3. Multirotor Dynamic Model

The multirotor used for this work was the quadcopter. There are two principal configurations for quadcopters; in this case, the configuration selected is described in Figure 2.

The robotic platform where the algorithm was tested is the Parrot AR.Drone 2.0 ^® quadrotor. Multirotor systems have an even number of rotors divided into two groups rotating in opposite directions. The configuration of the selected platform is depicted in Figure 2. For this specific configuration, where the robot structure does not match with the x and y axis from the body frame (Figure 3), the movement of the vehicle is given by the following combination of rotor actions: increasing or decreasing the speed of the four rotors in the same proportion changes the altitude of the system. Then, because the multirotor is an underactuated system, there is no actuator that generates movement in x and y directions directly; instead, these displacements are achieved by changes in the attitude due to combinations of the two pairs of propellers: the rotation in y axis (pitch $\theta$) results in translational movement in x; contrarily, a rotation in x (roll $\varphi$) results in translational movement in y. Similarly, the orientation (yaw $\psi$) needs a combination of the four propellers and it is the result of the difference of the counter-torque between both pairs.

In general, the center of mass is considered to be at the origin of the body fixed frame. The quadrotor orientation in space is given by a rotation matrix $\mathbf{R}\in SO\left(3\right)$ from the quadrotor $\mathbf{B}$ to the inertial frame $\mathbf{E}$. As in [4], the dynamics of the quadrotor may be expressed as follows

$$\left[\begin{array}{cc}m{\mathbf{I}}_{3\times 3}& 0\\ 0& \mathbf{I}\end{array}\right]\left[\begin{array}{c}\dot{\mathbf{V}}\\ \dot{\omega }\end{array}\right]+\left[\begin{array}{c}\omega \times m\mathbf{V}\\ \omega \times \mathbf{I}\omega \end{array}\right]=\left[\begin{array}{c}\mathbf{F}\\ \tau \end{array}\right]$$

(1)

where $\mathbf{I}$ is the inertia matrix, $\mathbf{V}$ is the body linear speed vector and $\omega$ is the angular speed.

The quadrotor equations of motion can be expressed as

$$\begin{array}{c}\dot{\zeta }=\mathbf{v}\hfill \\ \dot{\mathbf{v}}=-g{e}_3+{\mathbf{R}}_{e_3}\left(\frac{b}{m}\sum {\mathsf{\Omega }}_i^2\right)\hfill \\ \dot{\mathbf{R}}=\mathbf{R}\widehat{\omega }\hfill \\ \mathbf{I}\dot{\omega }=-\omega \times \mathbf{I}\omega -\sum {\mathbf{J}}_r\left(\omega \times e_3\right){\mathsf{\Omega }}_i+{\tau }_a\hfill \end{array}$$

(2)

where the gravity g is acting on z axis ($e_3$), $\dot{\zeta }$ is the linear velocity vector, the rotation matrix is represented by $\mathbf{R}$, $\widehat{\omega }$ is the skew symmetric matrix of the vector of angular velocity and ${\mathsf{\Omega }}_i$ represents the speed of rotor i. $\mathbf{I}$ and ${\mathbf{J}}_r$ are the body and the rotor inertia respectively. m is the mass of the system, b is the thrust factor, l is the distance from the center of mass to the rotor (it is assumed the same distance to all rotors) and ${\tau }_a$ represents the torque applied to the quadrotor. For this configuration, ${\tau }_a$ is denoted as follows

$${\tau }_a=\left(\begin{array}{c}\frac{\sqrt{2}}{2}lb\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)\\ \frac{\sqrt{2}}{2}lb\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_4^2\right)\\ d\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_4^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2\right)\end{array}\right)$$

(3)

with a drag factor d.

The quadrotor dynamics, as described in [4], are given by

$$\begin{array}{c}\ddot{x}=\left(cos\left(\varphi \right)sin\left(\theta \right)sin\left(\psi \right)+sin\left(\varphi \right)sin\left(\psi \right)\right)\frac{U_1}{m}\hfill \\ \ddot{y}=\left(cos\left(\varphi \right)sin\left(\theta \right)sin\left(\psi \right)-sin\left(\varphi \right)sin\left(\psi \right)\right)\frac{U_1}{m}\hfill \\ \ddot{z}=-g+\left(cos\left(\varphi \right)cos\left(\theta \right)\right)\frac{U_1}{m}\hfill \\ \ddot{\varphi }=\dot{\theta }\dot{\psi }\left(\frac{I_y-I_z}{I_x}\right)-\frac{J_r}{I_x}\dot{\theta }\mathsf{\Omega }+\frac{l}{I_x}{U}_2\hfill \\ \ddot{\theta }=\dot{\varphi }\dot{\psi }\left(\frac{I_z-I_x}{I_y}\right)+\frac{J_r}{I_y}\dot{\varphi }\mathsf{\Omega }+\frac{l}{I_y}{U}_3\hfill \\ \ddot{\psi }=\dot{\varphi }\dot{\theta }\left(\frac{I_x-I_y}{I_z}\right)+\frac{U_4}{I_z}\hfill \end{array}$$

(4)

where $\mathsf{\Omega }$ represents the gyroscopic effects induced by the propellers, which are usually neglected [53], and they are given by

$$\mathsf{\Omega }={\mathsf{\Omega }}_2+{\mathsf{\Omega }}_4-{\mathsf{\Omega }}_1-{\mathsf{\Omega }}_3$$

(5)

$U_i$ are the system inputs: $U_1$ is related to its translation and $U_2$ to $U_4$ are related to its attitude and orientation. The relation between both subsystems can be seen in Figure 4. The inputs $U_i$ for this specific configuration of multirotor are given by

$$\begin{array}{c}{U}_1=b\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2+{\mathsf{\Omega }}_3^2+{\mathsf{\Omega }}_4^2\right)\hfill \\ U_2=\frac{\sqrt{2}}{2}b\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_4^2\right)\hfill \\ U_3=\frac{\sqrt{2}}{2}b\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_3^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_4^2\right)\hfill \\ U_4=b\left({\mathsf{\Omega }}_1^2+{\mathsf{\Omega }}_4^2-{\mathsf{\Omega }}_2^2-{\mathsf{\Omega }}_3^2\right)\hfill \end{array}$$

(6)

and they are calculated by the neural network. In this work, a Multilayer Perceptron trained with the Extended Kalman Filter is selected.

4. MLP trained with the EKF

The architecture of an ANN was inspired by biological neurons. Like synaptic connection in a regular biological neuron, each node in an ANN receives a signal from an adjacent node and its output is computed by some nonlinear function of the sum of the inputs. These connections between adjacent neurons have weights that adjusts as the network learns [19].

The training process of the MLP can be seen as an estimation problem for a nonlinear system that can be solved with the EKF [54,55,56]. Neural networks trained with the EKF have demonstrated faster learning speeds and convergence times than networks trained with algorithms based on backpropagation, which is ideal for real time applications [57,58]. The objective is to find the optimal weights that minimize the prediction error [59].

Consider an MLP with L weights and m output nodes. The neural network can be modeled as follows

$$w(k+1)=w\left(k\right)$$

(7)

$$\widehat{y}\left(k\right)=h\left(w\left(k\right),u\left(k\right)\right)$$

(8)

where $w\left(k\right)$ is the state vector, $u\left(k\right)$ is the input vector, $\widehat{y}$ is the output vector and h is the nonlinear output function. From Kalman Filter equations, it is known that

$$\mathbf{K}\left(k\right)=\mathbf{P}\left(k\right){\mathbf{H}}^T\left(k\right){\left[\mathbf{R}\left(k\right)+\mathbf{H}\left(k\right)\mathbf{P}\left(k\right){\mathbf{H}}^T\left(k\right)\right]}^{-1}$$

(9)

$$\mathit{w}\left(k+1\right)=\mathit{w}\left(k\right)+\eta \mathbf{K}\left(k\right)\left[\mathit{y}\left(k\right)-\widehat{\mathit{y}}\left(k\right)\right]$$

(10)

$$\mathbf{P}\left(k\right)=\mathbf{P}\left(k\right)-\mathbf{K}\left(k\right)\mathbf{H}\left(k\right)\mathbf{P}\left(k\right)+\mathbf{Q}\left(k\right)$$

(11)

where L is the total number of weights, $\eta$ is the learning rate, $\mathbf{P}\left(k\right)\in {\Re }^{L\times L}$ and $\mathbf{P}(k+1)\in {\Re }^{L\times L}$ are the prediction error covariance matrices in k and $k+1$. $\mathbf{K}\left(k\right)\in {\Re }^{L\times m}$ is the Kalman gain matrix. $\mathit{y}\in {\Re }^m$ is the system output, and $\widehat{\mathit{y}}$ is the network output. $\mathbf{Q}\in {\Re }^{L\times L}$ is the process noise covariance matrix, and $\mathbf{R}\in {\Re }^{m\times m}$ represents the measurement covariance error. $\mathit{w}\in {\Re }^L$ is the weights (state) vector, and $\mathbf{H}\in {\Re }^{m\times L}$ contains the partial derivatives of each output of the neural network $\widehat{\mathit{y}}$ with respect to each weight $w_j$ and it is defined as

$${\mathbf{H}}_{ij}\left(k\right)={\left[\frac{\partial {\widehat{y}}_i\left(k\right)}{\partial w_j\left(k\right)}\right]}_{w\left(k\right)=\widehat{w}\left(k+1\right)},i=1\dots m,j=1\dots L$$

(12)

Consider the MLP in Figure 5 with one hidden layer and one node at the output layer, where p denotes the number of inputs to the network and h the number of nodes in the hidden layer. The output of the neural network is defined by

$$\begin{array}{ccc}\hfill {\sigma }_i& =& \frac{1}{1+e^{-n_i}}i=1\dots h\hfill \end{array}$$

(13)

$$\begin{array}{ccc}\hfill n_i& =& \sum _{j=0}^p{w}_{ij}^{\left(1\right)}{x}_j{x}_0=+1\hfill \end{array}$$

(14)

$$\begin{array}{ccc}\hfill v_1& =& \sum _{k=0}^h{w}_{1k}^{\left(2\right)}{u}_k{u}_0=+1\hfill \end{array}$$

(15)

$$\begin{array}{ccc}\hfill \widehat{y}& =& v_1\hfill \end{array}$$

(16)

Finally, vector $\mathbf{H}$ can be expressed as

$$\mathbf{H}=\frac{\partial \widehat{y}}{\partial w}=\left[\begin{array}{cccc}\frac{\partial \widehat{y}}{\partial w_{10}^{\left(1\right)}}& \frac{\partial \widehat{y}}{\partial w_{11}^{\left(1\right)}}& \dots & \frac{\partial \widehat{y}}{\partial w_{1h}^{\left(2\right)}}\end{array}\right]$$

(17)

where

$$\begin{array}{cccc}\frac{\partial \widehat{y}}{\partial w_{10}^{\left(1\right)}}=\frac{w_{11}^{\left(2\right)}{e}^{-n_1}}{{\left(1-e^{-n_1}\right)}^2}{x}_0;& \frac{\partial \widehat{y}}{\partial w_{11}^{\left(1\right)}}=\frac{w_{11}^{\left(2\right)}{e}^{-n_1}}{{\left(1-e^{-n_1}\right)}^2}{x}_1;& \dots & \frac{\partial \widehat{y}}{\partial w_{1p}^{\left(1\right)}}=\frac{w_{11}^{\left(2\right)}{e}^{-n_1}}{{\left(1-e^{-n_1}\right)}^2}{x}_p;\\ \frac{\partial \widehat{y}}{\partial w_{20}^{\left(1\right)}}=\frac{w_{12}^{\left(2\right)}{e}^{-n_2}}{{\left(1-e^{-n_2}\right)}^2}{x}_0;& \frac{\partial \widehat{y}}{\partial w_{21}^{\left(1\right)}}=\frac{w_{12}^{\left(2\right)}{e}^{-n_2}}{{\left(1-e^{-n_2}\right)}^2}{x}_1;& \dots & \frac{\partial \widehat{y}}{\partial w_{2p}^{\left(1\right)}}=\frac{w_{12}^{\left(2\right)}{e}^{-n_2}}{{\left(1-e^{-n_2}\right)}^2}{x}_p;\\ ⋮& ⋮& ⋮& ⋮\\ \frac{\partial \widehat{y}}{\partial w_{h0}^{\left(1\right)}}=\frac{w_{1h}^{\left(2\right)}{e}^{-n_h}}{{\left(1-e^{-n_h}\right)}^2}{x}_0;& \frac{\partial \widehat{y}}{\partial w_{h1}^{\left(1\right)}}=\frac{w_{1h}^{\left(2\right)}{e}^{-n_h}}{{\left(1-e^{-n_h}\right)}^2}{x}_1;& \dots & \frac{\partial \widehat{y}}{\partial w_{hp}^{\left(1\right)}}=\frac{w_{1h}^{\left(2\right)}{e}^{-n_h}}{{\left(1-e^{-n_h}\right)}^2}{x}_p;\\ \frac{\partial \widehat{y}}{\partial w_{10}^{\left(2\right)}}=1;& \frac{\partial \widehat{y}}{\partial w_{11}^{\left(2\right)}}=\frac{1}{1+e^{-n_1}};& \dots & \frac{\partial \widehat{y}}{\partial w_{1h}^{\left(2\right)}}=\frac{1}{1+e^{-n_h}};\end{array}$$

(18)

Let us define a new variable $\gamma$ as follows

$$\gamma \left(n_i\right)=\frac{w_{1i}^{\left(2\right)}{e}^{-n_i}}{{\left(1+e^{-n_i}\right)}^2},i=1\dots h;$$

(19)

then, the vector H for the MLP shown in Figure 5 with sigmoid activation functions for the hidden layers and linear function for the output node can be expressed as follows

$$\mathbf{H}=\left[\begin{array}{cccccccccc}\gamma \left(n_1\right)x_0& \dots & \gamma \left(n_1\right)x_p& \gamma \left(n_2\right)x_0& \dots & \gamma \left(n_h\right)x_p& u_0& u_1& \dots & u_h\end{array}\right]$$

(20)

The network designed for this work has two inputs which are the error and the derivative of the error between the desired pose and the pose of the system which is calculated using monocular visual odometry.

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 $({\mathbf{x}}_i,{\mathbf{y}}_i)$ 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\{({\mathbf{x}}_1,{\mathbf{y}}_1)\cdots ({\mathbf{x}}_n,{\mathbf{y}}_n)\right\}$ is given with

$$\begin{array}{ccc}\hfill {\mathbf{x}}_i& \sim & \mathcal{N}(\lambda {\mu }_i,{\sigma }_x^2{\mathbf{I}}_{dxd})\hfill \end{array}$$

(21)

$$\begin{array}{ccc}\hfill {\mathbf{y}}_i& \sim & \mathcal{N}({\mu }_i,{\sigma }_y^2{\mathbf{I}}_{dxd})\hfill \end{array}$$

(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{{∥{\mathbf{x}}_i-\lambda {\mu }_i∥}^2}{{\sigma }_x^2}+\frac{{∥{\mathbf{y}}_i-{\mu }_i∥}^2}{{\sigma }_y^2}\right)$$

(23)

The global minimum of (23) is unique; its derivation can be found in [52,61], and it is stated as follows

$$\begin{array}{cc}\hfill {\mu }_i^{*}& =\frac{{\lambda }^{*}{\sigma }_y^2{\mathbf{x}}_i+{\sigma }_x^2{\mathbf{y}}_i}{{\lambda }^{*2}{\sigma }_y^2+{\sigma }_x^2}\hfill \end{array}$$

(24)

$$\begin{array}{cc}\hfill {\lambda }^{*}& =\frac{s_{xx}-s_{yy}+sign\left(s_{xy}\right)\sqrt{{(s_{xx}-s_{yy})}^2+4s_{xy}^2}}{2{\sigma }_x^{-1}{\sigma }_y{s}_{xy}}\hfill \end{array}$$

(25)

with

$$\begin{array}{cc}\hfill s_{xx}& :={\sigma }_y^2\sum _{i=1}^n{\mathbf{x}}_i^T{\mathbf{x}}_i\hfill \end{array}$$

(26)

$$\begin{array}{cc}\hfill s_{yy}& :={\sigma }_x^2\sum _{i=1}^n{\mathbf{y}}_i^T{\mathbf{y}}_i\hfill \end{array}$$

(27)

$$\begin{array}{cc}\hfill s_{xy}& :={\sigma }_x{\sigma }_y{\left(\sum _{i=1}^n{\mathbf{x}}_i^T{\mathbf{y}}_i\right)}^2\hfill \end{array}$$

(28)

Assuming ${\sigma }_x^2$ and ${\sigma }_y^2$ are known, (24) 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\left(t_i\right)\in \mathbb{R}$ there is a corresponding metric altitude measurement $a_m\left(t_i\right)\in \mathbb{R}$ over a window of sensor measurements, and then a visual and metric distance traveled within a period is computed as follows.

$$\begin{array}{c}\hfill {\mathbf{x}}_i:=a_v\left(t_i\right)-a_v\left(t_{i-k}\right)\end{array}$$

(29)

$$\begin{array}{c}\hfill {\mathbf{y}}_i:=a_m\left(t_i\right)-a_m\left(t_{i-k}\right)\end{array}$$

(30)

For visual odometry, EKF is used to identify and reject falsely tracked frames and compensate for time delays [6]. The filter includes the motion model of the quadrotor; the state space of the EKF is given by

$${\mathbf{x}}_t:={(x_t,y_t,z_t,{\dot{x}}_t,{\dot{y}}_t,{\dot{z}}_t,{\mathsf{\Phi }}_t,{\mathsf{\Theta }}_t,{\mathsf{\Psi }}_t,{\dot{\mathsf{\Psi }}}_t)}^T$$

(31)

where $(x,y,z)$ is the position of the quadrotor; $(\dot{x},\dot{y},\dot{z})$ its velocity; $(\mathsf{\Phi },\mathsf{\Theta },\mathsf{\Psi })$ are the roll, pitch and yaw angle respectively (in degrees); and $\dot{\mathsf{\Psi }}$ is the yaw rotational speed. For each sensor, an observation function is defined as below

$$h\left({\mathbf{x}}_i\right):={({\dot{x}}_{t}cos{\mathsf{\Psi }}_t-\dot{y}sin{\mathsf{\Psi }}_t,{\dot{x}}_{t}sin{\mathsf{\Psi }}_t+\dot{y}cos{\mathsf{\Psi }}_t,\dot{z},{\mathsf{\Phi }}_t,{\mathsf{\Theta }}_t,{\dot{\mathsf{\Psi }}}_t)}^T$$

(32)

and the respective observation vector, derived from sensor measurements is defined as shown

$${\mathbf{z}}_t:={\left({\widehat{v}}_{x,t},{\widehat{v}}_{y,t},\frac{{\widehat{h}}_t-{\widehat{h}}_{t-1}}{{\delta }_{t-1}},{\widehat{\mathsf{\Phi }}}_t,{\widehat{\mathsf{\Theta }}}_t,\frac{{\widehat{\mathsf{\Psi }}}_t-{\widehat{\mathsf{\Psi }}}_{t-1}}{{\delta }_{t-1}}\right)}^T$$

(33)

where ${\widehat{v}}_x,t$ and ${\widehat{v}}_y,t$ are velocities in $xy$ plane directly measured by the quadrotor. The velocity in z direction can be computed since the altitude ${\widehat{h}}_t$ is given by an ultrasonic or an air pressure sensor and ${\delta }_t$ is the time from time step t and $t+1$. Equally, for rotation readings, roll $\mathsf{\Phi }$ and pitch $\mathsf{\Theta }$ are directly measured and the yaw rotation speed can be calculated since $\mathsf{\Psi }$ is given by the IMU.

If PTAM tracks a video frame, the pose estimation is scaled with $\lambda$ and transformed to the coordinate system of the quadrotor, leading to direct observation of its pose as follows.

$$\begin{array}{cc}\hfill h_p\left({\mathrm{x}}_t\right)& :={\left(x_t,y_t,z_t,{\mathsf{\Phi }}_t,{\mathsf{\Theta }}_t,{\mathsf{\Psi }}_t\right)}^T\hfill \end{array}$$

(34)

$$\begin{array}{cc}\hfill {\mathrm{z}}_p& :=f\left(E_{DC}{E}_{C,t}\right)\hfill \end{array}$$

(35)

where $E_{C,t}$ is the estimated camera pose scaled with $\lambda$, $E_{DC}$ is the rigid transformation from the camera to the quadrotor coordinate system and f is the function that maps from $SE\left(3\right)$ to roll-pitch-yaw representation.

The Kalman filter predicts how the state vector ${\mathrm{x}}_{\mathrm{t}}$ evolves to the next time step. The horizontal acceleration is proportional to the horizontal force and is given by

$$\left(\frac{\ddot{x}}{\ddot{y}}\right)\propto F_{acc}-F_{drag}$$

(36)

where $F_{acc}$ is the drag force and $F_{acc}$ denotes the accelerating force which is proportional to the projection of the z axis of the quadrotor onto the horizontal plane, which leads to

$$\begin{array}{c}\hfill \ddot{x}\left({\mathrm{x}}_t\right)=c_1\left(cos{\mathsf{\Psi }}_{t}sin{\mathsf{\Phi }}_{t}cos{\mathsf{\Theta }}_t-sin{\mathsf{\Psi }}_{t}sin{\mathsf{\Theta }}_t\right)-c_2{\dot{x}}_t\end{array}$$

(37)

$$\begin{array}{c}\hfill \ddot{y}\left({\mathrm{x}}_t\right)=c_1\left(-sin{\mathsf{\Psi }}_{t}sin{\mathsf{\Phi }}_{t}cos{\mathsf{\Theta }}_t-cos{\mathsf{\Psi }}_{t}sin{\mathsf{\Theta }}_t\right)-c_2{\dot{y}}_t\end{array}$$

(38)

where coefficients $c_1$ and $c_2$ are estimated from data collected from test flights. The influence of the control inputs $u_t=\left(\overline{\mathsf{\Phi }},\overline{\mathsf{\Theta }},\overline{\dot{z}},\overline{\dot{\mathsf{\Psi }}}\right)$ is described by

$$\begin{array}{cc}\hfill \dot{\mathsf{\Phi }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)& =c_3{\overline{\mathsf{\Phi }}}_t-c_4{\mathsf{\Phi }}_t\hfill \end{array}$$

(39)

$$\begin{array}{cc}\hfill \dot{\mathsf{\Theta }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)& =c_3{\overline{\mathsf{\Theta }}}_t-c_4{\mathsf{\Theta }}_t\hfill \end{array}$$

(40)

$$\begin{array}{cc}\hfill \ddot{\mathsf{\Psi }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)& =c_5{\overline{\dot{\mathsf{\Psi }}}}_t-c_6{\dot{\mathsf{\Psi }}}_t\hfill \end{array}$$

(41)

$$\begin{array}{cc}\hfill \ddot{z}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)& =c_7{\overline{\dot{z}}}_t-c_8{\dot{z}}_t\hfill \end{array}$$

(42)

where coefficients $(c_3,\cdots ,c_8)$ are estimated from flight tests. The overall state transition is given by

$$\left(\begin{array}{c}{x}_{t+1}\\ y_{t+1}\\ z_{t+1}\\ {\dot{x}}_{t+1}\\ {\dot{y}}_{t+1}\\ {\dot{z}}_{t+1}\\ {\mathsf{\Phi }}_{t+1}\\ {\mathsf{\Theta }}_{t+1}\\ {\mathsf{\Psi }}_{t+1}\\ {\dot{\mathsf{\Psi }}}_{t+1}\end{array}\right)\leftarrow \left(\begin{array}{c}{x}_t\\ y_t\\ z_t\\ {\dot{x}}_t\\ {\dot{y}}_t\\ {\dot{z}}_t\\ {\mathsf{\Phi }}_t\\ {\mathsf{\Theta }}_t\\ {\mathsf{\Psi }}_t\\ {\dot{\mathsf{\Psi }}}_t\end{array}\right)+{\delta }_t\left(\begin{array}{c}{\dot{x}}_t\\ {\dot{y}}_t\\ {\dot{z}}_t\\ \ddot{x}\left({\mathrm{x}}_t\right)\\ \ddot{y}\left({\mathrm{x}}_t\right)\\ \ddot{z}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)\\ \dot{\mathsf{\Phi }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)\\ \dot{\mathsf{\Theta }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)\\ {\dot{\mathsf{\Psi }}}_t\\ \ddot{\mathsf{\Psi }}\left({\mathrm{x}}_t,{\mathrm{u}}_t\right)\end{array}\right)$$

(43)

For a complete derivation of the state transition and delay compensation for a quickly reacting system to avoid oscillations and unstable behavior, the authors refer to [52].

6. Quadrotor Control Scheme

In this section, the control scheme is described. An MLP is used with an architecture as shown in Figure 5. The ANN has two inputs: the error between the desired value and the output measured from the system, and the derivative of this error to get information about the rate of change of the process output. PTAM estimates the positional observations (x, y, z), and the yaw angle $\psi$ can be directly measured by the IMU.

The network has one hidden layer with four nodes and one neuron at the output layer, and there are four MLP modules, one for each controllable Degree of Freedom (Figure 6). The weights are randomly selected and uniformly distributed between $[-1,1]$. The outputs of the four MLPs represent the control inputs $U_i$ from (6). Despite all Degrees of Freedom being internally coupled, the advantage of using the MLP modules separately (one for every DoF) is an easier implementation and interpretation of the control scheme behavior.

7. Results

The platform selected to test the algorithm was the Parrot Ar.Drone 2.0, which is a quadrotor equipped with a 3-axis gyroscope, 3-axis accelerometer, a magnetic compass, an ultrasound altimeter, one camera looking in x direction and another camera looking downward. For these experiments, the camera looking forward was used; it has a field of view of ${92}^{\circ }$ and a resolution of $640\times 360$. Its video is streamed at 30 fps to the ground station. Sensor measurements of the Ar.Drone 2.0 can be sent to the ground station at a frequency of 200 Hz; however, due to integration of the ANN training, control, localization and mapping, this transmission rate is decreased to 30 Hz in the following experiments.

7.1. Simulation Results

Simulation experiments were carried out in Linux Ubuntu with Gazebo, which is an open-source simulator with easy integration with a Robot Operating System (ROS). The world scene used for the test and the camera view are shown in Figure 7.

In the first experiment, a trajectory in the $xy$ plane is selected to train weights for both directions at the same time. For each degree, all the weights of an MLP module with ten nodes in the hidden layer are selected. As can be seen in Figure 8, the overshoot is reduced and eventually eliminated after some iterations.

After some iterations of x and y training, the z axis is added and a new test is performed training the three Degrees of Freedom. Again, the z axis MLP module is initialized with random weights. The results of these tests are shown in Figure 9. In Figure 10 the PTAM output can be seen. It can be seen that the overshoot in z is reduced after some iterations, and eventually, the path is followed correctly.

Table 1. Root Mean Squared Error (RMSE) for each axis from simulation from Figure 10.

${\mathit{RMSE}}_{\mathit{x}}$	${\mathit{RMSE}}_{\mathit{y}}$	${\mathit{RMSE}}_{\mathit{z}}$
0.1919	0.1989	0.2855

shows the Root Mean Squared Error (RMSE) for each axis.

7.2. Experimental Results

The experimental tests were carried out in indoor unknown environment; however, since the localization of the system is estimated using only onboard sensors, it can also navigate in outdoor conditions. It is preferable that the scene includes sufficient objects (features) to be seen by the camera in a range of 0–7 m. The weights were initialized with the last value of the simulation. Since the Ar.Drone 2.0 driver for ROS was used, the parameters should be close to the real system. The data were received at 30 Hz (the time interval of each iteration was 1/30 s). Figure 11 shows the camera view and mapping output of the experiment. In Figure 12 are the results of controlling the quadrotor using the MLP using online training. As shown, oscillations and overshot were eliminated.

For the yaw angle $\psi$, in contrast with the other three modules, the weights were initialized randomly. The fourth MLP module was trained online to control the yaw angle, and the results are shown in Figure 13. As can be seen, the neural network adapts its weights to follow the reference.

For a second experiment, a mass is added to the system during the flight in order to test its adaptability; the mass has a value of 45 g, which represents, approximately, 1/3 of the maximum payload of the Ar.Drone 2.0. The results of this experiment are shown in Figure 14. As can be seen, at iteration 750 approximately, the mass is added on the x axis of the quadrotor. After some iterations, the oscillations are eliminated.

8. Conclusions

In this paper, a direct control approach for a quadrotor was presented. The controller was based in a Multilayer Perceptron trained with the Extended Kalman Filter. Each MLP module consists of two inputs, ten nodes in the hidden layer and one output, which is the control action. Each controllable degree of freedom required an MLP module that was trained online to adapt its control law to uncertainties, loss of rotor efficiency, time delays and changes in the dynamic model of the system. The results were first validated via simulation using the Ar.Drone 2.0 ROS driver with Gazebo simulator. The neural network was initialized with random weights for x, y and z axes, and once the overshoot and oscillations were reduced, the weights were used to initialize the MLP modules in the real system. In the simulation, every degree of freedom was separately trained. For experimental tests, x, y, and z were initialized with the weights calculated in simulation, and all of them continued their training online. As can be seen in Figure 12, even using the simulation results, the neural networks must continue the training, since they present oscillations around the reference location. After some iteration steps, the oscillations were eliminated. For the experimental tests on the yaw angle $\left(\psi \right)$, the weights were initialized randomly; that is, without previous training in a simulation. Figure 13 can be seen as the erratic behavior at the beginning of the experiment, and it is shown that the MLP adapt its weights to control the yaw of the quadrotor. Finally, in Figure 14, it has been shown that the controller adapts its weights when changing the dynamics during the flight.