Enhanced Attitude and Altitude Estimation for Indoor Autonomous UAVs

Abstract

In recent years the use of Unmanned Aerial Vehicles (UAVs) has considerably grown in the civil sectors, due to their high flexibility of use. Currently, two important key points are making them more and more successful in the civil field, namely the decrease of production costs and the increase in navigation accuracy. In this paper, we propose a Kalman filtering-based sensor fusion algorithm, using a low cost navigation platform that contains an inertial measurement unit (IMU), five ultrasonic ranging sensors and an optical flow camera. The aim is to improve navigation in indoor or GPS-denied environments. A multi-rate version of the Extended Kalman Filter is considered to deal with the use of heterogeneous sensors with different sampling rates, and the presence of non-linearities in the model. The effectiveness of the proposed sensor platform is evaluated by means of numerical tests on the dynamic flight simulator of a quadrotor. Results show high precision and robustness of the attitude estimation algorithm, with a reduced computational cost, being ready to be implemented on low-cost platforms.

1. Introduction

In the last two decades, the use of small and micro-UAVs has considerably spread, with the increased level of autonomy and the development of low cost electronics devices, i.e., microcontrollers, sensors, etc. [1]. In particular, the possibility of multi-rotors (quadcopter, hexacopeter, etc.) to take off and land vertically, to move in any direction and hover over a fixed position gives them employment for reconnaissance missions in hostile and hazardous environment [2,3], where other aircraft and robots cannot be used. However, their use requires the solution of different technological problems, first of all the need of a robust and reliable attitude estimator, possibly executable on low-cost computational hardware and using only measurements from light-weight sensors.

Nowadays the most widely used platforms for UAV navigation are based on the Global Positioning System (GPS) [4] and the Inertial Navigation System (INS) [5] including an IMU with magnetic, angular rate, and gravity sensors (MARG). However, GPS is able to provide data about position by receiving information from a network of several satellites, resulting unusable in indoor environment, due to the loss of signal.

MARG sensors consists of Micro Electro-Mechanical Systems (MEMS) based on gyroscopes, accelerometers and magnetometers which can provide the orientation of a rigid body with respect to a fixed reference system. Unlike GPS sensors, they can provide attitude information in indoor environment too, since they do not use signal from external sources. Moreover, the growing success of these sensors in many applications [6], is due to their low-weight, low-cost, low power consumption and relatively high performance in real-time [7].

Being an essential task, the problem of attitude estimation on the basis of sensor measurements can be non-trivial [8,9].

Most of the recent sensor fusion algorithms provide orientation estimation in quaternion form [10,11]. Quaternions are a useful mathematical tool that does not result in singularity configurations as the Euler representation.

Over the years, several solutions have been developed to solve the problem of attitude estimation and different sensor data fusion algorithms have been implemented and tested on-board [12]. The most famous techniques make use of complementary filters [13,14,15,16,17], thanks to their low computational burden. In [13], a constant gain filter is considered to estimate the attitude of a rigid body. Such method ensures good attitude estimation at low computational cost, but it has a drawback related to the local magnetic disturbances. In [14], a nonlinear explicit complementary filter is proposed, improved with an anti-windup nonlinear integrator that allows an effective gyro-bias compensation. To increase robustness to magnetic disturbances, a decoupling strategy was implemented to make roll and pitch estimates not dependent on magnetometer. In [17], a complementary filter provides a quaternion estimation like the algebraic solution of a system from inertial/magnetic observations. Tilting and heading estimation are separately obtained to avoid the impact of magnetic disturbances on roll and pitch.

However, despite the effectiveness and the computational performances of such techniques, more effective solutions have been based on Kalman Filtering (KF) with the increase of computational power in low-cost electronic boards. Such techniques are widely used in human motion analysis [18,19,20,21,22], robotics [23,24] and in aerospace applications [25,26]. Most UAV attitude estimation algorithms are based on the Extended Kalman Filter (EKF) [27,28,29] and the Unscented Kalman Filter (UKF) [30,31].

At the cost of a further increase in computational burden, in literature several examples based on Particle Filtering (PF) [9,32,33,34] exist.

Since attitude estimation depends on sensors accuracy and can suffer from the presence of fast dynamics, the integration of INS, based on a sensor fusion algorithm and an IMU, with other kinds of sensors can be useful. Speed and position can be also obtained implementing an integration of IMU and GPS sensors [35] which has been used in the field of autonomous vehicle only recently [36]. Thanks to the complementary characteristics of GPS and IMU sensors, in [37] the author developed an integrated and advanced GPS/IMU system to estimate the attitude and position of a rigid body in 3-D space. Attitude estimation is obtained comparing the acceleration vector, implicitly available in the derivative of the GNSS velocity, with measurements provided by accelerometers. In [38], authors present a nonlinear observer for estimating position, velocity, attitude, and gyro bias by combining a GNSS receiver with an INS. The presented navigation equations take into account the Earth rotation and curvature, to improve accuracy. In [39], authors exploited the GPS Precise Point Positioning (PPP) on a fixed-wing UAV for photogrammetric mapping with centimeter-level precision on the horizontal plane.

Such solutions are not effective in indoor scenarios or in absence of GPS signal and the only MARG-based dead reckoning results in poor position estimates. To improve attitude and position estimation, several solutions based on vision and sound propagation techniques have been proposed over the years.

In low-cost and commercial drones, one of the most popular solutions makes use of the visual odometry. Optical flow is a naturally inspired technique, introduced in 1950s by the american psychologist Gibson [40] to describe the animal visual perception of the world. It can be considered as the projection of the 3D world apparent motion of objects caused by the relative movements between an observer and the scene.

The concept of optical flow has been widely used in many fields and different types of algorithms have been developed over the years. The Lucas-Kanade algorithm [41] and the Horn-Schunck method [42] are based on the gradient descent approach. In [43], a method for object recognition is presented based on the image features, while in [44] the author used the interpolation technique for the computation of optical flow information and ego-motion.

Starting from the experience of pioneers in this discipline, many researchers have involved optical flow algorithms in conjunction with inertial platforms for navigation and obstacle avoidance purposes in the UAV field [45,46,47,48,49].

From the hardware point of view, several kinds of optical sensors were involved on small and micro-UAVs. Optical mouse based sensors were firstly used thanks to their very low cost, but the need of a strong lighting limited their success. Being more sensitive, CMOS image sensors can alleviate the problem, allowing operations in indoor environment without using an artificial lighting as in [50].

Another solution to improve indoor navigation is the Simultaneous Localization and Mapping (SLAM) [51,52,53,54,55,56,57,58,59]. However, although SLAM techniques are extremely useful in unknown environments, their cost in terms of weight and computational burden is usually high, making it difficult to be implemented on micro and mini UAV.

In this paper, we propose a novel technique to compute accurate attitude and altitude estimation, by fusing data from an IMU, a downward oriented optical flow and five downward oriented distance sensors based on ultrasonic or time of flight (TOF) devices. Being placed in different points, distance sensors can improve attitude estimation accuracy in addition to UAV altitude. One of the scopes of this work is to show the benefits of the presence of more distance sensors in attitude estimation as well as in velocity and altitude measurement. Furthermore, the presence of a downward oriented optical flow gives the ability to estimate the velocity vector in the horizontal plane as well as to correct attitude in fast maneuvers.

Since the involved sensors run at different sampling rates, the proposed algorithm makes use of a multi-rate EKF [60,61,62]. A campaign of numerical simulations is carried out to prove the effectiveness of the idea, comparing results obtained with several sensor fusion schemes. Two configurations are available: one involving a single TOF sensor and one with 5 TOF devices. Each configuration is presented in two possible architectures: a tightly coupled and a loosely coupled scheme.

The paper is organized as follows: Section 2 defines the UAV attitude and altitude estimation problems; Section 3 presents the multi-rate EKF approach and, finally, Section 4 describes the proposed testing platform and it shows the numerical results obtained by integrating the system on a quadrotor dynamic simulator.

2. UAV Attitude and Altitude Estimation Problem

To define the UAV position and orientation, two reference frames are considered:

• North-East-Down (NED) Reference Frame, located on the earth surface, with:

  −

  $X_E$-axis points north, parallel to the earth surface;

  −

  $Y_E$-axis points east, parallel to the earth surface;

  −

  $Z_E$-axis points downward, toward the earth surface.

• Body Reference Frame, centred in the Center of Gravity ($CG$) of the quadrotor, with:

  −

  $X_B$-axis points along the arm 1–3, positive forward;

  −

  $Y_B$-axis points along the arm 2–4, positive rightward;

  −

  $Z_B$-axis points downward, to form a right-handed reference frame.

as shown in Figure 1.

Vehicle attitude is defined as its orientation with respect to a reference frame and its accurate estimation is an important topic in the robotics and aerospace fields.

At the initial time, NED and body reference frames can be overlapped at the UAV departing point.

After that, the transformation from NED to body frame can be obtained as a sequence of three ordered rotations (Figure 2), called $3-2-1$ [63]:

• a rotation around the $Z_E$ axis by the yaw angle $\psi$, from $O{X}_E{Y}_E{Z}_E$ to $O{X}^{\prime }{Y}^{\prime }{Z}^{\prime }$;
• a rotation around the $Y^{\prime }$ axis by the pitch angle $\theta$ from $O{X}^{\prime }{Y}^{\prime }{Z}^{\prime }$ to $O{X}^{″}{Y}^{″}{Z}^{″}$;
• a rotation around the $X^{″}$ axis by the roll angle $\varphi$ from $O{X}^{″}{Y}^{″}{Z}^{″}$ to $O{X}_B{Y}_B{Z}_B$.

Euler angles $\psi$, $\theta$ and $\varphi$ describe the aircraft attitude. An alternative representation makes use of the quaternion definition [17,64], considering $\mathit{q}={[q_0,q_1,q_2,q_3]}^T$ as the unit quaternion vector associated to ${\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\right)$ from the NED reference frame to the body frame:

$${\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\right)=\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_0^2& 2(q_1{q}_2-q_3{q}_0)& 2(q_1{q}_3+q_2{q}_0)\\ 2(q_1{q}_2+q_3{q}_0)& -q_1^2+q_2^2-q_3^2-q_0^2& 2(q_2{q}_3-q_1{q}_0)\\ 2(q_1{q}_3-q_2{q}_0)& 2(q_2{q}_3+q_1{q}_0)& -q_1^2-q_2^2+q_3^2+q_0^2\end{array}\right]$$

(1)

Gyroscope model. If gyroscope was not affected by noise and uncertainties, the attitude could be estimated by integrating the angular velocity $\mathit{\omega }={[p,q,r]}^T$ from a known initial condition. In fact, quaternion vector shows the following dynamics:

$$\dot{\mathit{q}}\left(t\right)=\frac{1}{2}\mathit{Q}\left(\mathit{\omega }\left(t\right)\right)·\mathit{q}\left(t\right)$$

(2)

where the matrix $\mathit{Q}\left(\mathit{\omega }\right(t\left)\right)$ is

$$\mathit{Q}\left(\mathit{\omega }\left(t\right)\right)=\left[\begin{array}{cccc}0& -p\left(t\right)& -q\left(t\right)& -r\left(t\right)\\ p\left(t\right)& 0& r\left(t\right)& -q\left(t\right)\\ q\left(t\right)& -r\left(t\right)& 0& p\left(t\right)\\ r\left(t\right)& q\left(t\right)& -p\left(t\right)& 0\end{array}\right]$$

(3)

However, the gyroscope output is affected by noise with zero mean and given standard deviation. In steady state conditions, a straightforward integration would lead to a non-zero final angle due to the so called Angular Random Walk (ARW) [65,66].

To compensate this effect, a gyro drift model is introduced, whose sensed angular speed vector ${\mathit{\omega }}_S\left(t\right)$ differs from $\mathit{\omega }\left(t\right)$ due to a bias ${\mathit{b}}_g\left(t\right)={[b_p\left(t\right),b_q\left(t\right),b_r\left(t\right)]}^T$ and a stochastic zero-mean noise ${\mathbf{\upsilon }}_{\omega }\left(t\right)={[{\upsilon }_p\left(t\right),{\upsilon }_q\left(t\right),{\upsilon }_r\left(t\right)]}^T$:

$${\mathit{\omega }}_S\left(t\right)=\left[\begin{array}{c}{p}_s\left(t\right)\\ q_s\left(t\right)\\ r_s\left(t\right)\end{array}\right]=\mathit{\omega }\left(t\right)+{\mathit{b}}_g\left(t\right)+{\mathbf{\upsilon }}_{\omega }\left(t\right)$$

(4)

Therefore, by computing $\mathit{\omega }\left(t\right)$ from (4), the equation describing attitude dynamics becomes

$$\dot{\mathit{q}}\left(t\right)=\frac{1}{2}\mathit{Q}\left({\mathit{\omega }}_S\left(t\right)-{\mathit{b}}_g\left(t\right)-{\mathbf{\upsilon }}_{\omega }\left(t\right)\right)·\mathit{q}\left(t\right)$$

(5)

Usually, in slowly varying conditions, the accelerometer and the magnetometer can be used to estimate the gyroscope biases and compensate the attitude drift, being their measurements dependent from the UAV orientation [67,68,69]. The gyroscope bias is commonly modeled as a first-order Gauss-Markov process [70,71], with a first-order stochastic differential equation given by

$$\dot{{\mathit{b}}_g}\left(t\right)=-\frac{1}{{\tau }_g}{\mathit{b}}_g\left(t\right)+{\mathbf{\upsilon }}_b$$

(6)

where ${\tau }_g$ is the correlation time of the process and ${\mathbf{\upsilon }}_b$ is the associated white Gaussian noise [72,73,74,75,76].

Accelerometer model. A tri-axial accelerometer measures the forces, per unit mass, acting on the body along three specific orthogonal directions. The inertial acceleration depends on these forces and the gravity acceleration $\mathit{g}={\left[\begin{array}{ccc}0& 0& g\end{array}\right]}^T$. The specific force in the body reference frame is:

$${\mathit{F}}^{\mathit{B}}\left(t\right)=-{\mathit{a}}^{\mathit{B}}\left(t\right)+{\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·\mathit{g}$$

(7)

The acceleration in the inertial reference frame ${\mathit{a}}^E\left(t\right)$ depends on ${\mathit{a}}_t^B\left(t\right)$ as follows

$${\mathit{a}}^E\left(t\right)={\mathit{a}}_t^B\left(t\right)+\mathit{\omega }\left(t\right)\times \mathit{\omega }\left(t\right)\times {\mathit{L}}^B+\dot{\mathit{\omega }}\left(t\right)\times {\mathit{L}}^B$$

(8)

where ${\mathit{L}}^B$ is the distance vector between the center of gravity and the accelerometer.

The acceleration in the body reference frame can be then written as follows:

$${\mathit{a}}^B\left(t\right)={\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·{\mathit{a}}^E\left(t\right)={\mathit{R}}_{\mathit{BE}}\left(t\right)\left(\mathit{q}\left(t\right))·({\mathit{a}}_t^B\left(t\right)+\mathit{\omega }\left(t\right)\times \mathit{\omega }\left(t\right)\times {\mathit{L}}^B+\dot{\mathit{\omega }}\left(t\right)\times {\mathit{L}}^B\right)$$

(9)

In the absence of noise and uncertainties, the velocity in the inertial frame could be obtained by integrating (8). However, to compensate the drifting integration of the acceleration, a bias ${\mathit{b}}_{\mathit{a}}\left(t\right)={[b_{ax}\left(t\right),b_{ay}\left(t\right),b_{az}\left(t\right)]}^T$ and a stochastic zero-mean noise ${\mathbf{\nu }}_{\mathit{a}}\left(t\right)={[{\nu }_{ax}\left(t\right),{\nu }_{ay}\left(t\right),{\nu }_{az}\left(t\right)]}^T$ are considered:

$${\mathit{a}}_S^B\left(t\right)=\left[\begin{array}{c}{a}_{x_S}^B\left(t\right)\\ a_{y_S}^B\left(t\right)\\ a_{z_S}^B\left(t\right)\end{array}\right]=-{\mathit{a}}^B\left(t\right)+{\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·\mathit{g}+{\mathit{b}}_{\mathit{a}}\left(t\right)+{\mathbf{\nu }}_{\mathit{a}}\left(t\right)={\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·\mathit{g}+{\dot{\mathit{a}}\prime }^B\left(t\right)+{\mathbf{\nu }}_{\mathit{a}}\left(t\right)$$

(10)

In case of slowly varying maneuvers, vector ${\mathit{a}\prime }^B\left(t\right)=-{\mathit{a}}^B\left(t\right)+{\mathit{b}}_a\left(t\right)$ can be estimated using a first-order dynamics:

$${\dot{\mathit{a}}\prime }^B\left(t\right)=-\frac{1}{{\tau }_a}{\mathit{a}\prime }^B\left(t\right)$$

(11)

where ${\tau }_a$ is a time constant related to the UAV dynamics.

The velocity in the inertial frame can be then obtained by integrating ${\mathit{a}}_b^{\prime }$

$${\dot{\mathit{V}}}^E\left(t\right)={\mathit{R}}_{\mathit{BE}}^{\mathbf{T}}\left(\mathit{q}\left(t\right)\right)·{\mathit{a}\prime }^B\left(t\right)$$

(12)

Altitude $z^E$ is related to $V^E$ as follows:

$${\dot{z}}^E\left(t\right)=[0,0,1]·{\mathit{V}}^E\left(t\right)$$

(13)

Magnetometer model. Magnetometer measures the local magnetic field in a given direction, which is a combination of both the Earth magnetic field and the magnetic field due to the presence of ferro-magnetic material [77]. The horizontal component of the local Earth magnetic field points towards the Earth magnetic north pole, while the ratio between the horizontal and vertical components depends on the location on the Earth.

Assuming that the sensor does not travel over significant distances and the local magnetic sources are weak and not moving, such that a preliminary calibration can estimate the constant magnetic field they induce, the local magnetic field ${\mathit{M}}^B$ in the body reference frame can be expressed as follows:

$${\mathit{M}}^B\left(t\right)={\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·{\mathit{M}}^E$$

(14)

where ${\mathit{M}}^E$ is the Earth’s magnetic field. However, at the time instant t, magnetometer measurements ${\mathit{M}}_S\left(t\right)$ are affected by sensor noise ${\nu }_M$.

$${\mathit{M}}_S\left(t\right)=\left[\begin{array}{c}{M}_{x,S}\left(t\right)\\ M_{y,S}\left(t\right)\\ M_{z,S}\left(t\right)\end{array}\right]={\mathit{R}}_{\mathit{BE}}\left(\mathit{q}\left(t\right)\right)·{\mathit{M}}^E+{\mathbf{\nu }}_M\left(t\right)$$

(15)

Here, the presence of additional fields due to ferro-magnetic material in an indoor environment was neglected, considering the problem independently handled in literature [78,79,80,81,82].

Optical Flow model. Optical flow is the distribution of apparent velocities about the movement of brightness patterns in an image [42]. It can be due to the relative motion between objects and the viewer [40]. According to the pinhole image modelling approach, that defines a relationship between the 3D motion field and the 2D image camera plane, the sensed optical flow [46,50] is:

$${\mathit{V}}_S\left(t\right)=\left[\begin{array}{c}{u}_S\left(t\right)\\ v_S\left(t\right)\end{array}\right]=\left[\begin{array}{c}\frac{-u\left(t\right)·\beta }{z^E\left(t\right)}-q\left(t\right)·\beta +r\left(t\right)d_y\left(t\right)+{\mathbf{\nu }}_{O_x}\left(t\right)\\ \frac{-v\left(t\right)·\beta }{z^E\left(t\right)}+p\left(t\right)·\beta -r\left(t\right)d_x\left(t\right)+{\mathbf{\nu }}_{O_y}\left(t\right)\end{array}\right]$$

(16)

where $u\left(t\right)$ and $v\left(t\right)$ are the components of the velocity vector in the body frame at the time instant t, $\beta$ is the focal length, $z^E\left(t\right)$ is the altitude of the UAV, $d_x\left(t\right)$ and $d_y\left(t\right)$ are the components on the horizontal plane of the distance vector between the center of gravity of the UAV and the camera, ${\nu }_{O_x}\left(t\right)$ and ${\nu }_{O_y}\left(t\right)$ represent the sensor noise on x and y axes.

Time-of-flight sensor model. The primary function of a time-of-flight sensor is to measure the distance between the transmitter and the objects in its field of view. The principle is based on the speed of the light: the distance is evaluated by measuring the time it takes for the transmitted signal to travel to and from the object.

The use of several devices allows to measure the distance to ground from several points on the vehicle, resulting useful to improve attitude estimation. Consider $n_{TOF}$ TOF devices mounted on the UAV structure and pointed towards the positive $z_B$ axis direction. Let ${\mathbf{\xi }}_i^B={\left[x_i^B,y_i^B,z_i^B\right]}^T$, $\forall i=1,...,n_{TOF}$ be the position of the i-th device in the body reference frame. During the flight, the position of the sensor ${\mathbf{\xi }}_i^E\left(t\right)=\left[x_i^E\left(t\right),y_i^E\left(t\right),z_i^E\left(t\right)\right]$ in the inertial reference frame is

$${\mathbf{\xi }}_i^E\left(t\right)={\mathit{R}}_{BE}^T\left(\mathit{q}\left(t\right)\right)·{\mathbf{\xi }}_i^B$$

(17)

The vector of the sensed distance measurements is

$$\begin{array}{c}{\mathit{D}}_S\left(t\right)={[d_{S,1}\left(t\right),...,d_{S,n_{TOF}\left(t\right)}]}^T\\ \mathrm{where}{d}_{S,i}\left(t\right)=\frac{[0,0,1]{\mathbf{\xi }}_i^E\left(t\right)}{cos\varphi \left(t\right)cos\theta \left(t\right)}+{\nu }_{TO{F}_i}\left(t\right)\forall i=1,...,n_{TOF}\end{array}$$

(18)

with ${\nu }_{TO{F}_i}\left(t\right)$ is the sensor noise on the i-th device (see Figure 3).

Overall mathematical model. Equations (5), (6), (11)–(13) can be expressed in the state-space form:

$$\dot{\mathit{x}}\left(t\right)=\gamma (\mathit{x}\left(t\right),\mathit{u}\left(t\right),\mathit{\upsilon }\left(t\right))$$

(19)

where $\mathit{x}\left(t\right)={\left[\mathit{q}{\left(t\right)}^T,{\mathit{b}}_g{\left(t\right)}^T,{\mathit{V}}_E{\left(t\right)}^T,{\mathit{a}}_B{\left(t\right)}^T,z_E\left(t\right)\right]}^T$ is the state vector, $\mathit{u}\left(t\right)={\mathit{\omega }}_S\left(t\right)$ is the input vector, and $\mathbf{\upsilon }={[{\mathbf{\upsilon }}_{\mathit{\omega }}^T,{\mathbf{\upsilon }}_b^T]}^T$ is the process noise. Similarly, Equations (10), (15), (16) and (18) can be written in the following form:

$$\mathit{y}\left(t\right)=\mathit{g}(\mathit{x}\left(t\right),\mathit{v}\left(t\right))$$

(20)

where $\mathit{y}\left(t\right)={\left[{\mathit{a}}_S{\left(t\right)}^T,{\mathit{M}}_S{\left(t\right)}^T,{\mathit{V}}_S^T\left(t\right),{\mathit{D}}_S^T\left(t\right)\right]}^T$ is the output vector and $\mathbf{\nu }\left(t\right)=\left[{\mathbf{\nu }}_a{\left(t\right)}^T,{\mathbf{\nu }}_M{\left(t\right)}^T\right.,{\left.{\mathbf{\nu }}_{TOF}{\left(t\right)}^T\right]}^T$ is the measurement noise vector.

3. Multi-Rate Extended Kalman Filtering Approach

In order to fuse raw data and obtain a reliable estimation of attitude and vertical position with respect to the ground, a sensors fusion algorithm must be adopted. Since sensors work at different asynchronous sampling rates, a multi-rate version of the EKF, hereinafter referred to as MR-EKF, is needed.

Let us consider the nonlinear discrete dynamic model of our system:

$${\mathit{x}}_k=\mathit{f}({\mathit{x}}_{k-1},{\mathit{u}}_{k-1},{\mathit{\upsilon }}_{k-1})$$

(21)

$${\mathit{y}}_k=\mathit{g}({\mathit{x}}_k,{\mathit{u}}_k,{\mathit{\nu }}_k)$$

(22)

where $\mathit{x}$ is the state, $\mathit{u}$ is the input, $\mathit{y}$ is the measured output, and $\mathit{\upsilon }$ and $\mathit{\nu }$ are the process and measurement noises, respectively. This discrete time system can be obtained by applying Euler’s discretization to (19) and (20) where ${\mathbf{\zeta }}_k=\mathbf{\zeta }\left(t_k\right)$ and $t_k=k{T}_S$ is the k-th sampling time instant.

Assume a zero-mean Gaussian distribution for the noises ${\mathbf{\upsilon }}_{k-1}$ and ${\mathbf{\nu }}_k$, with known covariance matrices ${\mathit{Q}}_{k-1}$ and ${\mathit{R}}_k$:

$${\mathbf{\upsilon }}_{k-1}\sim (\mathbf{0},{\mathit{Q}}_{k-1}){\mathbf{\nu }}_k\sim (\mathbf{0},{\mathit{R}}_k)$$

(23)

The estimation algorithm is composed of two distinct phases, namely prediction and correction phase.

At each time step, the prediction phase provides the a-priori state estimation ${\widehat{\mathit{x}}}_k^{-}$ and the state covariance matrix ${\mathit{P}}_k^{-}=E(({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{-})}^T)$, whereas the update phase computes the a-posteriori state estimation ${\widehat{\mathit{x}}}_k^{+}$ and the state covariance matrix ${\mathit{P}}_k^{+}=E(({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{+}){({\mathit{x}}_k-{\widehat{\mathit{x}}}_k^{+})}^T)$.

According to the KF approach, at each time instant k the current state can be estimated by using the previous a-posteriori estimation ${\widehat{\mathit{x}}}_{k-1}^{+}$ and the last acquired set of measures ${\mathit{y}}_{m,k}$ depending on the frequency rate of each sensor.

Therefore, the a-priori estimation of the state ${\widehat{\mathit{x}}}_k^{-}$, is computed by using the Equation (21) with null process noise ($\mathbf{\upsilon }=\mathbf{0}$):

$${\widehat{\mathit{x}}}_k^{-}=f({\widehat{\mathit{x}}}_{k-1}^{+},{\mathit{u}}_{k-1},\mathbf{0})$$

(24)

The a-priori state covariance matrix ${\mathit{P}}_k^{-}$ is computed as follows:

$${\mathit{P}}_k^{-}={\mathit{A}}_{k-1}{\mathit{P}}_{k-1}^{+}{\mathit{A}}_{k-1}^T+{\mathbf{Y}}_{k-1}{\mathit{Q}}_{k-1}{\mathbf{Y}}_{k-1}^T$$

(25)

where

$${\mathit{A}}_{k-1}={\left(\frac{\partial \mathit{f}}{\partial \mathit{x}}\right)}_{{\widehat{\mathit{x}}}_{k-1}^{+},{\mathit{u}}_{k-1},\mathbf{0}}{\mathbf{Y}}_{k-1}={\left(\frac{\partial \mathit{f}}{\partial \mathbf{\upsilon }}\right)}_{{\widehat{\mathit{x}}}_{k-1}^{+},{\mathit{u}}_{k-1},\mathbf{0}}$$

are the Jacobian matrices of function $\mathit{f}(·)$, and the ${\mathit{P}}_{k-1}^{+}$ is the a-posteriori state covariance matrix at the time instant $k-1$.

Consider n sensors that provide measurements with different sampling rates

$${\rho }_1\ge {\rho }_2\ge ...\ge {\rho }_{n}i=1,...,n.$$

(26)

To perform the update process of the a-priori state estimation, assume the minimum sample time $T_S=\frac{1}{{\rho }_1}$, to obtain the discrete time model (21), (22).

The update process must be executed taking into account the available measurements at the time k. Measurements available at k can be denoted as a vector ${\tilde{\mathit{y}}}_{m_k,k}$ with a variable size.

To take into account missing measurements at some discrete time, in the update phase, only a partial measurement model can be adopted.

Let’s define ${\Delta }_k$ a diagonal matrix with ${\Delta }_k^{jj}=1$ if the $j-th$ output is available, ${\Delta }_k^{jj}=0$ otherwise.

The partial estimated output is obtained by using the output Equation (22) and neglecting the measurements noise.

$${{\widehat{\mathit{y}}}^{\prime }}_k={\Delta }_k\mathit{g}\left({\widehat{\mathit{x}}}_k^{-},{\mathit{u}}_k,\mathbf{0}\right).$$

(27)

Once a new measure has been acquired, the a-posteriori state estimation can be computed updating the a-priori state estimation with a feedback term that depends only on the active measurements:

$${\widehat{\mathit{x}}}_k^{+}={\widehat{\mathit{x}}}_k^{-}+{\tilde{\mathit{L}}}_k\left({{\mathit{y}}^{\prime }}_{m,k}-{{\widehat{\mathit{y}}}^{\prime }}_k\right)$$

(28)

where the gain ${\tilde{\mathit{L}}}_k$ is given by the following equation:

$${\tilde{\mathit{L}}}_k={\mathit{P}}_k^{-}{\tilde{\mathit{C}}}_k^T{\left({\tilde{\mathit{C}}}_k{\mathit{P}}_k^{-}{\tilde{\mathit{C}}}_k^T+{\tilde{\mathit{N}}}_k{\tilde{\mathit{R}}}_k{\tilde{\mathit{N}}}_k^T\right)}^{-1}$$

(29)

with

$${\tilde{\mathit{C}}}_k={\Delta }_k{\left(\frac{\partial \mathit{g}}{\partial \mathit{x}}\right)}_{{\widehat{\mathit{x}}}_k^{-},{\mathit{u}}_k,\mathbf{0}}{\tilde{\mathit{N}}}_k={\Delta }_k{\left(\frac{\partial \mathit{g}}{\partial \mathbf{\nu }}\right)}_{{\widehat{\mathit{x}}}_k^{-},{\mathit{u}}_k,\mathbf{0}}$$

Finally, the a-posteriori state matrix is updated as follows:

$${\mathit{P}}_k^{+}=\left(\mathit{I}-{\tilde{\mathit{L}}}_k{\tilde{\mathit{C}}}_k\right){\mathit{P}}_k^{-}$$

(30)

where $\mathit{I}$ is the identity matrix.

4. Results

To validate the proposed algorithm, numerical results have been obtained on a quadrotor dynamic simulation environment including sensors dynamics and measurement noise. Such uncertainty was simulated by adding a bias and a white Gaussian noise to every sensor output. The main simulation parameters are summarized in

Table 1. Quadcopter and sensors main parameters. ${\mathit{I}}_N$ denotes the identity matrix $N\times N$.

Parameter	Value
Arm length—[m]	1
Mass—[kg]	0.6
Gyroscope sampling frequency—[Hz]	${10}^3$
Accelerometer sampling frequency—[Hz]	${10}^3$
Magnetometer sampling frequency—[Hz]	${10}^2$
Optical Flow sampling frequency—[Hz]	10
TOF sampling frequency—[Hz]	50
Gyroscope bias—[rad/s]	${[0.1,0.08,0.11]}^T$
Accelerometer bias—[m/s2]	${[0.1,0.15,0.2]}^T$
Magnetometer bias—[G]	${[0.05,0.02,0.06]}^T$
Optical Flow bias—[m/s]	${[-0.1,0.07]}^T$
TOF bias—[m]	${[1.0,0.7,-0.5,-0.1,0.2]}^T*{10}^{-3}$
Gyroscope noise covariance—[(rad/s)${}^2$]	$8.0·{\mathit{I}}_3·{10}^{-8}$
Accelerometer noise covariance—[(m/s2)2]	$1.0·{\mathit{I}}_3·{10}^{-4}$
Magnetometer noise covariance—[(G)${}^2$]	$4.0·{\mathit{I}}_3·{10}^{-8}$
Optical Flow noise covariance—[(m/s)${}^2$]	$1.0·{\mathit{I}}_2·{10}^{-6}$
TOF noise covariance—[(m)${}^2$]	$2.6·{\mathit{I}}_5·{10}^{-8}$

.

The idea is to test the algorithm in view of its implementation on a low-cost embedded board.

Although the navigation system has the objective of acquiring, filtering and fusing sensors data to extract information about UAV and surrounding environment states, the same electronic board can be used also for guidance and control tasks.

The sensors platform involves a low-cost IMU consisting of a 3-axis accelerometer, a 3-axis gyroscope and a 3-axis magnetometer, an optical flow and 5 time-of-flight distance sensors. Four of them can be installed below every arm and the fifth at the centre of the structure. The Optical flow and the time-of-flight devices are oriented downwards in order to measure the velocity vector in the horizontal plane and the relative distance to the ground.

The proposed simulation campaign considers as available sensors a tri-axial accelerometer, a gyroscope, a magnetometer, an optical flow, and two possibilities for TOFs: five or only a single device (see Figure 4). Sensor fusion algorithm is implemented using two different architectures: a tightly coupled scheme (A) and a loosely coupled scheme (B).

According with a tightly coupled approach, measurements are fused together by using the same EKF (see Figure 5), whose model is defined by state Equations (5), (6), (11)–(13), and output Equations (10), (15), (16) and (18).

The loosely coupled scheme involves two separated filters (see Figure 6): the former EKF is based on the IMU and TOF devices, to estimate the attitude on the basis of Equations (5), (6), (10), (15), (18); the latter MR-EKF, is based on the TOF plus the optical flow, to estimate altitude and velocity by means of Equations (11)–(13), (16) and (18).

Hence four possibilities are analysed:

$A_5$

$n_{TOF}=5$ and tightly coupled EKF architecture;

$A_1$

$n_{TOF}=1$ and tightly coupled EKF architecture;

$B_5$

$n_{TOF}=5$ and loosely coupled EKF architecture;

$B_1$

$n_{TOF}=1$ and loosely coupled EKF architecture;

where two of them, $A_5$ and $B_5$, use more than one TOF to augment attitude estimation, while $A_1$ and $B_1$ schemes are typical architectures, used to compare results with the state of the art.

As shown in Figure 4, the quadrotor dynamics includes attitude and altitude control. Simulation starts at $t=0$ from an equilibrium condition in hovering at the altitude $h=10$ m. Roll and pitch maneuvers are imposed by applying two reference doublet signals having a duration of 16 s and an amplitude of 24 degrees on the roll angle at $t_{\varphi }=2$ s, and on the pitch angle at $t_{\theta }=30$ s. A yaw maneuver is carried out by applying a trapezoidal signal with a duration of 7 s and an amplitude of 12 degrees to the yaw reference signal. Finally a variation in altitude is simulated using a ramp reference signal, passing from 10 m to 11 m.

Figure 7 and Figure 8 show sensors measurements.

Figure 9, Figure 10 and Figure 11 show the results of the estimation process for every involved configuration, while

Table 2. Models comparison in terms of Mean Absolute Error.

	Model ${\mathit{A}}_5$	Model ${\mathit{A}}_1$	Model ${\mathit{B}}_5$	Model ${\mathit{B}}_1$
$MA{E}_{\varphi }$   [deg]	0.0066	0.7369	0.0063	0.2156
$MA{E}_{\theta }$   [deg]	0.0098	0.6616	0.0096	0.0707
$MA{E}_{\psi }$   [deg]	0.0179	0.2104	0.019	0.1572
$MA{E}_{z_E}$   [m]	0.0012	0.084	0.0013	0.0899
$MA{E}_{V_{Ex}}$   [m/s]	0.0106	0.0149	0.0075	0.0594
$MA{E}_{V_{Ey}}$   [m/s]	0.0174	0.0089	0.009	0.0082
$MA{E}_{V_{Ez}}$   [m/s]	0.0055	0.017	0.0058	0.0055

and

Table 3. Models comparison in terms of standard deviation of errors.

	Model ${\mathit{A}}_5$	Model ${\mathit{A}}_1$	Model ${\mathit{B}}_5$	Model ${\mathit{B}}_1$
${\sigma }_{\varphi }$    [deg]	0.0619	2.8755	0.0640	3.0172
${\sigma }_{\theta }$    [deg]	0.0730	3.0470	0.0770	3.0838
${\sigma }_{\psi }$    [deg]	0.1949	1.5606	0.1979	1.66002
${\sigma }_{z_E}$    [m]	0.0028	0.1627	0.0030	0.1851
${\sigma }_{V_{Ex}}$    [m/s]	0.1042	0.4232	0.1316	0.4406
${\sigma }_{V_{Ey}}$    [m/s]	0.1136	0.4255	0.1307	0.3780
${\sigma }_{V_{Ez}}$    [m/s]	0.0231	0.0858	0.0250	0.1941

resume the performance of each model in terms of mean absolute errors and standard deviations [13,83].

Figure 9 shows the improvements obtained by the proposed technique over other standard EKF based INS. Indeed, a standard INS neglects the term of pure linear acceleration, resulting in an over-estimation of pitch and roll angles due to the unobservable body accelerations measured during a maneuver. Furthermore, the linear approximation needed in the EKF formulation makes roll and pitch angles more sensitive to the vertical component of acceleration, causing a coupling effect during the pitch maneuver at $t=30$ s. The use of a tightly coupled sensor fusion algorithm solves these issues (models $A_1$ and $A_5$, using IMU, optical flow and TOF sensors together). Furthermore, the presence of five TOF devices is the greatest improvement in terms of pitch and roll estimation (model $A_5$). Such improvement is visible also in the velocity vector estimation: while components on the horizontal plane depend moreover on the optical flow and the increasing performance affects the correction of relatively small errors, the greatest improvements is obtained in the vertical component of speed and in the altitude estimation. It should be clear how the presence of five TOF sensors permits to avoid errors on the attitude, making the altitude estimation more reliable.

Table 4. CPU time (ratio between the time spent in the EKF function and the overall simulation time).

	Model ${\mathit{A}}_5$	Model ${\mathit{A}}_1$	Model ${\mathit{B}}_5$	Model ${\mathit{B}}_1$
CPU %	3.34	3.15	3.03	1.9

resumes the computational load in terms of cpu occupancy, needed to execute the proposed filters on a Intel core i7 10750H based laptop. Being a multi-rate EKF, it was measured by computing the ratio (percentage) between the time spent to perform filtering and the overall simulation time. As shown in the table, the introduction of the multi-rate EKF limits the increase in the computational burden between model $B_1$ and $A_5$.

5. Conclusions

Small unmanned aerial vehicles need an accurate attitude and position estimation in order to deal with several application, where the risk of collision is high and the control must be absolutely reliable.

With this aim, the paper deals with the design of a Kalman filtering based sensor fusion algorithm to augment typical drone navigation, based on inertial sensors, accelerometers, gyroscopes, magnetometers, with an optical flow camera and several time-of-flight distance sensors, in order to improve attitude estimation while measuring distance to ground.

Results show the effectiveness of the proposed estimation procedure, comparing several sensors configuration and highlighting typical issues of inertial navigation, subject to attitude estimation errors in accelerated flight.

Currently, on commercial small drone the typical sensors configuration involves the use of the optical flow and only one ground distance sensor (sonar or TOF). The cost of adding more distance sensors is minimal in terms of weight and computational burden, but it would permit a great improvement in the attitude estimation as well as in the velocity and altitude measurement.