1 Introduction

With the rapid development of the microelectromechanical system (MEMS) technology and advanced control technologies, four-rotor aircraft has become a research hot spot in recent years [1, 2]. Compared with other unmanned aerial vehicles (UAVs), four-rotor aircraft has the advantages of small size, low cost, and good maneuverability. Therefore, it has been widely used in transmission line inspections and aerial photography [3, 4]. For four-rotor aircraft, pose estimation is the basis for autonomous flight and therefore has attracted more and more attentions from researchers and engineers [5,6,7,8,9]. To estimate the attitude of UAV, complementary filtering was firstly applied [5]. However, the calculation accuracy obtained via complementary filtering is rather low. To tackle this issue, the extended Kalman filter (EKF) and unscented Kalman filter (UKF) were employed to posture calculations such that the higher attitude estimation accuracy can be achieved [6, 7]. It is well known that UAV must actually fly within the wind; however, wind interference was not taken into account among the above-mentioned methods. Concerning this problem, a wind parameter estimation algorithm was proposed by exploiting a single-antenna global positioning system (GPS) and tachometer [8]. The authors of [9] developed the nonlinear disturbance observer for UAV to estimate the velocity and acceleration of the wind, which is exploited to address the UAV attitude tracking control issue with the wind field. However, UAV has not been well controlled due to the inaccurate wind speed. In [10], an approach was developed to estimate wind velocity, angle of attack (AOA) and sideslip of angle (SSA) of a fixed-wing UAV only using kinematic relationships with Kalman filter (KF). Unfortunately, the obtained wind speed is not utilized to control UAV. Moreover, an adaptive controller design was considered for the attitude and altitude of UAV quadrotors subjected to wind disturbances [11], howbeit, which is still contaminated by wind field because of some unrealistic assumptions. It should be noted that the algorithm of [8,9,10,11] only investigated wind parameter estimation without involving the influence of wind power on drone control or attitude measurement. However, aircraft is susceptible to wind in application, which will result in a rather high attitude estimation error and then control of the UAV can be effected heavily [12].

Focusing on this issue, an attitude measurement of UAV method is developed in this paper in the wind disturbance environment to enhance the robustness of the attitude estimation such that the UAV control performance can be improved. Considering the wind force as observation, the extended Kalman state equation is established on the basis of the quaternion differential equation and gyro noise error. Moreover, the gyro estimation error can be compensated by using the data collected via accelerometer, magnetometer, GPS, and airspeed meter [13, 14].

2 The solution of attitude matrix

To describe clearly the attitude information such as pitch, yaw, and roll angle, the following two coordinate systems are employed in this paper: the navigation coordinate system \( n \), i.e., the east–north–up (ENU) coordinate system shown in Fig. 1, and \( \psi \), \( \theta \), and \( \phi \) indicate yaw, pitch, and roll angle, respectively, and the body coordinate system \( b\left( {x_{b} ,\,y_{b} ,z_{b} } \right) \) shown in Fig. 2, where \( x_{b} \), \( y_{b} \), and \( z_{b} \), respectively, point right along the horizontal, forward along the longitudinal, and upward along the vertical axis of the body. The origin of the drone is the center of gravity of itself. Due to the fact that the attitude calculation is implemented under the navigation coordinate system, and therefore the attitude information should be mapped from the coordinate system \( b \) to \( n \) through the coordinate transformation matrix \( {\mathbf{c}}_{n}^{b} \in {\mathbb{C}}^{3 \times 3} \) illustrated as [15, 16]:

$$ {\mathbf{c}}_{n}^{b} = \left[ {\begin{array}{*{20}c} {C_{1}^{1} } &\quad {C_{1}^{2} } &\quad {C_{1}^{3} } \\ {C_{2}^{1} } &\quad {C_{2}^{2} } &\quad {C_{2}^{3} } \\ {C_{3}^{1} } &\quad {C_{3}^{2} } &\quad {C_{3}^{3} } \\ \end{array} } \right] $$
(1)

where \( {\mathbf{q}} = [q_{0} \, q_{1} \, q_{2} \, q_{3} ]^{\text{T}} { = }\left[ {q_{0} \, e} \right]^{\text{T}} \) denotes a quaternion, \( q_{0} \) is the scalar part, \( e = \left[ {q_{1} \, q_{2} \, q_{3} } \right] \) is the vector part, and

$$ \begin{aligned} C_{1}^{1} & = q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} ,\;C_{1}^{2} = 2\left( {q_{1} q_{2} - q_{0} q_{3} } \right) \\ C_{1}^{3} & = 2\left( {q_{1} q_{3} + q_{0} q_{2} } \right),\;C_{2}^{1} = 2\left( {q_{1} q_{2} + q_{0} q_{3} } \right) \\ C_{2}^{2} & = q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} ,\;C_{2}^{3} = 2\left( {q_{2} q_{3} + q_{0} q_{1} } \right) \\ C_{3}^{1} & = 2\left( {q_{1} q_{3} - q_{0} q_{2} } \right),\;C_{3}^{2} = 2\left( {q_{2} q_{3} + q_{0} q_{1} } \right) \\ C_{3}^{3} & = q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} \\ \end{aligned}. $$
Fig. 1
figure 1

Navigation coordinate system

Full size image
Fig. 2
figure 2

Body coordinate system

Full size image

The direction cosine form of \( {\mathbf{c}}_{n}^{b} \), i.e., Euler angle form, can be illustrated as:

$$ {\mathbf{c}}_{n}^{b} = \left[ {\begin{array}{*{20}c} {L_{1}^{1} } &\quad {L_{1}^{2} } &\quad {L_{1}^{3} } \\ {L_{2}^{1} } &\quad {L_{2}^{2} } &\quad {L_{2}^{3} } \\ {L_{3}^{1} } &\quad {L_{3}^{2} } &\quad {L_{3}^{3} } \\ \end{array} } \right] $$
(2)

where

$$ \begin{aligned} L_{1}^{1} & = \cos \theta \cos \psi ,\;L_{1}^{2} = \cos \psi \sin \theta \sin \phi - \cos \phi \sin \psi \\ L_{1}^{3} & = \sin \phi \sin \psi + \cos \phi \sin \theta s\cos \psi ,\;L_{2}^{1} = \cos \theta \sin \psi \\ L_{2}^{2} & = \cos \phi \cos \psi + \sin \phi \sin \theta \sin \psi \\ L_{3}^{1} & = - \sin \theta ,\;L_{3}^{2} = \sin \phi \cos \theta \\ L_{3}^{3} & = \cos \phi \cos \theta ,\;L_{2}^{3} = \cos \phi \sin \theta \sin \psi - \sin \phi \cos \psi \\ \end{aligned} $$

in which \( \psi \), \( \theta \), and \( \phi \) indicate yaw, pitch, and roll angle, respectively. Comparing (1) with (2), the quaternion of the attitude angle can be concluded as follows [17]:

$$ \begin{aligned} \psi & = \arctan \left( {\frac{{2\left( {q_{1} q_{2} + q_{0} q_{3} } \right)}}{{q_{1}^{2} + q_{0}^{2} - q_{2}^{2} - q_{3}^{2} }}} \right) \\ \theta & = - \arcsin \left( {2\left( {q_{1} q_{3} - q_{0} q_{2} } \right)} \right) \\ \phi & = \arctan \left( {\frac{{2\left( {q_{2} q_{3} + q_{0} q_{1} } \right)}}{{ - q_{1}^{2} + q_{0}^{2} - q_{2}^{2} + q_{3}^{2} }}} \right) \\ \end{aligned} $$
(3)

where \( \arctan \,( \cdot ) \) and \( \arcsin \,( \cdot ) \) denote arctangent and arcsine function, respectively.

With the discussion above, the quaternion representation of the attitude angle can be obtained. By using this formulation, the extended Kalman state equation under wind disturbance condition can be formulated in the following section.

3 The proposed algorithm

3.1 Kalman filtering state equation of attitude calculation

According to the description in [18], the Kalman filter prediction equation can be written as:

$$ {\mathbf{x}}_{k} = \varPhi_{k,k - 1} {\mathbf{x}}_{k - 1} + {\mathbf{w}}_{k - 1} $$
(4)

where \( {\mathbf{x}}_{k} \in {\mathbb{C}}^{4 \times 1} \) denotes the state vector estimation matrix at time \( k \), \( \varPhi_{k,k - 1} \in {\mathbb{C}}^{4 \times 4} \) is one-step transfer matrix from time \( k - 1 \) to \( k \) and \( {\mathbf{w}}_{k - 1} \in {\mathbb{C}}^{4 \times 1} \) is the system excitation noise sequence.

Since the state variable is a quaternion, by using the quaternion differential formulation, the state equation can be depicted as [19]:

$$ {\dot{\mathbf{q}}}(t) = \frac{1}{2}\left[ {\begin{array}{*{20}c} 0 & { - \omega_{x} } & { - \omega_{y} } & { - \omega_{z} } \\ {\omega_{x} } & 0 & {\omega_{z} } & { - \omega_{y} } \\ {\omega_{y} } & { - \omega_{x} } & 0 & {\omega_{x} } \\ {\omega_{z} } & {\omega_{y} } & { - \omega_{x} } & 0 \\ \end{array} } \right]{\mathbf{q}}(t) $$
(5)

where \( {\dot{\mathbf{q}}}(t){ = }\left[ {q_{0} (t) \, q_{1} (t) \, q_{2} (t) \, q_{3} (t)} \right]^{\text{T}} \) stands for the state estimate of the quaternion form at time \( t \). \( \omega_{x} \), \( \omega_{y} \), and \( \omega_{z} \) are, respectively, the angular velocity of the body frame with respect to navigation frame along with the x-, y-, and z-axis, respectively.

Because the system state equation shown in (5) is continuous, it is not easy to be tackled by using the discrete methods. Concerning this, the Picca approximation [6] can be exploited to discretize (5), which can be shown as:

$$ {\mathbf{q}}(k) = \varPhi_{k,k - 1} {\mathbf{q}}(k - 1) $$
(6)

where \( {\mathbf{q}}(k) = [\begin{array}{*{20}c} {q_{0} (k)} & {q_{1} (k)} & {q_{2} (k)} & {q_{3} (k)} \\ \end{array} ]^{\text{T}} \) is the state estimate of the quaternion form at time \( k \), and

$$ \varPhi_{k,k - 1} = \left[ {\begin{array}{*{20}c} {\varPhi_{1}^{1} } &\quad {\varPhi_{1}^{2} } &\quad {\varPhi_{1}^{3} } &\quad {\varPhi_{1}^{4} } \\ {\varPhi_{2}^{1} } &\quad {\varPhi_{2}^{2} } &\quad {\varPhi_{2}^{3} } &\quad {\varPhi_{2}^{4} } \\ {\varPhi_{3}^{1} } &\quad {\varPhi_{3}^{2} } &\quad {\varPhi_{3}^{3} } &\quad {\varPhi_{3}^{4} } \\ {\varPhi_{4}^{1} } &\quad {\varPhi_{4}^{2} } &\quad {\varPhi_{4}^{3} } &\quad {\varPhi_{4}^{4} } \\ \end{array} } \right] $$
$$ \begin{aligned} \varPhi_{1}^{1} & = 1 - \frac{{\Delta \theta_{0}^{2} }}{8} + \frac{{\Delta \theta_{0}^{4} }}{384},\;\varPhi_{1}^{2} = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{x} \\ \varPhi_{1}^{3} & = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{y} ,\;\varPhi_{1}^{4} = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{z} \\ \varPhi_{2}^{1} & = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{x} ,\;\varPhi_{2}^{2} = 1 - \frac{{\Delta \theta_{0}^{2} }}{8} + \frac{{\Delta \theta_{0}^{4} }}{384} \\ \varPhi_{2}^{3} & = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{z} ,\;\varPhi_{2}^{4} = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{y} \\ \varPhi_{3}^{1} & = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{y} ,\;\varPhi_{3}^{2} = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{z} \\ \varPhi_{3}^{3} & = 1 - \frac{{\Delta \theta_{0}^{2} }}{8} + \frac{{\Delta \theta_{0}^{4} }}{384},\;\varPhi_{3}^{4} = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{x} \\ \varPhi_{4}^{1} & = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{z} ,\;\varPhi_{4}^{2} = \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{y} \\ \varPhi_{4}^{3} & = - \left( {\frac{1}{2} - \frac{{\Delta \theta_{0}^{2} }}{48}} \right)\Delta \theta_{x} ,\;\varPhi_{4}^{4} = 1 - \frac{{\Delta \theta_{0}^{2} }}{8} + \frac{{\Delta \theta_{0}^{4} }}{384} \\ \end{aligned} $$
$$ \begin{aligned} \Delta \theta_{x} & = \int_{k}^{k + 1} {\omega_{x} \,{\text{d}}t} ,\;\Delta \theta_{y} = \int_{k}^{k + 1} {\omega_{y} \,{\text{d}}t} \\ \Delta \theta_{z} & = \int_{k}^{k + 1} {\omega_{z} \,{\text{d}}t} ,\;\Delta \theta_{0}^{2} = \Delta \theta_{x}^{2} + \Delta \theta_{y}^{2} + \Delta \theta_{z}^{2} \\ \end{aligned}. $$

3.2 The observation equation in the presence of wind interference

In the wind disturbance environment, observations consist of the following three measurements: accelerometer, magnetometer, and the wind power. With these measurements, the Kalman observation equation can be described as:

$$ {\mathbf{z}}(k) = {\mathbf{h}}_{k} {\mathbf{x}}(k) + {\mathbf{v}}(k) $$
(7)

where z(k) denotes measured value matrix at time \( k \), \( {\mathbf{h}}_{k} \) is measurement matrix, x(k) is the state vector and v(k) indicates the measurement noise.

Firstly, accelerometer and magnetometer are considered. With the reference coordinate system, the gravity and geomagnetic vector can be defined as \( {\mathbf{g}} = \left[ {\begin{array}{*{20}c} 0 & 0 & 1 \\ \end{array} } \right]^{\text{T}} \) and \( {\mathbf{h}}_{0} = \left[ {\begin{array}{*{20}c} {h_{x} } & {h_{y} } & {h_{z} } \\ \end{array} } \right]^{\text{T}} \), respectively [20]. The matrix forms of them can be written as:

$$ \left[ {\begin{array}{*{20}c} {g_{x} } \\ {g_{y} } \\ {g_{z} } \\ \end{array} } \right] = {\mathbf{c}}_{n}^{b} \left[ {\begin{array}{*{20}c} 0 \\ 0 \\ 1 \\ \end{array} } \right] $$
(8)
$$ \left[ {\begin{array}{*{20}c} {m_{x} } \\ {m_{y} } \\ {m_{z} } \\ \end{array} } \right] = {\mathbf{c}}_{n}^{b} \left[ {\begin{array}{*{20}c} {h_{x} } \\ {h_{y} } \\ {h_{z} } \\ \end{array} } \right] $$
(9)

where \( g \) and \( m \) are measurements of accelerometer and the magnetometer in the body coordinate system \( b \), respectively.

With Eq. (2), by using the Jacobian matrix, (8) and (9) can be reformulated as:

$$ \begin{aligned} g_{x} & = \text{2}\left( {q_{1} q_{3} - q_{0} q_{2} } \right) \, \\ g_{y} & = \text{2}\left( {q_{2} q_{3} + q_{0} q_{1} } \right) \, \\ g_{z} & = q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} \\ \end{aligned} $$
(10)
$$ \begin{aligned} m_{x} & = m_{x1} + m_{x2} + m_{x3} \\ m_{y} & = m_{y1} + m_{y2} + m_{y3} \\ m_{z} & = m_{z1} + m_{z2} + m_{z3} \\ \end{aligned} $$
(11)

where

$$ \begin{aligned} m_{x1} & = \left( {q_{0}^{2} + q_{0}^{2} - q_{0}^{2} - q_{0}^{2} } \right)h_{x} ,\;m_{x2} = 2\left( {q_{1} q_{1} + q_{1} q_{1} } \right)h_{y} \\ m_{x3} & = 2\left( {q_{1} q_{3} - q_{0} q_{2} } \right)h_{z} ,\;m_{y1} = 2\left( {q_{1} q_{2} - q_{0} q_{3} } \right)h_{x} \\ m_{y2} & = \left( {q_{0}^{2} + q_{0}^{2} - q_{0}^{2} - q_{0}^{2} } \right)h_{y} ,\;m_{y3} = 2\left( {q_{0} q_{1} + q_{2} q_{3} } \right)h_{z} \\ m_{z1} & = 2\left( {q_{1} q_{3} + q_{0} q_{2} } \right)h_{x} ,\;m_{z2} = 2\left( {q_{2} q_{3} - q_{0} q_{1} } \right)h_{y} \\ m_{z3} & = \left( {q_{0}^{2} - q_{0}^{2} - q_{0}^{2} + q_{0}^{2} } \right)h_{z} \\ \end{aligned}. $$

With the discussion above, the quaternion representation of the accelerometer and magnetometric measurements can be obtained. The third measurement, i.e., the wind power, is analyzed as follows.

3.3 Wind observation equation

3.3.1 Air flow coordinate system

The air flow can be represented by the airspeed vector \( {\mathbf{V}}_{T} \) with an amplitude of \( A_{T} \). Its direction is defined by the two angles relative to the body, i.e., the attack angle \( \alpha \) and the side slip angle \( \beta \), which are, respectively, illustrated as [21]:

$$ A_{T} = \sqrt {u_{T}^{2} + v_{T}^{2} + w_{T}^{2} } $$
(12)
$$ \alpha = \arctan \left( {\frac{{w_{T} }}{{u_{T} }}} \right) $$
(13)
$$ \beta = \arcsin \left( {\frac{{v_{T} }}{{V_{T} }}} \right) $$
(14)

where \( u_{T} \), \( v_{T} \), and \( w_{T} \) are the three-axis components of airspeed in the body coordinate system \( b \). \( V_{T} \) denotes the value of airspeed.

Let \( {\mathbf{c}}_{b}^{w} \in {\mathbb{C}}^{3 \times 3} \) be the rotation matrix from the body coordinate system \( b \) to the airflow coordinate system \( w \), due to the fact that \( {\mathbf{c}}_{b}^{w} = ({\mathbf{c}}_{b}^{w} )^{ - 1} = ({\mathbf{c}}_{b}^{w} )^{\text{T}} \), and then, we have [21]:

$$ {\mathbf{a}}^{w} = {\mathbf{c}}_{b}^{w} {\mathbf{a}}^{b} $$
(15)

where the vector \( {\mathbf{a}} \) is, respectively, represented as aw and ab in the navigation coordinate system \( w \) and body coordinate system \( b \).

Similar to that in [21], the rotation matrix \( {\mathbf{c}}_{b}^{w} \) can be expressed as:

$$ \begin{aligned} {\mathbf{c}}_{b}^{w} & = \left[ {\begin{array}{*{20}c} {\cos \beta } & {\sin \beta } & 0 \\ { - \sin \beta } & {\cos \beta } & 0 \\ 0 & 0 & 1 \\ \end{array} } \right]\left[ {\begin{array}{*{20}c} {\cos \alpha } & 0 & {\sin \alpha } \\ 0 & 1 & 0 \\ { - \sin \alpha } & 0 & {\cos \alpha } \\ \end{array} } \right] \\ & = \left[ {\begin{array}{*{20}c} {\cos \alpha \cos \beta } & {\sin \beta } & {\sin \alpha \cos \beta } \\ { - \sin \beta \cos \alpha } & {\cos \beta } & { - \sin \alpha \sin \beta } \\ { - \sin \alpha } & 0 & {\cos \alpha } \\ \end{array} } \right] \\ \end{aligned}. $$
(16)

Replacing \( {\mathbf{a}} \) with \( {\mathbf{V}}_{T}^{{}} \), Eq. (15) can be rewritten as:

$$ {\mathbf{V}}_{T}^{w} = {\mathbf{c}}_{b}^{w} {\mathbf{V}}_{T}^{b} $$
(17)

where \( {\mathbf{V}}_{T}^{{}} \) denotes the airspeed vector and \( {\mathbf{c}}_{b}^{w} \in {\mathbb{C}}^{3 \times 3} \) stands for the rotation matrix from the body coordinate system \( b \) to the airflow coordinate system \( w \).

3.3.2 Wind disturbance

The inertia speed \( {\mathbf{V}} \) of the aircraft is the sum of airspeed \( {\mathbf{V}}_{T} \) and wind speed \( {\mathbf{W}} \) [22], which can be formulated as:

$$ {\mathbf{V}} = {\mathbf{V}}_{T} + {\mathbf{W}}. $$
(18)

By defining the disturbance wind as \( {\mathbf{W}}^{n} \) in the navigation coordinate system \( n \), the speed in the body coordinate system \( b \) can be expressed as:

$$ {\mathbf{V}}^{b} = {\mathbf{V}}_{T}^{b} + {\mathbf{c}}_{n}^{b} {\mathbf{W}}^{n} $$
(19)

which can be rewritten as:

$$ {\mathbf{c}}_{n}^{b} {\mathbf{W}}^{n} = {\mathbf{V}}^{b} - {\mathbf{V}}_{T}^{b} $$
(20)

where \( {\mathbf{W}}^{n} { = }\left[ {W_{N} \, W_{E} \, W_{D} } \right]^{\text{T}} \), \( {\mathbf{V}}_{T}^{b} { = }\left[ {u_{T} \, v_{T} \, w_{T} } \right]^{\text{T}} \), and \( {\mathbf{V}}^{b} { = }\left[ {u \, v \, w} \right]^{\text{T}} \). In the coordinate system n, the matrix form of (20) can be depicted as:

$$ \left[ {\begin{array}{*{20}c} {u_{T} } \\ {v_{T} } \\ {w_{T} } \\ \end{array} } \right] = \left[ \begin{aligned} u \hfill \\ v \hfill \\ w \hfill \\ \end{aligned} \right] - {\mathbf{c}}_{n}^{b} \left[ \begin{aligned} W_{N} \hfill \\ W_{E} \hfill \\ W_{D} \hfill \\ \end{aligned} \right] $$
(21)

where \( W_{N} \), \( W_{E} \), and \( W_{D} \), respectively, denote wind speed with respect to navigation frame along with N-, E-, and D-axis. \( u_{T} \), \( v_{T} \), \( w_{T} \) and \( u \), \( v \), \( w \) indicate air and ground speed with respect to body coordinate system along with the x-, y-, and z-axis, respectively.

With the observation shown above, it is obvious that the quaternion is a nonlinear function with respect to measurement of accelerometer, magnetometer, and the wind force. Therefore, it is difficult to be solved. Focusing on this issue, it can be linearized by using the Jacobi matrix [23]. With (10), (11), and (15), \( {\mathbf{h}}_{k} \in {\mathbb{C}}^{9 \times 4} \) can be expressed as:

$$ {\mathbf{h}}_{k} = \left[ {\begin{array}{*{20}c} { - 2q_{2} } &\quad {2q_{3} } &\quad { - 2q_{0} } &\quad {2q_{1} } \\ {2q_{1} } &\quad {2q_{0} } &\quad {2q_{3} } &\quad {2q_{2} } \\ {2q_{0} } &\quad { - 2q_{1} } &\quad { - 2q_{2} } &\quad {2q_{3} } \\ {H_{1} } &\quad {H_{2} } &\quad {H_{3} } &\quad {H_{4} } \\ {H_{5} } &\quad {H_{6} } &\quad {H_{7} } &\quad {H_{8} } \\ {H_{9} } &\quad {H_{x} } &\quad {H_{y} } &\quad {H_{z} } \\ {2q_{3} } &\quad { - 2q_{2} } &\quad { - 2q_{1} } &\quad {2q_{0} } \\ { - 2q_{0} } &\quad {2q_{1} } &\quad { - 2q_{2} } &\quad {2q_{3} } \\ { - 2q_{1} } &\quad { - 2q_{0} } &\quad { - 2q_{2} } &\quad { - 2q_{3} } \\ \end{array} } \right] $$
(22)

where

$$ \begin{aligned} H_{1} & = 2\left( {q_{0} h_{x} + q_{3} h_{y} - q_{2} h_{z} } \right),\;H_{2} = 2\left( {q_{1} h_{x} + q_{2} h_{y} + q_{3} h_{z} } \right) \\ H_{3} & = 2\left( {q_{1} h_{x} - q_{2} h_{y} - q_{0} h_{z} } \right),\;H_{4} = 2\left( {q_{0} h_{x} - q_{3} h_{y} + q_{1} h_{z} } \right) \\ H_{5} & = 2\left( {q_{0} h_{x} - q_{3} h_{y} + q_{1} h_{z} } \right),\;H_{6} = 2\left( {q_{2} h_{x} - q_{1} h_{y} + q_{0} h_{z} } \right) \\ H_{7} & = 2\left( {qh_{x} + q_{2} h_{y} + q_{3} h_{z} } \right),\;H_{8} = 2\left( {q_{2} h_{x} - q_{0} h_{y} - q_{3} h_{z} } \right) \\ H_{9} & = 2\left( {q_{2} h_{x} - q_{1} h_{y} + q_{0} h_{z} } \right),\;H_{x} = 2\left( {q_{3} h_{x} - q_{0} h_{y} - q_{1} h_{z} } \right) \\ H_{y} & = 2\left( {q_{0} h_{x} + q_{3} h_{y} - q_{2} h_{z} } \right),\;H_{z} = 2\left( {q_{1} h_{x} + q_{2} h_{y} + q_{3} h_{z} } \right) \\ \end{aligned}. $$

3.4 Attitude calculation based on the extended Kalman filtering

Based on the above-mentioned Kalman prediction Eq. (6), observation Eq. (7), and the measurement matrix with wind disturbance \( {\mathbf{h}}_{k} \) shown in (22), the EKF can be exploited to acquire the attitude angle, in which prediction and update process are shown in Fig. 3.

Fig. 3
figure 3

EKF flowchart

Full size image
  1. (1)

    Prediction step:

State one-step prediction:

$$ \varvec{X}\left( {k |k - 1} \right) = {\varvec{\Phi}}_{k |k - 1} \varvec{X}\left( {k - 1} \right) $$
(23)

where \( \hat{\varvec{X}}(k |k - 1) \) is the prediction of the state variable \( \hat{\varvec{X}}(k) \) and \( {\varvec{\Phi}}_{k/k - 1} \) is the state one-step transfer matrix.

One-step prediction covariance matrix:

$$ \varvec{P}(k |k - 1) = {\varvec{\Phi}}_{k |k - 1} \varvec{P}(k - 1){\varvec{\Phi}}_{k |k - 1}^{\text{T}} + \varvec{Q} $$
(24)

where \( \varvec{P}(k |k - 1) \) is the prediction of the state covariance matrix \( \varvec{P}(k - 1) \), \( \varvec{Q} \) is the system noise covariance matrix, and \( {\varvec{\Phi}}_{k |k - 1} \) is the state one-step transfer matrix.

  1. (2)

    Correction step:

Kalman gain update:

$$ \varvec{K}(k) = \varvec{P}(k |k - 1){\mathbf{h}}_{k}^{\text{T}} \left( {{\mathbf{h}}_{\varvec{k}} \varvec{P}(k - 1){\mathbf{h}}_{k}^{\text{T}} + \varvec{R}} \right)^{ - 1} $$
(25)

where \( \varvec{R} \) is the observed noise covariance matrix.

Status update:

$$ \hat{\varvec{X}}(k) = \hat{\varvec{X}}(k |k - 1) + \varvec{K}(k)\left( {\varvec{Z}(k) - h_{k} \hat{\varvec{X}}(k |k - 1)} \right) $$
(26)

where \( \varvec{Z}(k) \) denotes the measurement matrix at time \( k \).

Status covariance matrix update:

$$ \varvec{P}(k) = \left( {\varvec{I} - \varvec{K}(k)h_{k} } \right)\varvec{P}(k - 1) $$
(27)

where \( \varvec{I} \) denotes the identity matrix.

EKF predicts the status of the update process. Initial value \( \hat{\varvec{X}}(0) \) is given by accelerometer, airspeed tube and magnetometer. The initial value of covariance matrix \( {\text{P(0)}} \) can be selected as an identity matrix. In this process, it is assumed that both the system and observed noise are white, and their covariance matrix can be, respectively, denoted as \( {\mathbf{\mathcal{Q}}} \) and \( \varvec{R} \). Based on the obtained \( {\varvec{\Phi}}_{k,k - 1} \) and \( {\mathbf{h}}_{k} \) with (6) and (22), respectively, one can acquire the UAV attitude estimation via (23)–(27).

4 Numerical simulations

In this section, the effectiveness of the developed algorithm can be verified with the data collected via the following sensors: gyroscope, magnetometer, GPS, and airspeed tube. It is noted that all numerical examples are implemented via MATLAB 2014b version on a laptop with CPU 1.8 GHz Intel Core(TM) i5 and 8 GB RAM.

The basic simulation parameters can be illustrated as follows:

(a) Both the attitude estimation frequency and sample frequency of MPU9150 are 200HZ.

(b) According to the geomagnetism in Dalian, the reference geomagnetism vector is:

$$ {\mathbf{m}}_{0} = \left[ {\begin{array}{*{20}c} {0.263} & {3.29} & {3.38} \\ \end{array} } \right]^{\text{T}} \times 10^{ - 5} $$
(28)

(c) With the platform being horizontal, the reference acceleration vector can be determined as:

$$ \varvec{a}_{0} = \left[ {0{ 0 1}} \right]^{\text{T}} $$
(29)

(d) The initials conditions of Kalman filter are:

The initial value of the quaternion can generally be depicted as:

$$ {\mathbf{q}}_{0} = \left[ {\begin{array}{*{20}c} 1 & 0 & 0 & 0 \\ \end{array} } \right]^{\text{T}}. $$
(30)

It is well known that the error covariance indicates the confidence of prediction state at present. The smaller it is, the more we believe the current prediction state. Moreover, the error covariance matrix determines the initial convergence speed, and then, a smaller value is generally set to facilitate the acquisition. Consequently, the initial value of error covariance matrix can be expressed as:

$$ {\mathbf{p}}_{0} = {\text{diag}}\left[ {\begin{array}{*{20}c} {0.2846} & {0.2846} & {0.2846} & {0.2846} \\ \end{array} } \right]^{\text{T}} $$
(31)

where \( {\text{diag}}\left( \cdot \right) \) denotes the diagonal matrix.

The smaller the value of \( \varvec{Q} \), the easier the system will converge; however, it will easily diverge if it is too small. Therefore, initial value of \( \varvec{Q} \) can be given as:

$$ {\mathbf{Q}} = {\text{diag}}(0.02\; 0. 0 2\; 0. 0 2\; 0. 0 2). $$
(32)

As for measurement noise, if it is too large, the Kalman filter response will be slower because the confidence in the newly measured value is reduced. Conversely, the smaller the value is, the faster convergence speed of the system is. However, it is easy to oscillate if it is too small. Considering this, the initial value of \( \varvec{R} \) can be installed as:

$$ \varvec{R} = {\text{diag}}(10,\;10,\;10,\;20,\;20,\;20). $$
(33)

(e) The initials values of wind force are:

The attack angle is \( \alpha = 5^{ \circ } \) and the side slip angle is \( \beta = 10^{ \circ } \). With the reference coordinate system, the wind vector can be defined as:

$$ {\mathbf{V}}^{b} { = }\left[ {u \, v \, w} \right]^{\text{T}} = \left[ {0 \, \text{1 0}} \right]^{\text{T}}. $$
(34)

To verify the effect of wind interference on the attitude estimation, Figs. 4 and 5 illustrate the attitude angle estimation error versus number of samples in the following three conditions: the absence of wind, considering wind and without considering wind in the presence of wind with its speed being, respectively, 5 m/s and 8 m/s. Obviously, it can be seen from Figs. 4 and 5 that the attitude angle estimation error in the absence of wind is smallest among these three cases, and the estimation error achieved by the proposed method is larger than that in the absence of wind. It can also be observed that the estimation error of roll, pitch, and yaw is, respectively, 1.3, 1.2, and 1, with its speed being 5 m/s and 1.9, 2.2, and 2.1 with speed being 8 m/s considering wind interference in the presence of wind. Besides, regardless of any condition, the attitude angle estimation error with wind speed being 8 m/s is larger than that of 5 m/s, which implies more wind power makes more attitude estimation error. Moreover, the angle estimation error with speed of 8 m/s is larger than that of 5 m/s. The above mentioned is attributed to the fact that the larger the wind force, the greater the disturbance to the UAV, and therefore, the greater the error, while there is no wind outside, the disturbance to UAV is almost zero, so the error is small. Furthermore, the estimation error without considering wind power in the presence of wind is largest, which indicates that the developed method can alleviate the effect of wind interference on the attitude estimation, and hence, the robustness of the flight control of UAV can be improved. In addition, the attitude estimation error can converge at the number of sample being 50 in above-mentioned three cases with various wind powers, which implies that the proposed method has a rather fast convergence speed. From the discussion above, it can be seen that the proposed algorithm can depress the divergence of attitude angle obviously, shorten the attitude convergence time sharply, upgrade the attitude measurement accuracy considerably, and therefore improve the performance of the UAV control effectively.

Fig. 4
figure 4

Attitude angle estimation error with wind speed being 5 m/s. a Roll angle estimation error, b pitch angle estimation error, c yaw angle estimation error

Full size image
Fig. 5
figure 5

Attitude angle estimation error with wind speed being 8 m/s. a Roll angle estimation error, b pitch angle estimation error, c yaw angle estimation error

Full size image

To testify the superiority of the proposed algorithm on the attitude estimation, the attitude angle estimation error acquired via the developed approach and EKF [6] as well as UKF [7] in the presence of wind with its speed being 5 m/s, versus number of samples, is illustrated in Fig. 6. Obviously, it can be seen from Fig. 6 that the attitude angle estimation error of the proposed algorithm has the smallest error in comparison with the EKF and UKF methods. Moreover, the attitude angle error fluctuation of the developed method proposed algorithm is also lowest. Consequently, it can be concluded that the proposed algorithm can obtain a more accurate and robust attitude angle estimation than that of EKF and UKF.

Fig. 6
figure 6

Attitude angle estimation error of three methods (EKF, UKF, the proposed method) with wind speed being 5 m/s. a Roll angle estimation error, b pitch angle estimation error, c yaw angle estimation error

Full size image

Remark Based on the above conducted experiments, one can see that the angle estimation error is reduced significantly by the developed algorithm in comparison with state-of-the-art algorithms under various wind conditions: the absence of wind, considering wind and without considering wind in the presence of wind with its speed being, respectively, 5 m/s and 8 m/s. Consequently, the proposed method can achieve more effective UAV control.

5 Conclusion

Focusing on the issue that the existence of wind interference has influence on the attitude measurement of UAV has been investigated via including the wind interference into the Kalman filter observation equation. Numerical simulations show that the proposed method can obtain the lowest attitude measurement error in comparison with EKF and UKF methods in the presence of wind interference, which can improve the attitude angles accuracy obviously, and the performance of UAV control can be improved significantly. However, the proposed algorithm has its drawbacks, since the gyroscope model we used is an empirical model, which might result in the estimate error. And we are going to study more accurate models in our future works.