Enhanced Attitude and Altitude Estimation for Indoor Autonomous UAVs

: In recent years the use of Unmanned Aerial Vehicles (UAVs) has considerably grown in the civil sectors, due to their high ﬂexibility of use. Currently, two important key points are making them more and more successful in the civil ﬁeld, namely the decrease of production costs and the increase in navigation accuracy. In this paper, we propose a Kalman ﬁltering-based sensor fusion algorithm, using a low cost navigation platform that contains an inertial measurement unit (IMU), ﬁve ultrasonic ranging sensors and an optical ﬂow 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 ﬂight 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.


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 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.

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.  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 (  Euler angles ψ, θ and φ describe the aircraft attitude. An alternative representation makes use of the quaternion definition [17,64], considering q = [q 0 , q 1 , q 2 , q 3 ] T as the unit quaternion vector associated to R BE (q) from the NED reference frame to the body frame: Gyroscope model. If gyroscope was not affected by noise and uncertainties, the attitude could be estimated by integrating the angular velocity ω = [p, q, r] T from a known initial condition. In fact, quaternion vector shows the following dynamics: where the matrix Q(ω(t)) is 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 Therefore, by computing ω(t) from (4), the equation describing attitude dynamics becomesq 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ḃ where τ g is the correlation time of the process and υ 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 g = 0 0 g T . The specific force in the body reference frame is: The acceleration in the inertial reference frame a E (t) depends on a B t (t) as follows where 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: 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 b a (t) = [b ax (t), b ay (t), b az (t)] T and a stochastic zero-mean noise ν a (t) = [ν ax (t), ν ay (t), ν az (t)] T are considered: In case of slowly varying maneuvers, vector a B (t) = −a B (t) + b a (t) can be estimated using a first-order dynamics:ȧ where τ a is a time constant related to the UAV dynamics. The velocity in the inertial frame can be then obtained by integrating a ḃ Altitude z E is related to V E as follows: 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 M B in the body reference frame can be expressed as follows: where M E is the Earth's magnetic field. However, at the time instant t, magnetometer measurements M S (t) are affected by sensor noise ν M .
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: where u(t) and v(t) are the components of the velocity vector in the body frame at the time instant t, β is the focal length, z E (t) is the altitude of the UAV, d x (t) and d y (t) are the components on the horizontal plane of the distance vector between the center of gravity of the UAV and the camera, ν O x (t) and ν O y (t) 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 The vector of the sensed distance measurements is with ν TOF i (t) is the sensor noise on the i-th device (see Figure 3). where T is the process noise. Similarly, Equations (10), (15), (16) and (18) can be written in the following form: where T is the output vector and ν(t) = ν a (t) T , ν M (t) T , ν TOF (t) T T is the measurement noise vector.

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: where x is the state, u is the input, y is the measured output, and υ and ν are the process and measurement noises, respectively. This discrete time system can be obtained by applying Euler's discretization to (19) and (20) where ζ k = ζ(t k ) and t k = kT S is the k-th sampling time instant. Assume a zero-mean Gaussian distribution for the noises υ k−1 and ν k , with known covariance matrices Q k−1 and R k : 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 estimationx − k and the state covariance matrix , whereas the update phase computes the a-posteriori state estimationx + k and the state covariance matrix . According to the KF approach, at each time instant k the current state can be estimated by using the previous a-posteriori estimationx + k−1 and the last acquired set of measures y m,k depending on the frequency rate of each sensor.
Therefore, the a-priori estimation of the statex − k , is computed by using the Equation (21) with null process noise (υ = 0): The a-priori state covariance matrix P − k is computed as follows: where are the Jacobian matrices of function f (·), and the 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 To perform the update process of the a-priori state estimation, assume the minimum sample time T S = 1 ρ 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ỹ 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.
The partial estimated output is obtained by using the output Equation (22) and neglecting the measurements noise.ŷ 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:x where the gainL k is given by the following equation: Finally, the a-posteriori state matrix is updated as follows: where I is the identity matrix.

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.
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).
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.     1.0 · I 2 · 10 −6 TOF noise covariance-[(m) 2 ] 2.6 · I 5 · 10 −8 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 φ = 2 s, and on the pitch angle at t θ = 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. Figures 7 and 8 show sensors measurements. Figures 9-11 show the results of the estimation process for every involved configuration, while Tables 2 and 3 resume the performance of each model in terms of mean absolute errors and standard deviations [13,83]. (e) TOF #5.     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 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 .

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.