A fuzzy adaptive complementary filter for attitude estimation based on norm judgment

Abstract

Unmanned aerial vehicle (UAV) has great application prospect because of its capability of three-dimensional space operation. The reliability of attitude and heading reference system (AHRS) for UAV attitude estimation is crucial for the application of UAV. When a UAV is subjected to unknown electromagnetic interference and force interference in flight, its attitude detection system can suffer from reduced accuracy or even failure. In this paper, a fuzzy adaptive complementary filter (CF) for attitude estimation based on norm judgment is proposed to solve the problem that the sensor is easily disturbed by the complex flight environment and the fixed filter parameters are difficult to obtain the UAV attitude accurately under different flight states. Firstly, the correction model of the gyroscope, which includes four filter parameters, namely accelerometer weight, magnetometer weight, proportional gain P and integral gain I, is established. Secondly, the influence of the four parameters on the estimation accuracy is analyzed. Finally, the adaptive adjustment rules are designed to adjust the filter parameters online and thus achieve the accurate and reliable measurement of UAV attitude. The feasibility of the proposed algorithm is verified through static, dynamic and interference experiments with the specially designed AHRS test platform. And the results show that the attitude estimation algorithm designed in this paper can ensure the high accuracy of the system in both steady state and high-speed rotation, shield the pitch and roll angles from the acceleration of motion, and keep the yaw angle from the external magnetic field.

Introduction

Compared with other unmanned equipment, UAV can upgrade the working space from two-dimension to three-dimension, so the UAV technology and its application have developed rapidly in recent years. ¹ However, at present, most of UAVs are mini-micro and mainly aimed at the consumer market while few of them are used in professional fields, such as material transportation, high-altitude fire protection, power line inspection and so on. The main reason is that the flight reliability, which is straight affected by the accuracy and the stability of AHRS, is difficult to meet the requirements.

AHRS is a multi-sensor system including Micro-Electro-Mechanical System gyroscope, accelerometer and magnetometer. ² In the real flight, UAV is affected by the following factors. (1) The poor low frequency characteristics and serious temperature drift of gyroscope tend to accumulate the error quickly when the UAV is in low speed or steady state. (2) Although the magnetometer and accelerometer have good low-frequency characteristics, the measured value of the former is easily disturbed by the surrounding magnetic field, and the rotation angle around the geomagnetic line cannot be detect. Besides, the measurement accuracy of the latter is only effective in steady state and the rotation angle around the gravitational acceleration direction cannot be detect. The factors mentioned above make it difficult to give accurate attitude information of UAV because of its complicated dynamic characteristics. To improve the accuracy of AHRS, many researchers adopt the method of fusing the signals of gyroscope, accelerometer and magnetometer, in which Kalman Filter (KF)^3–5 and Complementary Filter are the two most popular sensor fusion schemes. Besides, the neural network-based pose estimation algorithm ⁶ is also a future-oriented development in the field of pose estimation, but since it requires more computational resources and a richer dataset, ⁷ it needs further development of microcontroller hardware and dataset before it can be practically and effectively applied.

However, KF requires that the system is linear and the noise is white noise, which is difficult to realize in many cases. To deal with the large amount of colored noise in sensors, many scholars have developed various extended and derived algorithms based on Kalman filter algorithm. Sabatelli et al. ⁸ designed a novel two-stage filter based on extended Kalman filter (EKF). Accelerometer data are used in the first stage, and Magnetometer data for angular position correction, which effectively improves the flexibility of calculation and robustness to magnetic anomaly, are used in the second stage. However, the accuracy of EKF is limited because it approximate nonlinear function by linear ways. For nonlinear systems, unscented Kalman filter (UKF) has more advantages. ⁹ Simulation results show that UKF has faster convergence speed, higher filtering accuracy and more stable estimation performance when compared with EKF. The algorithm based on Kalman filter requires UAV to have stable flight state, which is difficult to achieve, resulting in poor long-term reliability in practical application. In view of the defects of Kalman filter used in attitude estimation, Mahony et al. ¹⁰ proposed an explicit complementary filter (ECF) for UAV attitude estimation. In this method, the accelerometer was used to compensate the gyro drift, and then the compensated angular velocity was integrated to obtain the attitude angle. The algorithm has good performance on roll angle and pitch angle estimation, but the yaw angle correction is low.

Therefore, on the basis of Inertial measurement unit (IMU) compensation filter, Sheng and Zhang ¹¹ introduced magnetic field sensor for yaw angle calibration, gave a complementary filtering scheme in the form of direction cosine, then established a complete AHRS three-dimensional attitude measurement system, and finally acquired good feedback. achieved good results. In essence, these algorithms used the low-frequency characteristics of accelerometer and magnetometer to compensate the cumulative error of gyroscope, but the fixed filter coefficient in the algorithm is difficult to ensure that the system has high accuracy both in high-speed and low-speed situations. To address this issue, Poddar et al. ¹² designed an adaptive complementary filtering algorithm (ACF) based on the Eulerian angle description, which solves the problem that the complementary filtering algorithm is difficult to obtain the optimal motion attitude angle in multiple motion situations due to fixed parameters; and Yan-Jun et al. ¹³ verified the relative superiority of the algorithm in filtering accuracy in static and dynamic filtering effect experiments. However, the method does not address the problems of external non-geomagnetic magnetic field interference and motion acceleration interference.

In light of the above research, a norm-based fuzzy adaptive complementary filter attitude fusion algorithm is proposed to solve the long-term reliability problem of AHRS. Compared with the shortcomings of traditional complementary filtering, that is, it is difficult to meet the pose estimation in all states when the complementary parameters of the filter PI controller are fixed, the introduction of fuzzy control algorithm can fuzzy adaptive adjustment of PI parameters for the characteristics of different motion states to achieve adaptive complementary filtering and improve the adaptability and reliability of the algorithm. In this paper, we innovatively propose to incorporate the real-time evaluation criteria of the reliability of accelerometers and magnetometers (mainly the judgment of the parity of the measured values of the two sensors) into the fuzzy control rules of the attitude estimation algorithm in the case of acceleration disturbances caused by unknown forces and external non-geomagnetic magnetic field disturbances to ensure the reliability and accuracy of the attitude estimation algorithm in complex flight environments. The basic idea is to firstly adaptively adjust the accelerometer weight according to the accelerometer measurement value norm to deal with the influence of UAV’s unsteady motion on the accelerometer information; then, adaptively adjust the magnetometer weight according to the magnetometer measurement value norm to deal with the interference of the external non geomagnetic magnetic field on the magnetometer; lastly, the PI coefficients are adjusted adaptively to make the filter have appropriate cut-off frequency and damping coefficient in different motion states of UAV, according to the measurement value norm of the gyroscope and the system deviation obtained by the complementary filter. The algorithm both considers the interference of complex environment on sensing and overcomes the adaptation problem of fixed filtering parameters under the dynamic change of UAV, providing a theoretical basis for highly reliable attitude feedback.

The structure of remaining of this paper is as follows: in the second section, the quaternion based space attitude description method of UAV is deduced and the initial alignment of AHRS is completed; in the third section, the complementary filter based on quaternion is designed; in the fourth section, adaptive adjustment rules are designed according to accelerometer norm, magnetometer norm and gyroscope norm; in the last section five, AHRS and the test platform are built, and static state dynamic and interference experiments are designed.

Attitude representation and initial determination

Attitude representation

Defining the global North-east-down (NED) reference frame as the navigation frame ( $n$ frame), where the $X$ , Y, and Z axes respectively aligned with the directions of north, east, and down.

And the orthogonal axis set of the body frame ( $b$ frame) is aligned with the roll, pitch and yaw axes of the vehicle. The $X$ , Y, and Z axes of the body frame (b frame) are selected as the directions of the right, front, and top of UAV respectively. The rotation matrix $C_n^{\mathrm{b}}$ from the n frame to the b frame has a relationship with the Euler angles (roll $\phi$ , pitch $\theta$ , yaw $\psi$ ), which is usually used to describe the attitude of UAV, as:

$$\begin{array}{c}{C}_n^{\mathrm{b}}=\hfill \\ \left[\begin{array}{ccc}\cos \theta \cos \psi \hfill & \cos \theta \sin \psi \hfill & -\sin \theta \hfill \\ \multicolumn{1}{c}{\sin \gamma \sin \theta \cos \psi -\cos \gamma \sin \psi }& \sin \gamma \sin \theta \sin \psi +\cos \gamma \cos \psi \hfill & \sin \gamma \cos \theta \hfill \\ \multicolumn{1}{c}{\cos \gamma \sin \theta \cos \psi +\sin \gamma \sin \psi }& \cos \gamma \sin \theta \sin \psi -\sin \gamma \cos \psi \hfill & \cos \gamma \cos \theta \hfill \end{array}\right]\hfill \end{array}$$

(1)

The formula (1) is not suitable for full attitude estimation for there exist a lot of trigonometric functions, which increase the difficult on real time calculation, and the degradation phenomenon may appears when the pitch angle θ closing to 90°.

In the attitude angle expressing, the quaternion is more flexible than Euler function and could avoid the angle singularity problem, which make the system better linearized. Moreover, it’s easy to transform quaternion into other rotation representation methods, such as rotation matrix or Euler angle sequence, and all of these features popularize the quaternion in the robot field.

A quaternion $Q=[q_0,q_1,q_2,q_3{]}^T$ is composed of a real number $q_0$ and a vector $v=[q_1,q_2,q_3{]}^T$ . In quaternion method, a rotation of rigid body is defined by a rotation axis and by the angle of rotation around it, thus the deadlock problem of the universal joint could be effectively avoided. The quaternion trigonometry $Q=\cos \frac{\theta }{2}+u\sin \frac{\theta }{2}$ includes all the information of the equivalent rotation: $\theta$ is the rotation angle, $u$ is the rotation axis. Set the cosine between the rotation axis and each axis of the reference system to be $\cos \alpha ,\cos \beta ,\cos \gamma$ . Then the quaternion trigonometry can be written as:

$$Q=\cos \frac{\theta }{2}+\sin \frac{\theta }{2}\cos \alpha ·i+\sin \frac{\theta }{2}\cos \beta ·j+\sin \frac{\theta }{2}\cos \gamma ·k$$

(2)

Since $\parallel Q\parallel =\mathrm{co}{s}^2\frac{\theta }{2}+(i^2-m^2-n^2)\mathrm{si}{n}^2\frac{\theta }{2}=1$ , the constraint conditions for quaternion to represent rotation can be obtained: The quaternion describing the rotation of a rigid body is a normalized quaternion with a norm of 1. Therefore, the quaternion vector needs to be normalized in each calculation cycle:

$$Q=Q/\sqrt{q_0^2+q_1^2+q_2^2+q_3^2}$$

(3)

According to the X-Y-Z sequence of Euler angle, a standard formula is used to convert the quaternion angle rotation representation to Euler angles. The roll θ, pitch $\gamma$ , and yaw angles $\psi$ are given by the following equations:

$$\{\begin{array}{c}\theta =\arcsin (2(q_0{q}_2-q_1{q}_3))\hfill \\ \gamma =\arctan \frac{2(q_2{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}\hfill \\ \psi =\arctan \frac{2(q_1{q}_2+q_0{q}_3)}{1-2(q_2^2+q_3^2)}\hfill \end{array}$$

(4)

According to equations (1) and (4), the rotation matrix from $b$ frame to $n$ frame can be expressed as:

$$\begin{array}{c}{C}_b^{\mathrm{n}}=\hfill \\ \left[\begin{array}{ccc}{q}_0^2+q_1^2-q_2^2-q_3^2\hfill & 2(q_1{q}_2-q_0{q}_3)\hfill & 2(q_1{q}_3+q_0{q}_2)\hfill \\ \multicolumn{1}{c}{2(q_1{q}_2+q_0{q}_3)}& q_0^2-q_1^2+q_2^2-q_3^2\hfill & 2(q_2{q}_3-q_0{q}_1)\hfill \\ \multicolumn{1}{c}{2(q_1{q}_3-q_0{q}_2)}& 2(q_2{q}_3+q_0{q}_1)\hfill & q_0^2-q_1^2-q_2^2+q_3^2\hfill \end{array}\right]\hfill \end{array}$$

(5)

Attitude initialization

The data measured by each sensor fixed on the UAV are based on $b$ frame. The accelerometer measures the specific force, that is, the force exerted on the carrier by a force other than gravity.

Before taking-off, the UAV is in a static state and the measured data of accelerometer is the projection of the supporting force which is opposite to gravity on $b$ -axis. Defining accelerometer measurement value as $g^b=[g_x,g_y,g_z{]}^T$ , and the gravity acceleration vector measured by accelerometer as $k=-g^b$ .

Let $||g^b||=\sqrt{g_x^2+g_y^2+g_z^2}$ be the norm of the accelerometer measurement. K could be normalized as:

$$k=-{[\frac{g_x}{||g^b||},\frac{g_y}{||g^b||},\frac{g_z}{||g^b||}]}^T=[k_x,k_y,k_z{]}^T$$

(6)

The measured value of magnetometer $m^b=[m_x,m_y,m_z{]}^T$ could be the projection of geomagnetic field vector $\mathrm{mag}$ in the $b$ frame without the interference of external magnetic field.

Considering the magnetic inclination, the geomagnetic field vector $\mathrm{mag}$ may be not perpendicular to the vector k, as shown in Figure 1. Therefore, the projection of $\mathrm{mag}$ on vector $k$ is first calculated: $\mathrm{\Delta }k=k*(m^b*k)$ , By $i=m^b-\mathrm{\Delta }k=[m_x-m_x,m_y,m_z{]}^T$ , the vector perpendicular to $k$ is obtained and normalized to get the unit vector pointing north $i=[i_x,i_y,i_z{]}^T$ .

Figure 1.

Vector projection of magnetometer.

Therefore, the vertical downward gravity vector obtained by the accelerometer would be in the same direction with the $z$ axis of $n$ frame, and could be expressed by $k$ in $b$ frame. Ignoring the magnetic declination angle, the vector obtained by magnetometer would pointing to the north, the same direction as the $x$ axis of $n$ frame, and be expressed as $i$ in $b$ frame. By the cross product of $i$ and $k$ , according to the right hand rule, we can get the vector which is perpendicular to $i$ , $k$ and in the same direction as $y$ axis of $n$ frame, or its counterpart $j$ of $b$ frame:

$$j=k\times i=[k_2{i}_3-k_3{i}_2,k_3{i}_1-k_1{i}_3,k_1{i}_2-k_2{i}_1{]}^T$$

(7)

Then the vectors $i$ , $j$ and $k$ are the projections of the axial unit vector of $n$ frame in $b$ frame respectively. According to the matrix rotation algorithm, the dot product between a vector in $b$ frame and each vector of $i$ , $j$ and $k$ will be projected to X, Y, and Z axes of $n$ frame. A three-dimensional rotation matrix $R$ from $b$ frame to $n$ frame could be defined as:

$$R=[i,j,k{]}^T=\left[\begin{array}{ccc}{C}_{11}\hfill & C_{12}\hfill & C_{13}\hfill \\ \multicolumn{1}{c}{C_{21}}& C_{22}\hfill & C_{23}\hfill \\ \multicolumn{1}{c}{C_{31}}& C_{32}\hfill & C_{33}\hfill \end{array}\right]$$

(8)

The quaternion representing the initial attitude of UAV can be obtained from the following equations.

$$\{\begin{array}{c}{q}_0=\frac{1}{2}{(1+C_{11}+C_{22}+C_{33})}^{1/2}\hfill \\ q_1=\frac{1}{4a}(C_{32}-C_{23})\hfill \\ q_2=\frac{1}{4a}(C_{13}-C_{31})\hfill \\ q_3=\frac{1}{4a}(C_{21}-C_{12})\hfill \end{array}$$

(9)

According to the WMM geomagnetic model provided by National Oceanic and Atmospheric Administration, the magnetic declination $\mathrm{ma}{g}_{\mathrm{del}}$ near the surface of the test site can be obtained, which may compensate the magnetic declination error of the initial state, and thus the accurate yaw angle information would be figured out.

The quaternion of magnetometer offset could be defined as $q_{\mathrm{del}}=\left[\begin{array}{c}{q}_{\mathrm{del}0}\hfill \\ \multicolumn{1}{c}{q_{\mathrm{del}1}}\\ \multicolumn{1}{c}{q_{\mathrm{del}2}}\\ \multicolumn{1}{c}{q_{\mathrm{del}3}}\end{array}\right]$

According to the definition of quaternion, the quaternion variation after yaw is as follows:

$$\{\begin{array}{c}{q}_{\mathrm{del}0}=\cos \frac{\mathrm{ma}{g}_{\mathrm{del}}}{2}\hfill \\ q_{\mathrm{del}1}=0\hfill \\ q_{\mathrm{del}2}=0\hfill \\ q_{\mathrm{del}3}=\sin \frac{\mathrm{ma}{g}_{\mathrm{del}}}{2}\hfill \end{array}$$

(10)

The quaternion transformation is multiplied by the initial quaternion. According to the physical meaning of quaternion multiplication, it is equivalent to the rotation of $b$ system around Z axis by $\mathrm{ma}{g}_{\mathrm{del}}$ degrees. Finally, the initial value of quaternion after magnetic declination is compensated can be obtained.

Quaternion differential equation

The gyroscope data is used as the input of quaternion differential equation to update quaternion. According to Coriolis theorem and quaternion multiplication representation of coordinate transformation, the quaternion differential equation using gyroscope data as input is obtained:

$$\stackrel{·}{Q}=\frac{1}{2}Q\otimes {\omega }_{\mathrm{nb}}^b$$

(11)

where, ${\omega }_{\mathrm{nb}}^b$ is the corrected gyroscope data, which is recorded as $\left[\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{{\omega }_x}\\ \multicolumn{1}{c}{{\omega }_y}\\ \multicolumn{1}{c}{{\omega }_z}\end{array}\right]$

⊗ represents quaternion multiplication.

Matrix form:

$$\begin{array}{c}\multicolumn{1}{c}{\left[\begin{array}{c}\stackrel{.}{q_0}\hfill \\ \multicolumn{1}{c}{\stackrel{.}{q_1}}\\ \multicolumn{1}{c}{\stackrel{.}{q_2}}\\ \multicolumn{1}{c}{\stackrel{.}{q_3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}\begin{array}{cc}0\hfill & -{\omega }_x\hfill \\ \multicolumn{1}{c}{{\omega }_x}& 0\hfill \end{array}\hfill & \begin{array}{cc}-{\omega }_y\hfill & -{\omega }_z\hfill \\ \multicolumn{1}{c}{{\omega }_z}& {\omega }_y\hfill \end{array}\hfill \\ \multicolumn{1}{c}{\begin{array}{cc}{\omega }_y\hfill & -{\omega }_z\hfill \\ \multicolumn{1}{c}{{\omega }_z}& {\omega }_y\hfill \end{array}}& \begin{array}{cc}0\hfill & {\omega }_x\hfill \\ \multicolumn{1}{c}{-{\omega }_x}& 0\hfill \end{array}\hfill \end{array}\right]\left[\begin{array}{c}{q}_0\hfill \\ \multicolumn{1}{c}{q_1}\\ \multicolumn{1}{c}{q_2}\\ \multicolumn{1}{c}{q_3}\end{array}\right]}\\ =\frac{1}{2}\left[\begin{array}{cc}\begin{array}{cc}{q}_0\hfill & -q_1\hfill \\ \multicolumn{1}{c}{q_1}& q_0\hfill \end{array}\hfill & \begin{array}{cc}-q_2\hfill & -q_3\hfill \\ \multicolumn{1}{c}{-q_3}& q_2\hfill \end{array}\hfill \\ \multicolumn{1}{c}{\begin{array}{cc}{q}_2\hfill & q_3\hfill \\ \multicolumn{1}{c}{q_3}& -q_2\hfill \end{array}}& \begin{array}{cc}{q}_0\hfill & -q_1\hfill \\ \multicolumn{1}{c}{q_1}& q_0\hfill \end{array}\hfill \end{array}\right]\left[\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{{\omega }_x}\\ \multicolumn{1}{c}{{\omega }_y}\\ \multicolumn{1}{c}{{\omega }_z}\end{array}\right]\hfill \\ =\left[\begin{array}{c}-{\omega }_x{q}_1-{\omega }_y{q}_2-{\omega }_z{q}_3\hfill \\ \multicolumn{1}{c}{{\omega }_x{q}_0+{\omega }_z{q}_2-{\omega }_y{q}_3}\\ \multicolumn{1}{c}{{\omega }_y{q}_0-{\omega }_z{q}_1+{\omega }_x{q}_3}\\ \multicolumn{1}{c}{{\omega }_z{q}_0+{\omega }_y{q}_1-{\omega }_x{q}_2}\end{array}\right]\hfill \end{array}$$

(12)

Through the first order Runge Kutta method:

$$\begin{array}{c}{y}^{\prime }=\frac{\mathrm{dy}}{\mathrm{dx}}=f(x,y)\hfill \\ y_{i+1}=y_i+h*K_1\hfill \\ K_1=f(x_1,y_1)\hfill \end{array}$$

(13)

The iterative equation of quaternion can be obtained $Q(t+\mathrm{\Delta }t)=Q\left(t\right)+\stackrel{·}{Q}\mathrm{\Delta }t$ :

Matrix form:

$${\left[\begin{array}{c}{q}_0\hfill \\ \multicolumn{1}{c}{q_1}\\ \multicolumn{1}{c}{q_2}\\ \multicolumn{1}{c}{q_3}\end{array}\right]}_{t+\mathrm{\Delta }t}={\left[\begin{array}{c}{q}_0\hfill \\ \multicolumn{1}{c}{q_1}\\ \multicolumn{1}{c}{q_2}\\ \multicolumn{1}{c}{q_3}\end{array}\right]}_t+\frac{\mathrm{\Delta }t}{2}\left[\begin{array}{c}-{\omega }_x{q}_1-{\omega }_y{q}_2-{\omega }_z{q}_3\hfill \\ \multicolumn{1}{c}{{\omega }_x{q}_0+{\omega }_z{q}_2-{\omega }_y{q}_3}\\ \multicolumn{1}{c}{{\omega }_y{q}_0-{\omega }_z{q}_1+{\omega }_x{q}_3}\\ \multicolumn{1}{c}{{\omega }_z{q}_0+{\omega }_y{q}_1-{\omega }_x{q}_2}\end{array}\right]$$

(14)

Where, $\mathrm{\Delta }t$ is the quaternion update period.

The updated quaternion is obtained and the next iteration is carried out.

Complementary filter

Complementary filter principle

Taking advantage of the good high-frequency characteristics of gyroscope and the good low-frequency characteristics of accelerometer and magnetometer, complementary filter uses accelerometer and magnetometer to compensate the drift of the gyroscope and then improves the attitude measurement accuracy. The specific methods are as follows:

The gyroscope data ${\omega }_{\mathrm{gyro}}$ is substituted into equation (14) to update quaternion. The updated quaternion is substituted into equation (4) to obtain the attitude angle estimation (including roll φ, pitch θ, yaw ψ). As mentioned above, the accelerometer cannot sense the rotation in the horizontal direction. Therefore, the accelerometer data $g^b$ measured by $b$ frame is corrected for gravity in $n$ frame to obtain the compensation of roll and pitch angles $e_g^{\omega }$ ; and the magnetometer data $m^b$ measured by $b$ frame is corrected for geomagnetic vector in $n$ frame to obtain the compensation of yaw angles $e_m^{\omega }$ . The $e_g^{\omega }$ and $e_m^{\omega }$ are multiplied by the weights of the accelerometer and magnetometer then summed, respectively, to obtain the gyro compensation $e^{\omega }$ .

In order to reduce the influence of sensor noise, eliminate the static error and improve the response speed, PI control is added to compensate $e^{\omega }$ of gyroscope. ¹⁴ Finally, the output of PI controller and gyroscope data ${\omega }_{\mathrm{gyro}}$ are accumulated to get the corrected gyro data $\hat{\omega }$ , and enter into the next iteration. The principle of complementary filter is shown in Figure 2.

Figure 2.

Based on quadratic complementary filter principle.

Fusion of gyroscope and accelerometer

The measured $g^b$ of the accelerometer is the component of the gravity vector in the three axes of the $b$ frame, while the gravity vector $g^n$ in the $n$ frame is determined. Therefore, the measurement error in the $b$ frame reflect the error of the rotation matrix when the $n$ frame rotates to the $b$ frame, thus calibrating the roll angle and pitch angle estimated by the gyroscope.

The calculated value ${\hat{g}}^b$ of gravity vector in $b$ frame is the matrix product of the normalized gravity vector $g^n$ in $n$ frame and the rotation matrix $C_n^{\mathrm{b}}$ , which could be represented as:

$${\hat{g}}^b=C_n^{\mathrm{b}}{g}^n=C_b^{{\mathrm{n}}^T}{g}^n=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)\hfill \\ 2(q_2{q}_3+q_0{q}_1)\hfill \\ q_0^2-q_1^2-q_2^2+q_3^2\hfill \end{array}\right]$$

(15)

Where, $g^n=[0,0,1{]}^T$ ; $C_b^{{\mathrm{n}}^T}$ is the transpose matrix of $C_b^{\mathrm{n}}$ .

The error between vectors can be calculated by the cross product of vectors. With the vector product of gravity vector $g^b$ and ${\hat{g}}^b$ , the compensation $e_g^{\omega }$ of accelerometer to gyroscope in pitch and roll direction is obtained:

$$\begin{array}{c}{e}_g^{\omega }={\hat{g}}^b\times g^b=\hfill \\ \left[\begin{array}{c}{g}_y*(q_0^2-q_1^2-q_2^2+q_3^2)-g_z*2(q_2{q}_3+q_0{q}_1)\hfill \\ g_z*2(q_1{q}_3-q_0{q}_2)-g_x*(q_0^2-q_1^2-q_2^2+q_3^2)\hfill \\ g_x*2(q_2{q}_3+q_0{q}_1)-g_y*2(q_1{q}_3-q_0{q}_2)\hfill \end{array}\right]\hfill \end{array}$$

(16)

Fusion of gyroscope and magnetometer

The measured $m^b$ of the magnetometer is the component of the geomagnetic vector in the three axes of the $b$ frame, while the geomagnetic vector $m^n$ in the $n$ frame is determined. Therefore, the measurement error in the $b$ frame reflects the error of the rotation matrix when the $n$ frame rotates to the $b$ frame, thus calibrating the yaw angle estimated by the gyroscope.

In $n$ frame, the calculated value ${\hat{m}}^n$ of the geomagnetic vector could be obtained through multiplying the magnetometer measured vector $m^b$ by the rotation matrix $C_b^{\mathrm{n}}$ , that is:

$$\begin{array}{c}\multicolumn{1}{c}{{\hat{m}}^n=\left[\begin{array}{c}{\hat{m}}_x\hfill \\ \multicolumn{1}{c}{{\hat{m}}_y}\\ \multicolumn{1}{c}{{\hat{m}}_z}\end{array}\right]=C_b^{\mathrm{n}}{m}^b}\\ =\left[\begin{array}{c}(q_0^2+q_1^2-q_2^2-q_3^2)*m_x+2(q_1{q}_2-q_0{q}_3)*m_y+2(q_1{q}_3+q_0{q}_2)*m_z\hfill \\ 2(q_1{q}_2+q_0{q}_3)*m_x+q_0^2-q_1^2+q_2^2-q_3^2*m_y+2(q_2{q}_3-q_0{q}_1)*m_z\hfill \\ 2(q_1{q}_3-q_0{q}_2)*m_x+2(q_2{q}_3+q_0{q}_1)*m_y+q_0^2-q_1^2-q_2^2+q_3^2*m_z\hfill \end{array}\right]\hfill \end{array}$$

(17)

Where, $m^b=[m_x,m_y,m_z{]}^T$ .

The magnetic deflection angle could be estimated from the calculated tangent value of $m_x,m_y$ . and furtherly, the correction error angle of magnetometer for gyro yaw direction could be obtained by comparing the estimated magnetic deflection angle with the real magnetic declination angle $\mathrm{ma}{g}_{\mathrm{del}}$ , namely:

$$m_{\mathrm{err}}=\tan ({\hat{m}}_y/{\hat{m}}_x)-\mathrm{ma}{g}_{\mathrm{del}}$$

(18)

A new vector is constructed, and $m_{\mathrm{err}}$ value is filled into the z-axis position of the vector; $[0,0,m_{\mathrm{err}}{]}^T$ and the rotation matrix $C_n^{\mathrm{b}}$ is left multiplied to obtain the compensation $e_m^{\omega }$ of magnetometer to gyroscope in yaw direction:

$$e_m^{\omega }=C_n^{\mathrm{b}}\left[\begin{array}{c}0\hfill \\ \multicolumn{1}{c}{0}\\ \multicolumn{1}{c}{m_{\mathrm{err}}}\end{array}\right]=\left[\begin{array}{c}2(q_1{q}_3-q_0{q}_2)*m_{\mathrm{err}}\hfill \\ 2(q_2{q}_3+q_0{q}_1)*m_{\mathrm{err}}\hfill \\ q_0^2-q_1^2-q_2^2+q_3^2*m_{\mathrm{err}}\hfill \end{array}\right]$$

(19)

Compensation of gyroscope

The calibration weight of accelerometer is defined as ${\omega }_{\mathrm{acc}}$ , the calibration weight of magnetometer is ${\omega }_{\mathrm{mag}}$ , and the compensation of gyroscope is defined as:

$$e^{\omega }=e_g^{\omega }*{\omega }_{\mathrm{acc}}+e_m^{\omega }*{\omega }_{\mathrm{mag}}$$

(20)

PI control is applied to the compensation $e^{\omega }$ of gyroscope:

$$\delta =k_p{e}^{\omega }+k_i\int e^{\omega }$$

(21)

where $k_p$ , $k_i$ is the proportional coefficient and integral coefficient, respectively. Among them, $k_p$ determines the cut-off frequency of the filter; $k_i$ determines the damping coefficient of the filter and affects the time to eliminate the static error.

Finally get the corrected gyroscope data as:

$$\hat{\omega }={\omega }_{\mathrm{gyro}}+\delta$$

(22)

where, ${\omega }_{\mathrm{gyro}}$ is the original data of the gyroscope, and $\hat{\omega }$ is the corrected data.

The modified gyroscope data $\hat{\omega }$ is used as the input of Quaternion Differential Equation to update quaternion. The norm of the updated quaternion is no longer equal to 1, so the quaternion needs to be normalized to get the final attitude quaternion, and enter into the next iteration.

The above complementary filter based on quaternion can effectively reduce the calculation amount of attitude estimation, and better realize the data fusion of AHRS sensors. However, in practical engineering applications, it is difficult to obtain satisfactory results by using a set of fixed filter parameters to adapt to various flight states and complex environmental disturbances. Therefore, it is necessary to adjust the filter parameters online adaptively according to the environmental interference and the dynamic changes of UAV to improve the accuracy and robustness of the solution.

Fuzzy adaptive complementary filter

Correction model

Substituting equations (20) and (21) in (22), a correction model of gyroscope is proposed:

$$\begin{array}{l}\widehat{\omega }=\omega +k_p{e}^{\omega }+k_i\int e^{\omega }\\ =\omega +k_p(e_g^{\omega }\ast {\omega }_{acc}+e_m^{\omega }\ast {\omega }_{mag})+k_i\int (e_g^{\omega }\ast {\omega }_{acc}+e_m^{\omega }\ast {\omega }_{mag})\end{array}$$

(23)

According to the solution principle and the gyroscope correction model, three factors affecting the accuracy of the solution are summarized here:

• (1) If the aircraft is in the variable speed flight state, the accelerometer would be disturbed by the motion acceleration, and the magnitude and direction of the measured acceleration vector might deviate from the gravity vector that is required for the measured value in the process of calculating the pitch and roll compensation $e_g^{\omega }$ , which will lead to excessive error of $e_g^{\omega }$ .

• (2) If there is non-geomagnetic interference in flight environment, the magnitude and direction of the measured magnetic field vector may deviate from the geomagnetic vector that is required for the measured value in the process of calculating the yaw compensation $e_m^{\omega }$ , which will lead to excessive error of $e_m^{\omega }$ .

• (3) The $k_p$ and $k_i$ determine the cut-off frequency and damping coefficient of the filter. Specifically, the smaller the proportional term $k_p$ , the more the complementary filter output attitude depends on the gyroscope data. However, when the system is at low speed or steady state, as the error caused by the serious temperature drift of the gyroscope accumulates, the calculated data will quickly deviate from the actual attitude angle of the measured object.

The larger $k_p$ is, the more the output attitude of complementary filter mainly depends on the data of magnetometer and accelerometer. But when the system is in high-speed rotation, there will be a large time delay because of the poor high-frequency characteristics of magnetometer and accelerometer. For the integral term $k_i$ , if too large, it will cause overshoot or even integral saturation, while a insufficient one may not eliminate the static error in a short time.

Therefore, a fuzzy control adaptive complementary filter based on norm judgment is designed in this paper. The algorithm structure diagram is shown in Figure 3. Firstly, a weight adaptive adjustment scheme is designed, which can adjust the accelerometer calibration weight adaptively according to the accelerometer measurement value norm $||g^b||$ , so as to answer the influence of the inaccuracy of compensation $e_g^{\omega }$ on the compensation value $e^{\omega }$ of gyroscope under the interference of motion acceleration. Next, according to the magnetometer measurement value norm $||m^b||$ , the magnetometer weight ${\omega }_{\mathrm{acc}}$ is adaptively adjusted. In order to deal with the disturbance of the external non-geomagnetic magnetic field on the magnetometer, the influence of the inaccuracy of the yaw compensation $e_m^{\omega }$ on the gyroscope compensation value $e^{\omega }$ is discussed. Finally, according to the characteristic that the gyroscope parametric $||{\mathit{\omega }}_{\mathit{gyro}}||$ can reflect the motion state of the body, the fuzzy controller is designed to adaptively adjust the filter ${\mathit{k}}_{\mathit{p}}$ coefficients according to the system motion state, and the specific law summarized by the adjustment experience of PI controller and a large number of experiments; the fuzzy controller is designed to adaptively adjust the filter ${\mathit{k}}_{\mathit{i}}$ coefficients according to the deviation ${\mathit{e}}^{\mathit{\omega }}$ of the system. So that the filter has both suitable cut-off frequency and damping coefficient in different rotation states.

Figure 3.

Fuzzy control adaptive complementary filter based on norm judgment.

Tuning of ${\mathit{\omega }}_{\mathit{acc}}$ and ${\mathit{\omega }}_{\mathit{mag}}$

When the UAV has motion acceleration, the problem described in the factor (1) will arise. Therefore, as the accelerometer norm $||g^b||$ reflecting the motion acceleration interference, it is necessary to dynamically adjust the accelerometer weight ${\mathit{\omega }}_{\mathit{acc}}$ according to the $||g^b||$ .

Let $||g^b||$ donate the norm of the system’s acceleration vector that measured by the accelerometer and $g$ denote the size of the gravity constant. When the system is in uniform motion or static state, $||g^b||$ is equal to $g$ , while when annoyed by motion acceleration interference, there will be a large error between $||g^b||$ and $g$ .

Considering the sensor noise error $e_a$ , if $||g^b||\in [g-e_a,g+e_a]$ , the accelerometer possesses the highest credibility, and should be given more weight; otherwise, it means that there is motion acceleration interference in the system and the accelerometer credibility is low, so the weight for accelerometer should be smaller. Therefore, the function curve describing the input $||g^b||$ and output ω should be a flat passband in the $[g-e_a,g+e_a]$ region, and the curve outside the passband should decay rapidly in a very small range, and its shape is similar to a bell. So the bell-shaped function is adopted here to design adaptive control scheme. The relationship between $||g^b||$ and ${\mathit{\omega }}_{\mathit{acc}}$ is expressed by the function $\mathit{f}(||g^b||)$ :

$${\mathit{\omega }}_{\mathit{acc}}=\mathit{f}(||g^b||)={\mathit{\omega }}_{\mathit{acc}\_\mathit{max}}*\frac{1}{1+{\left|\frac{||g^b||-c}{\mathit{a}}\right|}^{2\mathit{b}}}$$

(24)

where, ${\mathit{\omega }}_{\mathit{acc}\_\mathit{max}}$ is the upper limit of the weight of the accelerometer; $\mathit{a},\mathit{b}$ are both positive number. Among them, $\mathit{b}$ determines the steepness of the curve of the function curce, that is, the larger the value of it, the smoother the passband, and the faster the roll-off on both sides; $\mathit{a}$ concerns the width of the curve, and the larger the value of it, the more gentle the peak, and the slower the rolling of both sides; $c$ is the coordinate point corresponding to the peak value of the curve, which is equal to the magnitude of the gravity constant $g$ in this paper.

The curve of the bell-shaped function $\mathit{f}(||g^b||)$ is shown in Figure 4:

Figure 4.

Curve of bell-shaped function.

The bell-shaped curve describes the variation of the accelerometer weight ${\mathit{\omega }}_{\mathit{acc}}$ as the $||g^b||$ changes. The top passband of the bell-shaped curve represents the condition that the weight ${\mathit{\omega }}_{\mathit{acc}}$ reaches the maximum, that is, when the $||g^b||$ is near the $c$ , the accelerometer has the highest reliability and the largest weight; with the deviation of the $g^b$ from the center point c to both sides, the curve decreases rapidly and the corresponding weight becomes smaller. Meanwhile, considering the error such as sensor noise, the curve should change smoothly when near the point c. By collecting a large number of data and calculating the accelerometer variance, the accelerometer noise error $e_a$ is determined to be $0.65\mathrm{m}/{\mathrm{s}}^2$ , and finally set $\mathit{b}=4$ , $\mathit{a}=1$ .

Similarly, when there is external magnetic interference around the system, the problem described in the factor (2) will arise. $||m^b||$ is the norm of magnetic field measured by magnetometer and $m$ denotes the size of the local magnetic constant. When there is no external magnetic interference, $||m^b||$ equals to $m$ ; otherwise, there is a large error between $||m^b||$ and $m$ . The adjustment method of magnetometer weight ${\mathit{\omega }}_{\mathit{mag}}$ is similar to the above adjustment method of ${\mathit{\omega }}_{\mathit{acc}}$ , which will not be repeated here.

Tuning of PI coefficients

When the filter coefficients $k_p,k_i$ are both fixed, the problem described in factor (3) will arise in the process of attitude estimation.

Tuning of proportional gain P

In this section, the fuzzy control rules are used to adjust the coefficients $k_p$ adaptively according to the system motion state $||{\omega }_{\mathrm{gyro}}||$ . The basic principles of the fuzzy rule are: when the UAV attitude angle is large maneuvering large overload situation, the scale parameter decreases, that is, more trust in the gyroscope measurement data; in relatively smooth motion, the scale parameter increases, that is, more trust in the accelerometer and magnetometer measurement data. The size of $||{\omega }_{\mathrm{gyro}}||$ represents the rotational speed of the body:

$$||{\omega }_{\mathrm{gyro}}||=\sqrt{{{\omega }_{\mathrm{gyrox}}}^2+{{\omega }_{\mathrm{gyroy}}}^2+{{\omega }_{\mathrm{gyroz}}}^2}$$

(25)

Taking the $||{\omega }_{\mathrm{gyro}}||$ as the input of the fuzzy controller, whose real range (fuzzy domain), according to the maximum range of the L3GD20H gyroscope, is [0,200], the fuzzy domain of the $||{\omega }_{\mathrm{gyro}}||$ could be divided into five fuzzy subsets {NB,NS,ZE,PS,PB},where NB, NS, ZE, PS, and PB represent extra small, small, normal, large, and extra large respectively. The output of the fuzzy controller is supposed as $k_p$ , and its range (fuzzy universe), [0,12] according to engineering experience and actual requirements, is also divided into 5 fuzzy subsets {NB, NS, ZE, PS, PB}.

The membership function for engineering applications is not unique. In this paper, the triangular membership function is selected as the membership function of the input and output variables based on the factors of the time for the microprocessor to realize fuzzification and defuzzification in the design, the memory space occupied by the rule look-up table, and the control effect.

In this section, fuzzy control rules, the basis of fuzzy reasoning, are established as follows:

1. When the system runs in low rotate speed or keeps steady, the $||{\omega }_{\mathrm{gyro}}||$ of gyro is small while the temperature drift of gyroscope is serious, and the attitude estimation mainly depends on the data of accelerometer and magnetometer, thus the $k_p$ should be increased;

2. When the system rotates in high-speed, the $||{\omega }_{\mathrm{gyro}}||$ is large and the magnetometer and accelerometer may have a large time delay due to the poor characteristics of high frequency. Under this circumstance, the attitude estimation mainly depends on the gyroscope data and the $k_p$ should be reduced.

Thus, the fuzzy control rules of P coefficient are shown in Table 1:

Table 1.

Fuzzy control rules of P coefficient.

$||{\mathit{\omega }}_{\mathit{gyro}}||$	NB	NS	ZE	PS	PB
${\mathit{k}}_{\mathit{p}}$	PB	PS	ZE	NS	NB

Tuning of integral gain I

The integration coefficient also determines the calibration effect of the direction cosine matrix in the complementary filtering algorithm and affects the performance of the attitude solving algorithm. A larger integration factor can quickly eliminate the steady-state error of the system in the steady state, but it leads to a larger overshoot when the system deviation is large. In this section, according to the system deviation $e^{\omega }$ and system deviation change rate ${e^{\omega }}_c$ which can be computed from ${e^{\omega }}_c=e^{\omega }/\mathrm{dt},$ the fuzzy control rules are used to adaptively adjust the coefficients $k_i$ .

The absolute value of the system deviation $\left|e^{\omega }\right|$ and the absolute value of the system deviation change rate $\left|{e^{\omega }}_c\right|$ are taken as the input of the fuzzy controller. On the basis of engineering experience, the real range (fuzzy domain) of $\left|e^{\omega }\right|$ and $\left|{e^{\omega }}_c\right|$ should be limited in [−3, 3] and divided into 7 fuzzy subsets, namely {NB,NM,NS,ZO,PS,PM,PB}. The range (fuzzy field) of $k_i$ , the output of the fuzzy controller, would distribute in [0,3] according to the engineering experience and practical requirements, which could be divided into {ZO,PS,PM,PB}, where NB, NM, NS, ZO, PS, PM, and PB represent negative large, negative medium, negative small, zero, positive small, positive middle, and positive large respectively.

The membership functions of input and output variables are still the triangular model function. The fuzzy control rules established in this section are as follows:

1. When $e^{\omega }$ is large, in order to avoid large overshoot of the system response, the integral action should be limited and usually take $k_i=0$ .

2. When the $e^{\omega }$ and ${e^{\omega }}_c$ are medium, the value of $k_i$ should be of appropriate size.

3. When the $e^{\omega }$ is small, in order to make the system have better stability performance and eliminate the static error quickly, the $k_i$ value should be larger.

According to the above principle, PID coefficients adaptive principle and operation experience, the fuzzy control rules of I coefficient are shown in Table 2:

Table 2.

Fuzzy control rules of I coefficient.

$k_i$		$e^{\omega }$
		NB	NM	NS	ZO	PS	PM	PB
${e^{\omega }}_c$	NB	ZO	PS	PB	PB	PB	PS	ZO
	NM	ZO	PS	PB	PB	PB	PS	ZO
	NS	ZO	ZO	PM	PB	PM	ZO	ZO
	ZO	ZO	ZO	PM	PB	PM	ZO	ZO
	PS	ZO	ZO	PM	PB	PM	ZO	ZO
	PM	ZO	ZO	PB	PB	PB	PB	ZO
	PB	ZO	ZO	PB	PB	PB	PS	ZO

The core design of PI coefficient fuzzy controller has been completed so far. The last thing needed for obtaining the specific value of the optimized PI coefficient and completing the adaptive adjustment of the PI coefficient is clarifying the output of the fuzzy. This method overcomes the attitude error caused by the measurement of high frequency part of accelerometer sensor and low frequency part of gyroscope.

Experimental methods and results

In this section, the test platform and UAV hardware platform based on open source flight control Pixhawk are built. The static, dynamic and interference experiments are designed and conducted to verify the stability and accuracy of the algorithm by comparing the results of adaptive complementary filter, traditional complementary filter, in which the best parameters of the adaptive complementary filter is fixed, and simple gyroscope integration.

Construction of AHRS hardware platform

The open source flight controller Pixhawk, which uses a 32-bit STM32F427 chip with FPU as the main controller and has a fault coprocessor chip with a type of STM32F103, is adopted for the platform. The sensors embedded in Pixhawk include ST’s MicroL3GD20H 16-bit gyroscope and Micro LSM303D 14-bit accelerometer/magnetometer, InvenSense Corp.’s MPU 6000 series 3-axis accelerometer/gyro, and MEAS’s MS5611 barometer.

By spi communication, the raw data from the sensor is transmitted to the main controller, in which the attitude solution is completed. Pixhawk transmits the experimental data (including sensor data and settlement results) back to the host computer in real time by J-link simulator. The serial port drawing software is installed in the host computer, so that the data returned by Pixhawk can be displayed in real time in the host computer.

Construction of test platform

The test platform is composed of three electric rotating turntables: yaw axis, pitch axis and roll axis and its overall structure is shown in Figure 5.

Figure 5.

Triaxial electric test turntable.

Its driving part includes three stepper motors, which is control the rotation of the yaw, pitch and roll direction of the experimental table from the bottom to the top respectively. Through the test, the error of the test bench is about 1° per rotation of 100 laps, so the accuracy of the test bench is 1/36,000° per degree, as shown in Figure 6. The PLC control box is designed to realize the constant speed and the fixed point rotation of the experimental table. The hardware platform is fixed at the connecting plate of the experimental platform, and the various flight attitude of the aircraft can be simulated by controlling the turntable motion.

Figure 6.

Laboratory dials.

Dynamic and static experiments

The experiment in this section compares three algorithms of traditional complementary filter, gyro direct integration and adaptive complementary filter. And among them, the dynamic and static tests are used to verify whether the adaptive algorithm can maintain a good and consistent attitude detection accuracy in different motion states of the body.

Static experiments

Set the sensor in static position and hold the sampling frequency at 200 Hz and the sampling time at 300 s. Since the actual angle does not change under static conditions, the solution error is the difference between the estimation result and the initial angle. The standard deviation and the maximum value of attitude angle error calculated by three algorithms under static condition are listed in Table 3.

Table 3.

Attitude angle estimation error table with still sensor.

	Name	ACF	CF	Gyroscope integration
Roll angle error	Absolute mean	0.073	0.104	4.156
	Standard deviation	0.029	0.045	2.792
	Maximum	0.150	0.177	9.143
Pitch angle error	Absolute mean	0.047	0.211	1.066
	Standard deviation	0.024	0.148	1.0868
	Maximum	0.123	0.361	3.167
Yaw angle error	Absolute mean	0.147	0.196	2.536
	Standard deviation	0.036	0.056	1.424
	Maximum	0.230	0.318	4.926

The roll angle, pitch angle and yaw angle errors solved by the three algorithms are shown in Figures 5 to 7, respectively.

Figure 7.

Static roll angle estimation error.

From Table 3, it can be concluded that the mean value of the solution error of adaptive complementary filter in roll, pitch and yaw direction are 0.073°, 0.047°, and 0.147° respectively, and the corresponding standard deviation is 0.029°, 0.024°, and 0.036° respectively, which are smaller than the other two algorithms. From Figures 7 to 9, it can be seen that there is no attitude drift in solution results of traditional complementary filter and adaptive complementary filter, while the cumulative error of gyroscope integration increases and deviates from the real angle quickly.

Figure 8.

Static pitch angle estimation error.

Figure 9.

Static yaw angle estimation error.

Dynamic experiments

The dynamic test is divided into two cases including the case of slow attitude change (setting the test turntable speed of 0.5°/s) and the case of fast attitude change (setting the test turntable speed of 20°/s).

For most aircrafts, their pitch and roll control angle is smaller than 45° in the real flight process. Thus the measurement range of roll and pitch angle is −45∼45°, and the measurement range of yaw angle is 0–360°. Since the electric turntable can operate at a uniform speed, it is not difficult to deduce that the function curve of the real angle $\alpha$ and rotation time $t$ is an oblique line starting from the initial angle ${\alpha }_0$ and with the slope of the rotational speed $\omega$ :

$$\alpha ={\alpha }_0+\omega t$$

(26)

From equation (26), we can get the real angle of the whole process. By subtracting the estimated angular position from the real angular position, the solution error of the whole process can be obtained. The absolute mean value, standard deviation and maximum value of attitude angle error calculated by the three algorithms in slow attitude transformation rate of 0.5 °/s are listed in Table 4.

Table 4.

Estimation error table of attitude angle under slow attitude change.

	Name	ACF	CF	Gyroscope integration
Roll angle error	Absolute mean	0.280	0.343	2.064
	Standard deviation	0.222	0.290	0.954
	Maximum	0.717	0.656	3.788
Pitch angle error	Absolute mean	0.392	0.795	1.853
	Standard deviation	0.412	0.454	1.416
	Maximum	0.936	1.57	4.265
Yaw angle error	Absolute mean	0.361	0.647	2.468
	Standard deviation	0.207	0.324	1.56
	Maximum	0.598	1.052	5.103

As the attitude changing slowly, the data and error of roll angle solution obtained by the three algorithms are shown in Figures 10 and 11, respectively; the data and error of pitch angle solution are shown in Figures 12 and 13, respectively; and the data and error of yaw angle solution are shown in Figures 14 and 15 respectively.

Figure 10.

Roll angle estimation data.

Figure 11.

Roll angle estimation error.

Figure 12.

Pitch angle estimation data.

Figure 13.

Pitch angle estimation error.

Figure 14.

Yaw angle estimation data.

Figure 15.

Yaw angle estimation error.

From Table 4, it can be obtained that the mean value of the solution error of adaptive complementary filter in roll, pitch and yaw direction, which are 0.280°, 0.392°, and 0.361°, and the error standard deviation, which are 0.222°, 0.412°, and 0.207° respectively, are both obviously smaller than the other two algorithms. It can be seen from Figures 9, 11, and 13 that the attitude drift does not happen to the solution results of traditional complementary filter and adaptive complementary filter, while the cumulative error of gyroscope integration increases and quickly deviates from the real angle.

Table 5 lists the absolute mean, standard deviation, and maximum of the attitude angle errors calculated by the three algorithms when the attitude changes rapidly (20°/s).

Table 5.

Estimation error table of attitude angle under fast attitude change.

	Name	ACF	CF	Gyroscope integration
Roll angle error	Absolute mean	0.152	0.802	0.262
	Standard deviation	0.172	0.357	0.256
	Maximum	0.324	1.385	0.726
Pitch angle error	Absolute mean	0.102	0.356	0.199
	Standard deviation	0.084	0.202	0.037
	Maximum	0.196	0.682	0.292
Yaw angle error	Absolute mean	0.170	1.170	0.188
	Standard deviation	0.161	0.487	0.186
	Maximum	0.459	2.137	0.534

In the case of fast attitude change, the roll angle solution data and the solution error under the three algorithms are shown in Figures 16 and 17, respectively; the pitch angle direction solution data and the solution error are shown in Figures 18 and 19, respectively; the yaw angle direction solution data and the solution error are shown in Figures 20 and 21, respectively.

Figure 16.

Roll angle estimation data under fast attitude change.

Figure 17.

Roll angle estimation error under fast attitude change.

Figure 18.

Pitch angle estimation data under fast attitude change.

Figure 19.

Pitch angle estimation error under fast attitude change.

Figure 20.

Yaw angle estimation data under fast attitude change.

Figure 21.

Yaw angle estimation error under fast attitude change.

From Table 5, it can be observed that the mean value of solution error of adaptive complementary filter in roll, pitch and yaw direction is 0.152°, 0.102°, and 0.170°, and the error standard deviation is 0.172°, 0.084°, and 0.161°, respectively, which are smaller than the other two algorithms. It can be seen from Figures 17, 19 and 21 that there is no attitude drift in the three algorithms, and the traditional complementary filtering scheme is the worst.

Comparing the experimental results in static and dynamic conditions, we can get the following conclusion:

• (1) Gyroscope shows better dynamic performance, which could provide a relatively accurate attitude angle estimation when the system rotates at a high speed (the attitude changes quickly), but suffers a lot from the drift when the system is in static or low rotation speed (the attitude changes slowly), which may lead to the error accumulation and the measurement result deviating quickly from the actual attitude angle of the measured object.

• (2) Traditional complementary filter can only meet the accuracy of the solution in one state due to the fixed filter coefficients. As in this paper, the traditional complementary filter can only guarantee better solution effect in the case of static or low speed rotation (the attitude changes slower), while the solution effect is the worst when the system rotates at high speed (the attitude changes faster);

• (3) The adaptive complementary filter can adjust the PI controller coefficients adaptively regardless of the low speed or high speed rotation of the system, so that the filter has the best solution effect in different rotation states.

Interference experiment

The interference experiment is divided into two parts: the magnetic interference experiment and the motion acceleration interference experiment. The former is used to test the precision variation of the adaptive algorithm under the external magnetic field interference, and the latter is aimed to test the precision variation of the adaptive algorithm under the motion acceleration interference.

Magnetic interference experiment

The mean, standard deviation and maximum values of the yaw direction attitude angle error calculated by adaptive complementary filter and traditional complementary filter in the case of external magnetic field interference, which is simulated by setting the sensor in static position and approaching it with a magnet, and then moving the magnet away, are listed in Table 6. The solution data of the adaptive complementary filter and the traditional complementary filter are shown in Figure 21.

Table 6.

Estimation error table of yaw angle under magnetic interference.

Roll angle error	Name	ACF	CF
	Absolute mean	0.152	0.802
	Standard deviation	0.172	0.357
	Maximum	0.324	1.385

As showed in Table 6 and Figure 22, the sensor is disturbed by a magnet at the point A, and the magnet leaves at the point B. The yaw angle calculated by the traditional complementary filter produces a large error of about 38° under magnetic interference, while the adaptive complementary filter has a maximum error of 0.248° and an average error of 0.141° in the case of magnetic field interference. It is proved that the adaptive complementary filter can effectively deal with the external magnetic field interference, and the solution effect is obviously better than the traditional algorithm.

Figure 22.

Yaw angle estimation data under magnetic interference.

Acceleration interference experiment

In this section, a linear variable speed guide is built and its specific composition is shown in Figure 23.

Figure 23.

Linear variable speed guide.

The sensor is fix on the guide slide with a certain attitude. As the motor rotating nonuniformly, the guide rail slider is driven to move in a variable speed and possess a motion acceleration, which is made to simulate the case of aircrafts interacting with the external environment during the their flight.

The mean, standard deviation, maximum values of the roll, pitch direction attitude angle errors calculated by adaptive complementary filter and traditional complementary filter in the case of motion acceleration interference are listed in Table 7. And the roll and pitch angle data of adaptive complementary filter and traditional complementary filter in the case of motion acceleration interference are showed in Figures 24 and 25 respectively.

Table 7.

Estimation data of roll and pitch angle under motion acceleration interference.

	Name	ACF	CF
Roll angle error	Absolute mean	0.026	0.285
	Standard deviation	0.038	0.501
	Maximum	0.320	3.641
Pitch angle error	Absolute mean	0.093	0.598
	Standard deviation	0.071	1.020
	Maximum	0.263	3.656

Figure 24.

Roll angle estimation data under motion acceleration interference.

Figure 25.

Pitch angle estimation data under motion acceleration interference.

Through Table 7 and Figures 24 and 25, it can be obtained that the roll and pitch angles calculated by the traditional complementary filter have a large error, in which the maximum error of roll angle is 3.641°, and the maximum error of pitch angle is 3.656°. While the maximum error of roll angle calculated by adaptive complementary filter is 0.320°, the average error is 0.026°, and the maximum error of pitch angle is 0.263°, and the average error is 0.093°. It is proved that the adaptive complementary filter can effectively deal with the motion acceleration interference, and the solution effect is obviously better than the traditional algorithm.

Conclusions

In order to solve the influence of environment disturbance and flight state on AHRS accuracy and reliability, an adaptive estimation scheme in which the weight and parameters of the compensation filter varying with the sensor norm is proposed in this paper. Firstly, the correction model of gyroscope is established. Secondly the adaptive adjustment rules of magnetometer and accelerometer weight are designed according to magnetic field norm and acceleration norm. Finally the fuzzy adaptive tuning rules of filter parameters are designed according to the motion state of aircraft and system deviation. So that AHRS can obtain accurate and reliable UAV attitude under different flight states, magnetic field interference and acceleration interference.

To verify the effectiveness of this scheme, AHRS test platform is designed and static, dynamic and interference experiments are carried out. The experimental results show that the scheme has high accuracy under all flight dynamic conditions. The scheme gave on accuracy of better than 0.280 in roll, 0.392 in pitch and 0.361 in yaw. Meanwhile, the proposed scheme is robust to both motion acceleration interference and magnetic field interference. The maximum estimation errors of roll angle, pitch angle and yaw angle are 0.320°, 0.263°, and 0.248°, respectively. All of the results demonstrate that the proposed algorithm can well provide a theoretical basis for improving the accuracy and reliability of UAV AHRS.