1 Introduction

The development and application of UAVs has grown in the last years. Beyond the military interest, the uses in the civilian field is also increasing; some examples are remote sensing, imaging and supervision [4].

A popular UAV platform is the quadrotor. It is a simple and relatively cheap system with very attractive characteristics, e.g. vertical take-off and landing and hovering flight.

In this paper, a quadrotor is studied with focus on navigation, more specifically, the determination of PVAT based on on-board sensors. The equipment used is shown in Fig. 1. It is called Gyro 200ED and has been developed by the Brazilian company Gyrofly for the use of universities and research institutes.

Fig. 1
figure 1

Gyrofly’s Gyro 200 ED quadrotor

Full size image

The determination of PVAT is performed by the data fusion of GPS receiver, barometer and IMU (accelerometers, gyroscopes and magnetometers). They are devices of MEMS technology, characterized by low-cost, low power consumption, small size and light weight. As a counterpart, they also have a relatively small accuracy. Which can prevent their application in feedback control systems, in such a way that the use of a state estimator is mandatory for improving the quality of measurements [7, 16].

One of the most popular techniques for state estimation in aerospace engineering is the EKF. The design of an EKF for determining the PVAT of a quadrator is described in [16], focusing on attitude estimation. The same EKF architecture was adopted in this work. However, some improvements are proposed concerning to both implementation issues and adaptive estimation.

The first contribution of this paper is represented by an architecture where one filter is used for the rotational and another one for the translational dynamics. Such separation enables the execution of each filter at a different time rate; this permits more flexibility for managing the on-board computational power. The filters are described in what follows with detailed information about design and implementation, in a deeper sense than the one usually found in the literature. Such information may be useful when developing an EKF for an UAV.

The second and main contribution of this work concerns the development of a new approach for adaptive estimation and its comparison with already existing techniques. AKF is important when the estimation is performed under conditions that go beyond the ideal underlying hypothesis about Kalman filtering [2].

The new method of AKF proposed here relies in the work of [12], with an extension based on fuzzy logic. It is compared with techniques that were recently applied to similar estimation problems: [2, 11].

The remaining of this paper is structured as follows. Section 2 describes the flight mechanics of the quadrotor, containing some equations used in both the design of the Kalman filters (KFs) and in the simulation model. In Sect. 3 the design of the EKF is described and several implementation issues are detailed, as well as the architecture of rotation and translation filters. Section 4 contains the new proposed method of AKF together with a summary of the previous known methods. Section 5 presents the hardware implementation and simulation studies for the standard and adaptive filters. Finally, Sect. 6 contains the conclusions of the work.

2 Flight mechanics

This section summarizes the flight mechanics of a quadrotor. It describes the foundations required for the development of the KFs and for the analysis of results. Its mechanical model can be found in several references such as [3, 8, 10, 14]. Figure 2 helps to understand the main definitions. The usual modelling hypotheses regarding hover flight and negligible drag are made in this work.

Fig. 2
figure 2

Illustration of the quadrotor main variables

Full size image

Two reference systems are adopted: the body one (BRF) and the inertial one (NED). BRF has the origin at the body’s cm. The inertial RF is obviously a simplification, since the rotation of the Earth is neglected. It is oriented along the local north, east and down directions, being the origin at the position of take-off.

The translational kinematic equation is represented in the NED RF:

$$\begin{aligned} {\dot{\mathbf {R}}}={\mathbf {V}},\quad \Rightarrow \ {\dot{x}}_n=v_n ,\quad {\dot{y}}_e=v_e ,\quad {\dot{z}}_d=v_d \end{aligned}$$
(1)

The translational dynamic equation is:

$$\begin{aligned} {\dot{\mathbf {V}}}={\mathbf {g}} + {\mathbf {C}}^b_0 \frac{{\mathbf {F}}_b}{m}={\mathbf {g}}+ {\mathbf {C}}^b_0 {\mathbf {a}}_e^b \end{aligned}$$
(2)

Note that \(\frac{{\mathbf {F}}_b}{m}={\mathbf {a}}_e^b\) is the specific force written in the BRF.

The dynamics of the rotational motion is given by the Euler equation:

$$\begin{aligned} {\mathbf {I}}{\dot{\varvec{\omega }}}={\mathbf {M}}-{\varvec{\omega }}\times {\mathbf {I}}{\varvec{\omega }} \end{aligned}$$
(3)

In this work, two parametrizations of the rotational kinematics are adopted. Quaternions are used for writing the differential equations of motion; Euler angles are taken for the visualization of results and feedback control.

A quaternion \({\mathbf {q}}\) is a “hyper-complex number”. It represents a rotation according to the definition:

$$\begin{aligned} {\mathbf {q}}=\left[ \begin{array}{c}q_0\\ q_1\\ q_2\\ q_3 \end{array} \right] =\left[ \begin{array}{c}\cos {\left( \alpha \over 2\right) }\\ u_1\sin {\left( \alpha \over 2\right) }\\ u_2\sin {\left( \alpha \over 2\right) }\\ u_3\sin {\left( \alpha \over 2\right) } \end{array} \right] \end{aligned}$$
(4)

where the scalar \(q_0=\cos {\left( \alpha \over 2\right) }\) is called the real part; the vector \({\mathbf {v}}=[q_1\ q_2\ q_3]^t=\sin {\left( \alpha \over 2 \right) }{\mathbf {u}}\) is the imaginary part. The angle \(\alpha\) and the unit vector \({\mathbf {u}}=[u_1\ u_2\ u_3]^t\) form a pair of angle and rotation axis that generate the current BRF from another RF (Euler’s rotation theorem [15]).

The quaternion’s rotational kinematics is determined by:

$$\begin{aligned} {\dot{\mathbf {q}}}=\frac{1}{2}{\varvec{\Omega }}_s{\mathbf {q}}=\frac{1}{2}{\mathbf {Q}}_s{\varvec{\omega }} \end{aligned}$$
(5)
$$\begin{aligned} {\varvec{\Omega }}_s=\left[ \begin{array}{rrrr} 0 &{} -p &{} -q &{} -r\\ p &{} 0 &{} r &{} -q\\ q &{} -r &{} 0 &{} p\\ r &{} q &{} -p &{} 0 \\ \end{array}\right] ,\quad {\mathbf {Q}}_s=\left[ \begin{array}{rrr} -q_1 &{} -q_2 &{} -q_3\\ q_0 &{} -q_3 &{} q_2\\ q_3 &{} q_0 &{} -q_1\\ -q_2 &{} q_1 &{} q_0 \\ \end{array}\right] \end{aligned}$$
(6)

The Euler angle’s kinematic equations are nonlinear and have singularities. On the other hand, the quaternion’s linear model (5) is more appropriate for developing the KFs.

The transformation matrix \({\mathbf {C}}^0_b\) can be obtained from the quaternion:

$$\begin{aligned} \mathbf {C}^0_b=\left[ \begin{array}{ccc} q_1^2-q_2^2-q_3^2+q_0^2 &{} 2(q_1q_2+q_3q_0) &{} 2(q_1q_3-q_2q_0)\\ 2(q_1q_2-q_3q_0) &{} -q_1^2+q_2^2-q_3^2+q_0^2 &{} 2(q_2q_3+q_1q_0)\\ 2(q_1q_3+q_2q_0) &{} 2(q_2q_3-q_1q_0) &{} -q_1^2-q_2^2+q_3^2+q_0^2 \end{array} \right] \end{aligned}$$
(7)

Because \({\mathbf {C}}^0_b\) is orthogonal: \({{\mathbf {C}}^b_0}^{-1}={{\mathbf {C}}^b_0}^t\ \Rightarrow \ {\mathbf {C}}^0_b={{\mathbf {C}}^b_0}^t\).

In aeronautical applications, the standard Euler angles (assumed in this work) are called 3-2-1. They generate the transformation from NED to BRF according to the sequence of elementary rotations \(\psi\)-z, \(\theta\)-y, \(\phi\)-x, which relate angles and appropriate intermediate axes [15]. Their conversion from quaternion is given by:

$$\begin{aligned}\phi &=\arctan {(2 (q_2q_3+q_0q_1)/(q_0^2-q_1^2-q_2^2+q_3^2))}\nonumber \\ \theta &=\arcsin {(-2(q_1q_3-q_0q_2))}\\ \psi &=\arctan {(2(q_1q_2+q_0q_3) / (q_0^2+q_1^2-q_2^2-q_3^2))}\nonumber \end{aligned}$$
(8)

2.1 Forces and moments

When the quadrotor is in hovering flight, a simple model is usually adopted for the aerodynamic forces and moments [3, 14]. See Fig. 2 for a general illustration. In this approach, the resulting non-gravitational force is:

$$\begin{aligned} {\mathbf {F}}_b=[0\quad 0\quad -T ]^t, \quad T=k_f( \omega _1^2+\omega _2^2+\omega _3^2+\omega _4^2) \end{aligned}$$
(9)

The torques \({\mathscr {L}}\) and \({\mathscr {M}}\) originate from the moment arm l:

$$\begin{aligned} {\mathscr {L}}=F_4 l - F_2 l = k_f l(\omega _4^2-\omega _2^2), \quad {\mathscr {M}}=F_1 l - F_3 l = k_f l(\omega _1^2-\omega _3^2) \end{aligned}$$
(10)

On the other hand, the propeller’s reaction aerodynamic moments are summed to originate the yawing moment \({\mathscr {N}}\):

$$\begin{aligned} {\mathscr {N}}=M_1-M_2+M_3-M_4=k_m( \omega _1^2-\omega _2^2+\omega _3^2-\omega _4^2) \end{aligned}$$
(11)

2.2 Feedback flight control

Fig. 3
figure 3

General scheme of the feedback flight control

Full size image

The feedback control architecture is based on [13]. The general scheme is illustrated in Fig. 3. The RCS acts upon the Euler angles \(\phi\), \(\theta\) and \(\psi\). A feedback loop of PIV type was designed for these angles. The proportional and integral actions receive the estimates of \(\phi\) and \(\theta\) from the KF, as well as \(\psi\) from the IMU. The velocity action receives angular speeds p, q and r measured by the gyroscopes.

TCS loop acts upon the height H and horizontal coordinates \(x_n\) and \(y_e\). A PIV loop was assumed again. The integral and proportional actions are based on estimates of H, \(x_n\) and \(y_e\); the velocity feedback use estimates of the speeds \(v_n\), \(v_e\) and \(v_d\), all these measurements are obtained from the KF.

Only the variables \(\psi\), H, \(x_n\) and \(y_e\) are independently controlled. The RCS is an internal loop of the controller that governs the horizontal displacement \(x_n-y_e\). The outputs of the TCS are the total thrust T and references for the angles \(\phi\) and \(\theta\). The outputs of the RCS are the rolling, pitching and yawing moments.

3 State estimation

The quadrotor is naturally unstable and its stabilization is strongly dependent on the feedback of the roll and pitch angles and respective angular speeds p and q. Then, a suitable estimate of these angles is crucial for the system operation. The angle \(\psi\) is important for pointing the \(x_b\) axis during the translational motion and also for orientation of instruments (e.g. a camera in imaging applications).

In order to perform autonomous flight, following a given reference path, the determination of translational speeds, altitude and horizontal position is necessary. These variables are taken in the NED RF based on GPS and barometer measurements. However, pure GPS data have errors and slow data updating that compromise its application in feedback flight controls. Therefore, the improvement of these measurements via sensor fusion is necessary.

In order to determine the PVAT, the quadrotor is equipped with accelerometers, gyroscopes, magnetometers, barometer and GPS receiver. The EKF is applied to perform the fusion of their data. The general EKF architecture employed in the UAV is described below. The scheme is similar to that shown in reference [16]. Nevertheless, in the present paper, there are individual filters for rotation and translation, which is useful during implementation. The text also presents a deeper detailing of the approach, that may help the reader interested in implementing the system.

Because the notation and the general form of equations in Kalman filtering may vary from one author to another, the Appendix presents the formulation taken in this paper which is similar to the one in reference [6].

3.1 Architecture of the UAV’s EKF

According to Sect. 2, the quadrotor translational mechanics depends on the rotational motion (see e.g. Eq. 2). Furthermore, when establishing the KF equations, the rotational mechanics also becomes dependent on the translational motion. However, despite this interdependence, two different KFs are developed: one for rotation, and another one for translation. The reason for the segregation regards implementation issues in the on-board processors: by separating the filters, each one can run at a specific time rate (the rotation filter having higher speed). Physically, this is justified by the fact that the time constants of rotation are much smaller than those of translation.

Fig. 4
figure 4

General scheme of the rotation and translation filters

Full size image

The Fig. 4 shows the general scheme of the rotation and translation filters. The REKF estimates the Euler angles \(\phi\) and \(\theta\) using the quaternion representation; the conversion is performed using the equation’s set 8. The TEKF, by its turn, estimates the inertial speeds \(v_n\), \(v_e\), \(v_d\), the horizontal coordinates \(x_n\) and \(y_e\), as well as the ground height H.

The inputs for the REKF are the p, q and r angular speeds measured by the gyroscopes, the specific force measurements \([a_e^x\ a_e^y\ a_e^z]^t\) from the accelerometers and the approximation of inertial acceleration in the NED RF \([a_i^x\ a_i^y\ a_i^z]^t\) (obtained from the TEKF).

The TEKF receives the specific force measurements from the accelerometers, the Euler angles from the REKF and IMU, the speeds \(v_n\), \(v_e\), \(v_d\) and latitude and longitude (in fact, their variations with respect to the point of take off) from the GPS receiver, and the height H from the barometer.

3.2 Rotation extended Kalman filter

A diagram of the REKF is shown in Fig. 5. The sub-index “k” indicates the k-th time sample in a discrete time implementation. The state of the REKF is the quaternion. The gyroscope’s measurements at time k are used in the prediction step to evaluate: \({\hat{q}}^{-}_{k+1}\). The accelerometer’s measurements and inertial acceleration approximation are inserted in the correction step to compute: \({\hat{q}}^{+}_{k+1}\).

Fig. 5
figure 5

Structure of the rotation extended Kalman filter

Full size image

In the Fig. 5, the prediction is performed using the function \({\mathbf {f}}_r(q,\omega )\) given by Eq. 5 (the rotational kinematics). A first-order numerical integration (Euler method) is performed with time step \(T_r\), which defines the frequency of the REKF: \(f_r=1/T_r\). The prediction of the error covariance matrix is done as usual, according to Eqs. 33–36. Note that this task depends on the state variable and on the angular speed, the former being necessary in Eq. 35 (see also Eqs. 5 and 30) and the last for computing the Jacobian in Eq. 34 (from Eq. 5, one can see that this Jacobian is one half of the first matrix on Eq. 6).

In Fig. 5, the prediction of the output (\({\hat{a}}_e\)) is performed by the function \(h_r(q,a_i)\) using \({\hat{q}}^{-}_{k+1}\). This relation is given by Eq. 12. The inertial acceleration \({\mathbf {a}}_i=[a_i^x\ a_i^y\ a_i^z]^t\) and g give the specific force in the NED RF. Then the matrix \({\mathbf {C}}^0_b\) (determined by the quaternion and Eq. 7) generates the predicted specific force in the BRF. The accelerometer determines a measurement in the BRF, which is compared to the prediction. The error is multiplied by the Kalman gain and the result permits to determine the corrected estimate \({\hat{q}}^{+}_{k+1}\).

$$\begin{aligned} {\mathbf {a}}_e^b(t)={\mathbf {C}}^0_b(q)\left[ \begin{array}{c}a_i^x\\ a_i^y\\ a_i^z-g \end{array}\right] \end{aligned}$$
(12)

As usual the Kalman gain is computed from Eqs. 37 and 38. The state and the inertial acceleration are required to compute the Jacobian of the output Eq. 12. The correction of the error covariance matrix is obtained from Eq. 40.

3.3 Translation extended Kalman filter

A diagram of the TEKF is shown in Fig. 6. The state of the TEKF is: \({\varvec{\chi }}=[v_n\ v_e\ v_d\ x_n\ y_e\ z_d]^T\). The accelerometer’s measurements at time k are used in the prediction: \({\hat{{\varvec{\chi }}}}^{-}_{k+1}\). The GPS receiver and barometer data are taken in the correction: \({\hat{\varvec{\chi }}}^{+}_{k+1}\). The horizontal displacements are given from the latitude and longitude via standard geodetic formulas. The Euler angles from the REKF and IMU are sent to the equations of prediction.

Fig. 6
figure 6

Structure of the translation extended Kalman filter

Full size image

In Fig. 6, the prediction is performed using the function \({\mathbf {f}}_t({\varvec{\chi }},{\mathbf {a}}_e,{\varvec{\Theta }})\) given by Eqs. 2 and 1. The variable \({\varvec{\Theta }}\) is a vector that contains the Euler angles, required to compute the transformation matrix \({\mathbf {C}}^b_0\). These are relations of translational kinematics since Eq. 2 receives the acceleration measurements of accelerations, not forces. A first order numerical integration (Euler method) is performed using the time step \(T_t\), which defines the frequency of the TEKF: \(f_t=1/T_t\). The prediction of the error covariance matrix is done as usual, according the Eqs. 33–36. Notice that this process depends on the Euler angles (Eqs. 2, 30 and 35). The Jacobian \({\mathbf {F}}\) in this filter is constant (see Eqs. 1 and 2).

In Fig. 6, the prediction of the output is simply the state: \({\hat{\varvec{\chi }}}^{-}_{k+1}\). The GPS receiver and barometer supply the measurement, from which the corresponding prediction is subtracted. The error is multiplied by the Kalman gain matrix, and the result is used to compute the corrected estimate: \({\hat{\varvec{\chi }}}^{+}_{k+1}\).

The Kalman gain is determined as usual from Eqs. 37 and 38. The correction of the error covariance matrix is performed according to Eq. 40. Since the measurement is the state vector itself, the matrix \({\mathbf {H}}_k\) is an identity.

3.4 Theoretical aspects concerning the filters

This section explains some assumptions and specific theoretical issues behind the development of the filters. Some implementation aspects are also discussed.

The gyroscope’s model is very simple, it is composed by an ideal measurement of angular velocity and additive noise:

$$\begin{aligned} {\varvec{\omega }}_g(t)={\varvec{\omega }}(t)+{\mathbf {w}}_g(t) \end{aligned}$$
(13)

where \({\varvec{\omega }}_g\) is the gyroscope’s measurement of the true velocity \({\varvec{\omega }}\). It is corrupted by white Gaussian noise \({\mathbf {w}}_g\) with zero mean and variance \({\mathbf {P}}_g\). In fact, the sensor has some bias, but it is determined prior to the filter operation and subtracted from the data. For this purpose, the sensor is kept at rest by 10 s and the average measurement is calculated.

The accelerometer’s model is analogous:

$$\begin{aligned} {\mathbf {a}}_a(t)={\mathbf {a}}_e^b(t)+{\mathbf {w}}_a(t) \end{aligned}$$
(14)
$$\begin{aligned} {\mathbf {a}}_e^b(t)={\mathbf {a}}^b(t)-{\mathbf {g}}^b(t) \end{aligned}$$
(15)

where \({\mathbf {a}}_a\) is the accelerometer’s measurement of the true specific force \({\mathbf {a}}_e^b\), the difference between inertial and gravitational accelerations expressed in the BRF. It is corrupted by white Gaussian noise \({\mathbf {w}}_a\) with zero mean and variance \({\mathbf {P}}_a\).

Equation 12 is a transformation of the ideal accelerometer measurement from the NED to the BRF. Supposing that a reference value in the NED RF is known, it can be compared to the data measured in the BRF using the transformation matrix. This method is known as “determination of attitude from vector observations”. According to [9], at least two non-collinear inertial directions are necessary for determining the attitude. This is the case of geomagnetic field and gravity. However, note that the magnetic data is not considered in the REFK of Sect. 3.2. In face of this limitation, only the angles \(\phi\) and \(\theta\) can be determined. The underlying reasons for avoiding the use of magnetometer in the REKF are:

  • The reference Earth magnetic field can be obtained from models available in the literature, for example, the International Geomagnetic Reference Field (IGRF-11) (World magnetic model, http://www.ngdc.noaa.gov/geomagmodels/IGRFWMM.jsp). However, these models do not take into account local anomalies of the field, which can corrupt the computation of the Euler angles;

  • The magnetometer can be calibrated to compensate local magnetic perturbations, e.g. the field generated by electric motors. However, some degradation in the Euler angles may still occur;

  • Experimental results show that the errors above can be easily tolerated when estimating the heading angle, which serves for pointing the reference axis \(x_b\) during translation. However, their impact on roll and pitch angles may be unacceptable. The quadrotor is unstable and its stabilization is strongly dependent on the feedback of \(\phi\) and \(\theta\). In some flight tests, the magnetometers errors caused the loss of stability.

Concerning the GPS and barometer models, they are analogous to the previous ones (gyroscope and accelerometer): white Gaussian noises with zero mean and given variances are added to the respective ideal measurements.

All these models are strong simplifications of the reality. They do not consider correlated noises usually found in the GPS and inertial sensors. A deep discussion about those errors can be found in the literature, e.g.: [6], chapter 7, and [5, 17].

4 Adaptive Kalman filter

In general, the design parameters of a KF are the process and measurement noise covariance matrices \({\mathbf {Q}}\) and \({\mathbf {R}}\), respectively.

Considering the filters in Sects. 3.2–3.3 and the sensor models of Sect. 3.4, the covariance matrices are:

$$\begin{aligned} {\mathbf {Q}}_{R}=\left[ \begin{array}{ccc}_{gyr}\sigma ^2_{p} &{} 0 &{}0\\ 0 &{} _{gyr}\sigma ^2_{q} &{}0\\ 0 &{} 0 &{}_{gyr}\sigma ^2_{r} \end{array}\right] ,\ \ {\mathbf {R}}_{R}=\left[ \begin{array}{ccc}_{acc}\sigma ^2_{x} &{} 0 &{}0\\ 0 &{} _{acc}\sigma ^2_{y} &{}0\\ 0 &{} 0 &{}_{acc}\sigma ^2_{z} \end{array}\right] , \end{aligned}$$
(16)

where \(_{gyr}\sigma ^2_{\alpha }\) is the variance of the noise in the measurement of the angular speed \(\alpha =p,\ q,\ r\). Analogously, \(_{acc}\sigma ^2_{\beta }\) is related to the measurement of specific forces along the axes \(\beta =x_b,\ y_b,\ z_b\).

$$\begin{aligned} {\mathbf {Q}}_{T}=\left[ \begin{array}{ccc}_{acc}\sigma ^2_{x} &{} 0 &{}0\\ 0 &{} _{acc}\sigma ^2_{y} &{}0\\ 0 &{} 0 &{}_{acc}\sigma ^2_{z} \end{array}\right] ,\ \ {\mathbf {R}}_{T}=\left[ \begin{array}{cccccc}_{gps}\sigma ^2_{v_n} &{} 0 &{}0 &{}0 &{}0 &{} 0\\ 0 &{} _{gps}\sigma ^2_{v_e} &{}0 &{} 0 &{} 0 &{} 0\\ 0 &{} 0 &{}_{gps}\sigma ^2_{v_d} &{} 0 &{} 0 &{} 0\\ 0 &{} 0 &{}0 &{} _{gps}\sigma ^2_{x_n} &{} 0 &{} 0\\ 0 &{} 0 &{}0 &{} 0 &{} _{gps}\sigma ^2_{y_e} &{} 0\\ 0 &{} 0 &{}0 &{} 0 &{} 0 &{} _{bar}\sigma ^2_{H} \end{array}\right] , \end{aligned}$$
(17)

where \(_{gps}\sigma ^2_{v_{\alpha }}\) is the variance of the noise in the measurement of the speed \(\alpha =n,\ e,\ d\). Analogously, \(_{gps}\sigma ^2_{\beta }\) is related to the measurement of horizontal displacement along the axes \(\beta =x_n,\ y_e\). \(_{bar}\sigma ^2_{H}\) refers to the measurement of the height H.

The design of the Kalman filter involves the determination of the variance of each measurement. If the properties of a sensor change, a new tuning is necessary. The objective of adaptive Kalman filters is the automatic adjustment of the filters based on some rules. There is a variety of approaches in the literature that can be applied, but some of them involve a heavy computational load that may be prohibitive for small UAVs with limited on-board processors. In this way, a search was performed in order to find simple and effective adaptive algorithms.

The methods of references [2, 11] were chosen for application in the UAV. They are related to the innovation process of the KF. An approach that involves metrics of bias and dispersion was also selected. This last technique was modified and a new formulation is proposed.

A summary of the methods of references [2, 11] is presented in Sects. 4.1 and 4.2, respectively. These discussions are included in order to introduce some concepts used in the remaining of the paper, to understand the results of Sect. 5 and to highlight the improvements of the new method of Sect. 4.3.

4.1 Experimental innovation covariance adaptation

Reference [11] develops a simple adaptive method. It involves the experimental determination of a covariance matrix. Here, it is called “experimental innovation adaptive estimation” (EIAE).

The innovation process is shown in Eq. 18. It is used in the correction of the state estimate in a KF, as shown in Eq. 39. The role of this process is to show the novelty brought to the system by the sensor measurement.

$$\begin{aligned} {\tilde{\mathbf {e}}}_{z_k}={\mathbf {z}}_k-{\mathbf {h}}({\hat{\mathbf {x}}}_k^-) \end{aligned}$$
(18)

If the KF is performing optimally, the innovation must be a white Gaussian sequence with zero mean. In this case, its theoretical ICM is given by:

$$\begin{aligned} {\mathbf {P}}_{\tilde{e}_{z_k}}={\mathbf {H}}_k{\mathbf {P}}_k^-{\mathbf {H}}_k^t+{\mathbf {R}}_k \end{aligned}$$
(19)

On the other hand, an approximation of the standard matrix \({\mathbf {P}}_{\tilde{e}_{z_k}}\) can be obtained from the measured data:

$$\begin{aligned} {\hat{\mathbf {C}}}_{\tilde{e}_{z_k}}=\frac{1}{N}\sum _{i=i_0}^{k}\tilde{{\mathbf {e}}}_{z_i}({\tilde{\mathbf {e}}}_{z_i})^t \end{aligned}$$
(20)

This approximation is evaluated by taking a moving average with a data window of N points. Here \(i_0=k-N+1\) is the first sample inside the window.

The adaptation in [11] was applied in this paper through the introduction of ICM approximation \({\hat{\mathbf {C}}}_{\tilde{e}_{z_k}}\) into the EKF gain matrix \({\bar{\mathbf {K}}}_k\); i.e., replacing \({\mathbf {P}}_{\tilde{e}_{z_k}}\) by \({\hat{\mathbf {C}}}_{\tilde{e}_{z_k}}\) in Eq. 37. The idea underlying this method is: if the measurement noise increases, the matrix \({\hat{\mathbf {C}}}_{\tilde{e}_{z_k}}\) should increase too. In this way, the filter gain \({\bar{\mathbf {K}}}_k\) will decrease (see Eq. 37), which means that the filter estimate will depend less on the measurement information.

4.2 Fuzzy innovation adaptive estimation

Note that the EIAE eliminates the matrix \({\mathbf {R}}\) from the KF algorithm. On the other hand, an adaptation involving the improvement of the matrix \({\mathbf {R}}\) is developed in reference [2]. This approach evaluates a covariance mismatch using a FIS. Here, it is called fuzzy innovation adaptive estimation (FIAE).

The fuzzy inference is performed using the error defined in Eq. 21. This matrix represents the mismatch between the theoretical and the measurement-based (approximated) ICM:

$$\begin{aligned} {\varvec{\alpha }}_k={\mathbf {P}}_{\tilde{e}_{z_k}}-{\hat{\mathbf {C}}}_{\tilde{e}_{z_k}} \end{aligned}$$
(21)

The underlying idea is to formulate fuzzy rules for tuning the matrix \({\mathbf {R}}_k\) in order to make the covariance mismatch as small as possible. Only the main diagonal of \({\mathbf {R}}_k\) is adjusted. Because the matrices \({\mathbf {R}}_k\) and \({\varvec{\alpha }}_k\) have the same dimensions, the tuning of the (i, i)-th element of \(\mathbf {R}_k\) considers the (i, i)-th element of \({\varvec{\alpha }}_k\). Therefore, only SISO FIS are needed.

The input of a single SISO FIS is the (i, i)-th element of the matrix \({\varvec{\alpha }}_k\), which generates a tuning factor \(\mu _i\) as an output. In [2], \(\mu _i=\Delta R^i_k\) is an additive factor:

$$\begin{aligned} {\mathbf {R}}_k(i,i)={\bar{\mathbf {R}}}(i,i)+\Delta {R}^i_k \end{aligned}$$
(22)

where \({\bar{\mathbf {R}}}\) denotes the nominal value of the matrix \({\mathbf {R}}\).

In the FIAE developed here for the quadrotor UAV, the additive tuning was replaced by a multiplicative one. The main reason is to provide a means of changing the order of magnitude of matrix \({\mathbf {R}}_k\). This choice comes from the design experience acquired in empirical tests. Equation 23 shows the multiplicative rule adopted. The exponent \({\mu _k^{i}}\) is the tuning factor generated by the FIS.

$$\begin{aligned} {\mathbf {R}}_k={\rm {diag}}(10^{\mu ^1_k}\ 10^{\mu _k^2}\ \ldots \ 10^{\mu _k^{p}}){\bar{\mathbf {R}}} \end{aligned}$$
(23)

4.3 Fuzzy adaptation with heuristic metrics

In reference [12], a novel technique for adjusting the matrix \({\mathbf {Q}}\) is proposed. The method involves the optimization of some heuristic metrics related to bias and amplitude of oscillations of the KF estimates. These metrics are interesting since they provide an alternative to evaluate the KF performance. However, it may be prohibitive when applied to an UAV with relatively small computational power, since it involves the execution of a numeric minimization procedure at each iteration. This method requires the processing of the KF several times during a single sample interval.

In order to avoid the high computational cost of the numeric minimization, a new approach is developed here: the use of fuzzy logic to assess the behaviour of the metrics. This also eliminates the need for simplifications in the kinematic models proposed by [12].

4.3.1 Bias and oscillation metrics

Fig. 7
figure 7

An illustration of biased and oscillating estimations

Full size image

Figure 7 shows an illustration of highly biased or oscillating filter estimates. In the case (a), a small covariance matrix \({\mathbf {Q}}\) means that the estimates are mostly related to the predictions; in such a case, smooth results are obtained, but they tend to drift the real value of the variable; this is caused by the integration of the process noise and bias. In case (b), a large matrix \({\mathbf {Q}}\) means that the results are mostly related to the corrections; in such a case, the filter output tends to follow the sensor measurements, generating oscillations.

Reference [12] aims to quantify the biased and oscillating behaviours of the filter. Two metrics are defined for an estimated scalar output \({\hat{z}}^i_k={\mathbf {h}}_i({\hat{\mathbf {x}}}_k^+)\), where k refers to the k-th filter iteration and i is the index of the i-th scalar output.

The bias metric represents the quadratic deviation between the KF estimates and the sensor measurements. Its definition, shown in Eq. 24, is based on smoothing the estimates \({\hat{z}}_i^k\) and sensor measurements \(z_i^k\) via LSF inside a time window of N elements:

$$\begin{aligned} b_k^i=||{\mathbf {f}}_{{\mathbf {z}}_i}-{\mathbf {f}}_{{\hat{\mathbf {z}}}_i}||^2 \end{aligned}$$
(24)

where \({\mathbf {f}}_{{\mathbf {z}}_i}\) is a vector that contains N elements inside the time window. They are the sensor outputs \(z_i^{\kappa }\), \(\kappa =k-N+1, k-N+2, \ldots k\), after smoothing. \({\mathbf {f}}_{{\hat{\mathbf {z}}}_i}\) is a vector of the same nature, but it is associated to the KF estimates \({\hat{z}}_i^k\).

The metric of amplitude of oscillations aims to evaluate the degree of dispersion of the KF estimates with respect to its mean behaviour. Once again, this “mean behaviour” is determined from LSF. Its definition is:

$$\begin{aligned} a_k^i=||{\mathbf {f}}_{{\hat{\mathbf {z}}}_i}-{\hat{\mathbf {z}}}_i||^2 \end{aligned}$$
(25)

where \({\mathbf {f}}_{{\hat{\mathbf {z}}}_i}\) is the same vector of Eq. 24. The vector \({\hat{\mathbf {z}}}_i\), in turn, contains the estimates itself: \({\hat{z}}_i^{\kappa }\), \(\kappa =k-N+1, k-N+2, \ldots k\).

According to [12], a crucial point underlying the metrics is the size N of the data window. If the measurements oscillate sufficiently fast, the difference \({\mathbf {f}}_{{\mathbf {z}}_i}-{\mathbf {f}}_{{\hat{\mathbf {z}}}_i}\) may have its sign changed inside the interval and the bias metric will start to measure oscillations. On the other hand, simulations of the quadrotor UAV showed that too small N may hinder the oscillation measurement.

In [12], a second-degree polynomial is chosen to perform the LSF. There it is shown that, under this assumption, the expressions for \(b_k^i\) and \(a_k^i\) are given by:

$$\begin{aligned} b_k^i=||{\mathbf {K}}\left( {\hat{\mathbf {z}}}_i(\kappa )-{\mathbf {z}}_i(\kappa )\right) ||^2 \end{aligned}$$
(26)
$$\begin{aligned} a_k^i=||\left( {\mathbf {K}}-{\mathbf {I}}\right) {\hat{\mathbf {z}}}_i(\kappa )||^2 \end{aligned}$$
(27)

where \({\hat{\mathbf {z}}}_i(\kappa )=[{\hat{z}}_i(k-N+1)\ {\hat{z}}_i(k-N+2)\ldots \ {\hat{z}}_i(k)]^t\) contains the Kalman estimates of the output \(z_i\) at the time samples \(\kappa =k-N+1, k-N+2, \ldots k\); on the other hand, \({\mathbf {z}}_i(\kappa )=[z_i(k-N+1)\ z_i(k-N+2)\ldots \ z_i(k)]^t\) contains the sensor measurements. \({\mathbf {I}}\) is the identity matrix. \({\mathbf {K}}\), which is shown in Eq. 28, is obtained from the solution of a quadratic minimization problem:

$$\begin{aligned} {\mathbf {K}}={\mathcal {K}}\left( {\mathcal {K}}^t{\mathcal {K}}\right) ^{-1}{\mathcal {K}}^t,\quad {\mathcal {K}}=\left[ \begin{array}{lll}1 &{} 1 &{} 1\\ 1 &{} 2 &{} 4\\ \vdots &{} \vdots &{} \vdots \\ 1 &{} N &{} N^2 \end{array} \right] \end{aligned}$$
(28)

4.3.2 The new method of fuzzy inference with bias and oscillation metrics

Design experience shows that, when tuning the matrices \({\mathbf {Q}}\) and \({\mathbf {R}}\) in a KF, the relative weighting between them is determinant for the filter performance. So, it seems reasonable to adjust the matrix \({\mathbf {R}}\) from the metrics of bias and amplitude of oscillations. In such a situation, the increase of matrix \({\mathbf {R}}\) causes the growth of bias, while the decrease of \({\mathbf {R}}\) amplifies the amplitude of oscillations.

In [12], matrix \({\mathbf {Q}}\) is adapted through the minimization of a cost that combines these two performance metrics (bias and amplitude of oscillations). On the other hand, in this paper both matrices \({\mathbf {Q}}\) and \({\mathbf {R}}\) are tuned based on these metrics, but using fuzzy inference. This adaptive filter is called FBA–EKF.

It is well known that the fuzzy inference can be used to aggregate empirical knowledge to an engineering design. The method in [2] shows this when adapting the KF from the innovation process. Similar reasoning is used here, but the metrics of performance chosen are the bias and amplitude of oscillations. Note that, in last case, the designer has two variables for evaluating the performance of each output of the filter, whereas in the former there is only one: the innovation. In this way, the designer has additional means for inserting empirical knowledge in the adaptation rules of the filter.

Another advantage of the proposed method is that the designer can chose which matrix is more suitable to be adapted: \({\mathbf {Q}}\) or \({\mathbf {R}}\)—this will depend on the physical characteristics of the system.

The method is explained from the properties of the flight mechanics of the UAV. The key idea is to insert as much physical information as possible, based on the versatility of the bias and oscillation metrics. The knowledge of flight mechanics introduced in Sect. 2 and the structure of the EKF presented in Sect. 3 are used to determine how the matrices \({\mathbf {Q}}\) and \({\mathbf {R}}\) will be adapted.

The adaptation of matrix \({\mathbf {Q}}\) is performed in the REKF, while the matrix \({\mathbf {R}}\) is adapted in the TEKF. The reason for this choice lies in the architecture of the sensors employed.

Matrices \({\mathbf {Q}}_R\) and \({\mathbf {R}}_R\) of the REKF are shown in Eq. 16. For small angles, when using the accelerometer to measure \(\phi\) and \(\theta\), the measurement of the x-axis accelerometer and \(\theta\) are related approximately by \(\theta \approx \sin ^{-1}(a_{e}^x/g)\), while the y, z-axis accelerometers are approximately related to \(\phi\) by \(\phi \approx \tan ^{-1}(a_e^y/a_e^z)\). On the other hand, the p and q gyroscopes, which correspond to \({\mathbf {Q}}_R(1,1)\) and \({\mathbf {Q}}_R(2,2)\), are approximately related with \(\phi\) and \(\theta\) respectively by \({\dot{\phi }}\approx p,\ {\dot{\theta }}\approx q\). Thus, the output \(z_1=a_e^x\) is used to adjust \({\mathbf {Q}}_R(2,2)\), while the outputs \(z_2=a_e^y\) and \(z_3=a_e^z\) are considered for the tuning of \({\mathbf {Q}}_R(1,1)\).

The \({\mathbf {Q}}_T\) and \({\mathbf {R}}_T\) matrices of the TEKF are shown in Eq. 17. In the TEKF, there are six measured outputs (\(v_n\), \(v_e\), \(v_d\), \(x_n\), \(y_e\) and H) and three process inputs (\(a_e^x\), \(a_e^y\), \(a_e^z\)). If these six outputs were chosen for the adaptation of the matrix \({\mathbf {Q}}_R\), this would cause practical and conceptual problems. From the practical point of view, six measurements are used to adapt only three parameters. In the conceptual side, these outputs do not have a clear relationship with the matrix \({\mathbf {Q}}\). For example, if \(\psi =0^{\circ }\), and \(\phi\) and \(\theta\) are small, \(v_n\) and \(x_n\) are related to \(a_e^x\), while \(v_e\) and \(y_e\) correspond to \(a_e^y\) (because the BRF and NED RF are almost parallel). But, if \(\psi =90^{\circ }\): \(v_n\), and \(x_n\) are related to \(a_e^y\), while \(v_e\) and \(y_e\) correspond to \(a_e^x\) (because \(x_b\) and \(y_b\) become collinear with \(y_e\) and \(x_n\), respectively). In view of these reasons, the adaptation of \({\mathbf {Q}}_T\) is less attractive, but the choice of \({\mathbf {R}}_T\) can solve the problem: each particular output variable matches the respective element in matrix \({\mathbf {R}}_T\) (see Eq. 17).

Based in these ideas, MFs and fuzzy logic rule bases were then developed. They were generated from the meaning of bias and amplitude of oscillations and their relationships with matrices \({\mathbf {Q}}_R\) and \({\mathbf {R}}_T\). In the fuzzy inference process, the metrics were slightly modified. The square root of them was assumed and the result was divided by N, obtaining a root mean square (\(b_{\rm{rms}}=\sqrt{b}/N\), \(a_{\rm{rms}}=\sqrt{a}/N\)). A normalization was also performed such that the input limit of FIS is contained in the range \([0,\ 1]\).

Figure 8 shows the input and output MFs. The normalization enable the use of the same input MF for both kinds of inputs (bias and amplitude of oscillation) and any filter (REKF and TEKF). Concerning the output MFs, recall that they represent the exponents of a multiplicative adaptation (see Eq. 23).

All the MFs are of the Gaussian type. Concerning input MFs, three fuzzy sets were chosen: small (SM), medium (ME) and large (LA). In output MFs, five fuzzy sets were taken: maintain (MA), decrease (DE), increase (IN), hard decrease (HDE) and hard increase (HIN). The range of the exponents is \([-2,\ 2]\).

Fig. 8
figure 8

Input and output membership functions for adapting \({\mathbf {Q}}\) and \({\mathbf {R}}\) based on heuristic metrics

Full size image
Table 1 Fuzzy logic rule base for the FBA–REKF
Full size table

Table 1 presents the fuzzy logic rule base of the REKF. \(\bar{b}_k^i\) is the normalized rms bias of the specific force of the accelerometer i (\(i=x, y, z\)). The definition of \(\bar{a}_k^i\) (amplitude of oscillations) is analogous. \(\mu _k^i\) is the tuning exponent of the (i, i)-th element of the matrix \({\mathbf {Q}}_R\). The exponent \(\mu _k^3\) (which is associated to the measurement of yaw rate) is not generated by the FIS (in the implementation: \(\mu _k^3=(\mu _k^1+\mu _k^2)/2\)).

Table 2 Fuzzy logic rule base for the FBA–TEKF
Full size table

Table 2 presents the fuzzy logic rule base for the TEKF. \(\bar{b}_k^i\) and \(\bar{a}_k^i\) are the normalized rms bias and amplitude of oscillation of the i-th output of the TEKF. \(\mu _k^i\) is the tuning exponent of the (i, i)-th element of the matrix \({\mathbf {R}}_R\). Ranging from 1 to 6, the index i addresses, respectively, the variables \(v_n\), \(v_e\), \(v_d\), \(x_n\), \(y_e\) and H.

The last rule in Table 2 can not be easily inferred from the latter discussion about the properties of the bias and amplitude of oscillations. It was defined for detecting one abnormal behaviour of the sensor, some error that could corrupt both performance measures at the same time.

5 Application and results

In this section, firstly simulation evaluations are described. They concern the rotation and translation filters for both the standard and adaptive versions. Then the hardware implementation evaluations for the rotation filters are presented. The experiments are exclusively carried out in a hand-controlled environment and do not include the flight of the quadrotor itself.

5.1 Simulation evaluations

In order to perform preliminary tests of the performance of the filters, simulations were done.

Through simulation estimation errors can be obtained by comparing the KF estimates to the plant state given by its model. Naturally, this is an approximate approach, since the simulation model is always a simplified representation of the reality, but useful information about the behaviour of estimation errors can be obtained. The simulations can also be used for evaluating dangerous scenarios like fault injection, which is difficult to perform in a flight test.

The simulation model is built from the concepts discussed in Sects. 2 and 3.4. It contains the rotational and translational dynamics, controllers, motor dynamics, sensor errors (bias and random noise), discretization and quantization. For the sake of brevity not all of these models were detailed in Sects. 2 and 3.4.

5.1.1 Simulation parameters

Based on procedures similar to those ones of references [1, 3, 8, 14], the flight mechanics parameters were determined. The goal was to build a simulation model representative of the vehicle being used.

Mass: \(m=1.03\) kg. Moment arm: \(l=0.26\) m. Inertia matrix:

$$\begin{aligned} {\mathbf {I}}=\left[ \begin{array}{ccc} 16.83\times 10^{-3} &{} 0 &{} 5.72\times 10^{-6}\\ 0 &{} 16.38\times 10^{-3} &{} 0 \\ 5.72\times 10^{-6} &{} 0&{} 28.34\times 10^{-3}\end{array}\right] {\rm {kg\,m}}^2 \end{aligned}$$
(29)

Force constant: \(k_f=1.4351\times 10^{-5}\ {\rm {N/(rad/s)}}^2\). Torque constant: \(k_m= 2.4086\times 10^{-7}\ {\rm {N\,m/(rad/s)}}^2\).

5.1.2 Kalman filter parameters

The sampling frequencies of the sensors are crucial for the EKF operation. The IMU sensors (accelerometers and gyroscopes) and barometer operate at 500 Hz. The GPS receiver runs at 5 Hz. The REKF frequency is set at 500 Hz (\(T_{r}=2\) ms), the TEKF works at 167 Hz (\(T_s=6\) ms). These frequencies are replicated in the respective rotational and translational control loops.

The covariance matrices of the rotational and translational filters are, respectively:

REKF: \({\mathbf {R}}_{R}={\rm {diag}}([1\ 1\ 1]\times 10^3)\) \(({\rm {m/s}^2})^2\), \({\mathbf {Q}}_{R}={\rm {diag}}([5\ 5\ 5]\times 10^{-4})\) \({\rm {(rad/s)}}^2\).

TEKF: \({\mathbf {R}}_{T}={\rm {diag}}([1\,({\rm {m/s}})^2\ 1\,({\rm {m/s}})^2\ 100\,({\rm {m/s}})^2\ 10\,{\rm {m}}^2\ 10\,{\rm {m}}^2\ 10\,{\rm {m}}^2])\), \({\mathbf {Q}}_{T}={\rm {diag}}([1\ 1\ 1]\times 10^{-1})\) \(({\rm {m/s}^2})^2\).

When designing the adaptive KFs, the missing design variables are the data window sizes of Sects. 4.1, 4.2 and 4.3, and the normalization factors of Sects. 4.2 and 4.3. The values adopted are:

  • EIAE–EKF: the data window size is \(N=10\), a value suggested in [12]. The same for REKF and TEKF;

  • FIAE–EKF: the data window size is \(N=10\) again. A normalization constant is applied to the ICM with different values for the two filters, namely REKF: \(c_r={\mathbf {R}}_{R}(1,1)\), TEKF: \(c_t={\mathbf {R}}_{T}(1,1)\);

  • FBA–EKF: in the rotation filter (FBA–REKF), the data window size is \(N=100\). The normalization constants for bias and amplitude of oscillations are \(b_{\rm{nr}}=4\,{\rm {m/s}^2}\) and \(a_{\rm{nr}}=1\times 10^{-3}\,{\rm {m/s}^2}\), respectively. In the translation filter (FBA–TEKF), \(N=200\), \(b_{\rm{ntv}}=0.5\,{\rm{m/s}}\) (speeds), \(b_{\rm{nts}}=2\,{\text {m}}\) (displacements), \(a_{\rm{ntv}}=0.25\,{\rm {m/s}}\) (speeds), \(a_{\rm{nts}}=0.25\,{\text {m}}\) (displacements).

5.1.3 Simulation results

The simulation scenario where the filters were evaluated is:

  • Simulation time: 40 s;

  • Step inputs for the references of \(x_n\), \(y_e\) and H with amplitudes \(10\,{\text {m}}\), \(-10\,{\text {m}}\) and \(10\,{\text {m}}\), respectively. The inputs are applied at \(t = 10\,{\text {s}}\);

  • Step input of \(10^{\circ }\) in the reference of \(\psi\) at \(t =\ 0\,{\text {s}}\).

One should note that the simulations are performed in closed loop. Thus, the Kalman filter estimates are fed back for the control laws of attitude and translation.

First, a nominal test is carried out, with all the sensors represented by standard models. Then, a fault is simulated by multiplying by 100 the bias in all gyroscope’s axes. The fault starts at the time 10 s and lasts for 5 s.

The reason for inserting a fault is that the error models are Gaussian. Hence the performance of the standard EKF is optimal and the filter produces the best estimates and the benefits of the adaptive methods cannot be evidenced.

Table 3 presents the AAE (\({\vert e \vert }_{av}\)) in the nominal case. Four tests were done, one for each filter. This table shows that EIAE–EKF worsens all the estimates. FIAE–EKF and FBA–EKF, in turn, have similar behaviours. Their results are also close to the standard EKF. In the REKF, the adaptive estimates are better for the variable \(\theta\), but worst for the variable \(\phi\); the TEKF performance is slightly better or slightly worse, depending on the variable.

Table 3 Estimation errors in the nominal simulation
Full size table
Table 4 Estimation errors in the simulation with gyroscope fault
Full size table

The power of the fuzzy adaptation arises in the simulation with the fault condition. Table 4 presents the AAE (\({\vert e \vert }_{av}\)). Once again, the worst results are given by EIAE–EKF, which generates unstable responses. On the other hand, the remaining filters are able of keeping a stable flight. The best performance is obtained with the fuzzy adaptive filters, being the FBA–EFK generally the best. These filters improve mostly the response of the REKF, which provides the critical information required for the UAV stability.

In order to highlight the behaviour of the FBA–EFK adaptive filter in comparison with the standard EKF, Fig. 9 shows the time responses of the roll and pitch angles \(\phi\) and \(\theta\) generated by them. Note that the FBA–EKF has a smaller estimation error during the fault and provides a faster recovery, returning to the nominal behaviour in a shorter interval of time.

Fig. 9
figure 9

Time responses of the \(\phi\) and \(\theta\) Euler angles during the simulation of the gyroscope fault

Full size image

5.2 Hardware implementation of the rotation filters

This section describes the hardware implementation evaluated in an indoor environment. Only the rotation filters were tested. The tests were performed in open loop with manual maneuvers.

Three filters were tested: standard EKF, adaptive filters FIAE–EKF and FBA–EKF. Each filter was tested separately, but their results were compared with those of a standard quadrotor IMU running in parallel.

5.2.1 Experimental setup

The experimental setup is composed by a host PC and a Gyrofly’s Gyro 200ED quadrotor. The quadrotor has two ARM Cortex-M4 NXP LPC4078 onboard MCs. The first MC contains the core routines of the quadrotor, including automatic flight control laws and the code of an IMU that estimates the Euler angles. This IMU is used to calibrate the magnetometers, the accelerometers and the gyroscopes. The second MC is open for custom routines. It has a basic code infra-structure that configure the hardware, read the IMU data and perform telemetry via Xbee. Additional code can be inserted by the user, this resource was used for programming the filters of interest.

The MC has a clock of 120 MHz, 512 kbytes of flash memory (code storage) and 64 kbytes of RAM. The integrated development environment (IDE) is the LPCXpresso provided by the developer of the MC. A free version of this software was adopted, which allows the use of up to 256 kbytes of flash memory. The interface between the IDE and the MC is provided by a JTAG LPC-LINK.

The real time routines of the MC run at 500 Hz, which means that the filters work at a sample time of 0.002 s. On the other hand, the telemetry is done by an Xbee module at a frequency of 10 Hz.

The same filters of Sect. 5.1 were implemented in Simulink and the MC was programmed in C. The necessary C codes were generated from the Simulink models using the Real-Time Workshop. MATLAB R2010b version 7.11 and respective tools were used.

5.2.2 Filters implementation

By using the Real-Time Workshop, standard EKF was implemented in the hardware exactly as it was in the simulation model.

However, there are several drawbacks when implementing the fuzzy adaptive filters (FIAE and FBA). The problems are associated with the code size (flash memory), runtime variables (RAM) and processing overflow (the code cannot be executed inside the time interval of 0.002 s). These limitations are hardware dependent and can be solved by using more powerful processors. In order to adequate the adaptive filters to the restrictions of the MC in use, the following simplifications were done (compared with the filters of Sect. 5.1):

  • FIAE: (1) Only the elements \(R_{11}\) and \(R_{22}\) of the matrix \({\mathbf {R}}\) of the REKF were adapted; (2) the data window size is reduced from 10 to 4; (3) these changes reduced the additional state variables from 90 (3 × 3 × 10) to 8 (2 × 4). This also reduced the amount of memory required to store parameters of the FIS.

  • FBA: (1) Only the element \(Q_{22}\) of the matrix \({\mathbf {Q}}\) of the REKF (associated with the y-gyroscope) was adapted, thus eliminating one-half of the fuzzy rules and 2/3 of the variables related to bias and oscillation metrics; (2) the data window size was dramatically reduced from 100 to 4; (3) these modifications reduced the additional state variables from 600 (2 × 3 × 100) to 8 (2 × 1 × 4).

The filters were tested according to the following procedure: (1) the code was uploaded to the MC; (2) a test maneuver was performed manually; (3) the data was transmitted to the host PC via Xbee telemetry; (4) the MC was reset and the maneuver was carried out again.

One filter was tested on each turn. It ran in parallel with the quadrotor IMU. Then, the Euler angles were obtained by two different ways in the same test. Because the quadrotor IMU is calibrated by the developer, it is assumed as the reference for performance evaluation.

Figure 10 shows the quadrotor in the indoor laboratory environment. It was suspended by hand, as it were in the test procedures. In fact, during the tests, both hands were used in the movement.

Fig. 10
figure 10

Quadrotor used in the laboratory tests. The attitude maneuvers were driven manually

Full size image

Figure 11 is representative of the maneuver: the attitude started close to zero, then a pitch up was performed, followed by a positive roll. The movement was repeated in the reverse direction, trying to return to zero. Then, a similar trajectory was executed in the opposite sense.

Fig. 11
figure 11

Time responses generated by the adaptive filter FBA–EKF and the quadrotor IMU in a laboratory experiment

Full size image

The data in Fig. 11 were computed by the quadrotor IMU (gray line) and the adaptive filter FBA–EKF (black line). Note that the major differences between the estimates occur in large angles or high rates of change.

In order to evaluate numerically the performance of the filters, 6 runs are summarized in Table 5 for the FIAE–EKF and FBA–EFK adaptive filters, whereas 4 runs are registered for the standard EKF. The last of Table 5 is the average of the above results for each filter. The number of tests reported for the standard EKF is smaller because this filter diverged for some runs. This is a clear indication of the capabilities of the adaptive filters, that can adjust the covariance matrices in order to prevent the divergence.

The first column of Table 5 presents the time duration of each maneuver. Because the movement is performed manually, the time is not the same for all the runs. However, a post processing was done in order to reduce the impact of time in the final results. Because the smaller errors occur with the quadrotor at rest, the data corresponding to the beginning and end of the sequence were removed. Two seconds of rest were kept in each of the boundaries, as it can be seen in Fig. 11.

Table 5 Errors between the implemented filters and the quadrotor IMU
Full size table

The second and third columns of Table 5 show the AAEs of the roll and pitch angles (the reference is the IMU). The fourth and fifth columns display the MAEs. Since the manual movement cannot ensure the same amplitude, the sixth and seventh columns have the maximum displacements of each run, while columns eighth and ninth contain the minimum ones (IMU data). Comparing the data of columns 4 and 5 with those of 6–8 and 7–9, respectively, one could suspect that the variation in the results arose from the maximum amplitude of the maneuvers (systematic errors). To evaluate this possibility, relative errors were determined (columns 10 and 11).

The main objective of these experiments was to compare the performance of the adaptive filters with the standard EKF. The smallest AAEs of Table 5 are given by the FIAE–EKF: comparing the final average results (bold rows), the FIAE–EKF gave AAEs less than one degree bellow the ones of the FBA–EKF. On the other hand, the FBA–EKF produced AAEs slightly smaller than that of the standard EKF.

Comparing the MAEs, the FIAE–EKF, gave again the best results, which were reasonably below the ones given by the FBA–EKF. This filter, by its turn, had the MAE of roll angle slightly larger than the one of the standard EKF, but a reasonably smaller one for the pitch angle.

In what concerns to the relative MAEs, the adaptive filters evidently produced smaller errors than the standard EKF. Comparing both the adaptive filters, the FBA–EKF gave a slightly smaller MAE for the roll angle, while the FIAE–EKF gave a reasonably smaller result for the pitch angle.

In short, the data of Table 5 show that the adaptive filters produced the best estimation results for the experiments considered. Also, the filter FIAE (available on the literature) had a performance reasonably better than the one of the filter FBA (proposed in this paper). In some sense, this was expected because the simplifications involved in the FBA–EKF are stronger than those in the FIAE–EKF.

6 Conclusion

This paper concerns the development of extended Kalman filters (EKFs) for state estimation of an UAV.

The UAV used was a naturally unstable quadrotor type. Hence a closed loop control is required for both stabilization and autonomous flight.

The state estimation procedure generated position, velocity and attitude (PVAT) for feedback control. The sensors used were GPS and low cost microelectromechanical systems (MEMS): accelerometer, gyroscope and barometer.

The filters were tested using hardware implementation and simulation of a quadrotor called Gyro 200 ED.

The standard EKF was improved with adaptive methods. Two innovation adaptive estimation were taken from the literature. The first one involves the replacement of the theoretical matrix of the innovation covariance by another one determined experimentally (EIAE–EKF); the second one involves the adjustment of the output noise covariance matrix of the Kalman filter (KF) via fuzzy logic (FIAE–EKF).

A new method for adaptive estimation was developed and applied to the UAV: heuristic metrics of bias and amplitude of oscillation were taken from the literature and modified in order to provide smaller computational cost. They were combined with fuzzy logic for adjusting the input and output noise covariance matrices of the KF (FBA–EKF).

The adaptive and standard schemes were tested in simulation with the injection of a gyroscope fault. The tests showed that the standard EKF has a good performance in nominal conditions of operation. However, in the fault condition, the best results were determined by the new adaptive filter FBA–EKF.

The method EIAE–EKF did not produce good simulated results. The method FIAE–EKF showed a performance similar to that of the new adaptive filter FBA–EKF.

The hardware implementation was done with the standard EKF, FIAE and FBA. Only the rotational filters were tested, in open loop, with manual maneuvers. The best results were obtained with the adaptive filters, being the FIAE–EKF reasonably better than FBA–EKF.

Strong simplifications were introduced in the FBA–EKF in order to enable its implementation in the microcontroller used. By the same reason, simplifications were introduced in the FIAE–EKF. Even so the adaptive capabilities of these filters could be observed.

The use of adaptive Kalman filter (AKF) on UAVs seems to be promising, because they can improve the performance without the need of more sophisticated sensors, which have higher cost, size, weight and power consumption.

The use of fuzzy AKF is interesting because it can be used for inserting empirical information in the filter, due to the inherent property of fuzzy logic in approximating human reasoning.

Fuzzy AKF is also relevant in comparison with other techniques, like optimization, because it requires relatively lower computational power, being appropriate for small UAVs. However, powerful microcontrollers are required in order to explore the full potential of the fuzzy AKF.