Research on Complementary Filtered Attitude Solution Method for Quadcopter Based on Double Filter Preprocessing

is proposed to address this problem. First, the signal decomposition, hard threshold denoising, and signal reconstruction of the accelerometer and gyroscope acquired data are performed using the Haar real-time wavelet filtering algorithm. The reconstructed signal is then filtered by the infinite impulse response (IIR) low-pass filtering algo-rithm to remove the residual high-frequency noise and complete the dual filtering preprocessing. Next, the gyroscope data is corrected with accelerometer data according to the complementary filtering algorithm. Finally, the corrected data are used to solve the quaternion and thus the attitude angle by combining the Longacurta method. The results show that the dual filtering preprocessing method can further reduce the noise in the measurements of accelerometer and gyroscope. The attitude angle results calculated by the proposed method in the static and dynamic hovering states of the four-rotor aircraft have a small degree of dispersion, which can effectively improve the accuracy of attitude calculation.


Introduction
The precondition for accurate control of multi-rotor aircraft is the provision of stable and reliable attitude and position data by the navigation module, and the attitude estimation problem has received extensive attention from scholars in the past few years [1,2].The micro-miniature high-precision attitude measurement system composed of sensors based on Micro-electromechanical systems (MEMS) technology is widely used in many fields such as aerospace, automotive electronics and intelligent robots due to its low cost, small size, high reliability, universal applicability and strong anti-interference ability [3,4].MEMS Inertial Measurement Unit (IMU) mainly consists of gyroscopes and accelerometers to provide real-time attitude information for UAVs [5].The gyroscope has high short-term accuracy, but it produces cumulative errors when used for attitude resolution, and the accelerometer is affected by airframe vibration, so the measurement results contain a lot of high-frequency noise.In addition, Microcontroller Unit (MCU) has limited capability in terms of computing speed and accuracy [6], and the solution of attitude angle determines the flight stability, so how to filter out the noise in MEMS IMU by MCU calculation and solve the attitude angle with high reliability and accuracy has become a research hotspot in recent years.
Aiming at improving the accuracy of sensor attitude angle calculation, the research methods mainly include Kalman Filter (KF), wavelet filter and sensor model method.The first method is based on Kalman filtering, where the process noise and measurement noise covariance matrices must be known a priori to achieve optimal solutions [7,8], but in most cases these parameters are guessed or adjusted by trial-and-error methods, neither of which guarantees optimality and convergence.Kaba [9] proposed an evolutionary algorithm to estimate the process noise and the measurement noise covariance matrix of the Kalman filter to improve the performance of the filter.Lu [10] proposed a fusion method of aircraft attitude information based on extended Kalman filtering algorithm to improve the antimagnetic interference performance.The extended Kalman filter, as an extended form of the standard Kalman filter in the nonlinear case, linearizes the nonlinear system using Taylor series expansions [11,12], and the limitations of this method include poor stability and large estimation bias.Li [13] used the gradient descent method to optimize the pro-cess noise covariance of the traceless Kalman filter and constructed an Adaptive Unscented Kalman Filter (AUKF) to improve the pose solving accuracy.The second method is based on wavelet filtering, wavelet analysis has multi-resolution analysis characteristics, which has obvious advantages for the analysis of non-stationary signals [14], using the multi-resolution characteristics of wavelet transform, multi-layer decomposition of non-stationary signals, which can effectively eliminate the jump error and high frequency noise of low frequency signals.Liu [15] applied wavelet threshold denoising method to MEMS gyroscope noise processing and got better results.The third category is by constructing a sensor model or fusing multiple algorithms, Liu [16] proposed a UAV attitude estimation algorithm considering motion acceleration interference, which improved the accuracy and interference resistance of UAV navigation system attitude estimation in dynamic environment.Zhao [17] proposed a gated recurrent unit (GRU) neural network-based method for modeling and compensating UAV heading errors, which has a good performance in predicting vehicle orientation.Liu [18] proposed an attitude solution algorithm based on acceleration correction model, which can attenuate the interference of non-gravitational acceleration on attitude calculation and avoid attitude angle divergence of aircraft navigation system in dynamic environment.Li [19] proposed a high-precision attitude measurement algorithm integrating complementary filtering and Kalman filtering, which effectively improves the attitude solution accuracy.
At present, the relevant research has improved the control performance of the aircraft, but it depends on highprecision components, which are very expensive and have great economic pressure for ordinary civil enterprises.Therefore, the ability to use lower cost sensors with low measurement accuracy to obtain accurate attitude information is one of the requirements for companies to develop aircraft.Although Kalman filtering is a relatively mature method, the Kalman filter for attitude estimation using quaternion attitude representation is generally more complex and its development cost is high; The attitude solution accuracy can be effectively improved by the method of constructing an accurate sensor model or by fusing multiple algorithms.However, the former is not universally adaptable, while the latter mostly concentrates on data fusion while ignoring the preprocessing of the original sensor signal.The application of wavelet threshold denoising to sensor attitude angle solving is feasible, but there is a relative lack of research on the accuracy of this method for attitude solving of low-cost aircraft.Therefore, this paper proposes a dual filtering preprocessing method combining Harr wavelet realtime filtering and Infinite Impulse Response (IIR) low-pass filtering to filter the raw gyroscope and accelerometer data, further reduce the noise in the raw data, and use it in the attitude angle solution based on quadratic complementary filtering method, which can effectively improve the attitude angle solution accuracy.

Quadrotor attitude description
Fig. 1 The quadrotor body coordinates and navigation coordinates The attitude of the aircraft is described by pitch an- gle , roll angle  and yaw angle .Due to the small size of the quadrotor relative to the earth, and the low flying altitude and low flying speed, the influence of factors such as the curvature of the earth and the rotation of the earth is ignored.
Assuming that the earth's surface is a flat surface, the attitude sensor is attached to the center of the quadrotor body and treats it as a rigid body.As shown in Fig. 1, the local Cartesian coordinates coordinate system on xn yn zn is selected as the navigation coordinates, the coordinate origin is the take-off position of the quadrotor, the quadrotor body coordinates are ob xb yb zb, and the coordinate origin is the center of the quadrotor body.The commonly used attitude angle representation methods are Euler angle method, rotation matrix method and quaternion method.Among them, Euler angle method has singularity problem.The attitude differential matrix equation of the rotation matrix method contains nine unknown parameters, resulting in a large amount of calculation.The quaternion description method contains all the attitude information of the four-axis aircraft, which can effectively avoid the singularity problem.Because it has only four unknown parameters, the calculation is relatively simple and can realize the full attitude operation of the aircraft.In this paper, the quaternion method is used to describe the attitude, and the attitude quaternion from the navigation coordinate system to the body coordinate system is defined as: The rotation matrix from the navigation coordinate system to the body coordinate system is represented by b n R and the transformation relationship between the two coordinate systems is: where  denotes quaternion multiplication and nb , rr are the representations of the vector r in the two coordinate systems, respectively.After substituting the expression of Q into equation (2), the following expression can be obtained from the law of quaternion calculation: b n q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q  + − − Solving quaternion attitude differential equation: In the formula,  is the rotation angle of the body coordinate system relative to the navigation coordinate system, which is given in the form of a quaternion, namely 0 , where: The first-order Runge-Kutta method is used to solve the quaternion differential equations: Where T is the sampling period.It is considered that each attitude angle of the UAV is 0 before take-off, and the initial quaternion is set to that, each update of the quaternion must be normalized: When the Euler angle rotation order is    →→, the quaternion can be obtained by inverting the Euler angle: arcsin q q q q q q q q arctan qq q q q q arctan qq 

1. IIR low-pass filter denoising method
Traditional signal preprocessing methods mostly use transform domain filtering [20], which usually designs the corresponding low-pass filter to filter the noise according to the frequency domain distribution characteristics of the useful signal and the noise.IIR low-pass filter has the characteristics of simple design and fast data processing, and is widely used in engineering.The output expression of the second-order IIR filter is: Where k Ŷ is the optimal value at time k, Yk is the measured value at time k, , the coefficients in the formula can be obtained as follows: ( )

Real-time wavelet filter denoising method
Wavelet noise reduction is a modern signal processing approach with a wide range of function adaptability and optimal adaptive noise reduction [21].Processing the signal using the multi-resolution property of orthogonal wavelets is equivalent to processing the signal using multiple filters with different bandwidths.In real-time noise reduction processing, both the noise reduction level and the signal processing speed must be considered.The computational speed of wavelet noise reduction depends on the speed of wavelet transform and its inverse transform, so the fast discrete wavelet transform algorithm, i.e.Mallat's multi-resolution wavelet transform algorithm, should be preferred for real-time wavelets.In engineering applications, the acquired signal is usually a continuous step signal, the Haar wavelet basis is fast and can approximate the original function well without boundary problems.Therefore, this paper adopts Haar wavelet and proposes the design method of real-time wavelet filtering algorithm to achieve fast noise reduction processing of continuous step signal.The design steps of the real-time wavelet filtering algorithm are as follows: Step 1: set the length of the moving window to , where N represents the number of layers of decomposition.
Step 2: at the moment k, if k < M, no filtering is performed, if k > M, there is a signal sequence: .
Step 3: Decompose the sequence signal N k X using the Nth layer low-pass filter  11), the multi-layer decomposition of the signal can be realized by the filter bank, and the process is shown in Fig. 2. where

Fig. 2 Two-channel filter banks
Step 4: The high-frequency signal obtained by multi-layer decomposition is thresholded according to certain threshold processing rules, and the approximate coefficients are retained.For the signal after threshold processing, according to the reverse derivation of Eq. ( 11), the original signal at k time is reconstructed to obtain the filtered sequence: ) , where ( , ) x N k is the filtering result at time k.
Step 5: as the time k changes, the signal sequence is updated with the previous moment filtering result and the above steps are repeated from the second step for the next processing.

Double filtering preprocessing method
For IIR low-pass filtering and real-time wavelet filtering, which are two traditional single filtering preprocessing methods, both have their own defects [22].IIR low- When the value of k changes, the signal sequence also needs to be updated with the double-filtered k-time data.
Fig. 3 The schematic diagram of double filtering process at time k

Complementary filtering attitude calculation method based on double filtering preprocessing
The dynamic response of the gyroscope has fast dynamic response and high measurement accuracy, but it needs to integrate the angular velocity when solving the attitude angle, which will produce cumulative error.Accelerometer can get stable attitude angle without cumulative error, but the dynamic response characteristics are poor and the measurement noise is large.These two have complementary frequency characteristics, and by using this feature to combine their information through complementary filtering, more accurate attitude angle information can be obtained.However, linear complementary filtering methods are difficult to operate stably in practical applications.Nonlinear complementary filters are roughly similar to linear complementary filters in that they use the complementary characteristics of accelerometers and gyroscopes.The difference is that the nonlinear complementary filter is based on a nonlinear relationship between angular velocity and rotation angle.The display complementary filter among the nonlinear complementary filters is more suitable for implementation on embedded hardware [23], so the display complementary filter is used in this paper as follows: the raw signals from accelerometer and gyroscope measurements are read, double filtered the raw signals of the accelerometer, and normalize the obtained gravitational acceleration com- The gravity vector in the navigation coordinate system is G = [0 0 1] T and the gravity vector in the airframe coordinate system is expressed in quaternion form as: The vector product of the actual measured value A of the normalized accelerometer and the gravity vector V in the body coordinate system is the error: The compensation error obtained by PI adjustment of the vector product error e is used to correct the gyroscope value to eliminate the drift error [24], as shown in Fig. 4.

Experimental design
In order to verify the accuracy and effectiveness of the proposed method, the experiment platform in Fig. 5 was built to conduct static, dynamic hovering and flight experiments of the quadrotor respectively.The sampling frequency was set to 100Hz, and the sensor measurement data were selected within 30s continuously, and the raw sensor data were loaded into the computer for algorithm comparison and analysis.The UAV is equipped with an attitude sensor consisting of MPU6050 three-axis accelerometer and   1.

Experiment 1: Comparative analysis of accelerometer and gyroscope data preprocessing
To verify the advantages and disadvantages of the dual filtering algorithm, the raw accelerometer and gyroscope data of the UAV under dynamic hovering and flying conditions were collected and pre-processed by Haar realtime wavelet filtering, IIR low-pass filtering and dual filtering methods, respectively.The standard deviations of the preprocessed data were calculated for the static and dynamic hover states, and the results are shown in Tables 2 and 3. Comparing the standard deviations of the three filtered values in Tables 2 and 3, it can be seen that for the preprocessing of accelerometer measurements, the order of merit of the three algorithms is: dual filtering algorithm > IIR lowpass filtering algorithm > Haar real-time wavelet filtering algorithm.For the preprocessing of gyroscope measurements, the order of preference of the three algorithms is: dual filtering algorithm > Haar real-time wavelet filtering algorithm > IIR low-pass filtering algorithm.A smaller standard deviation indicates that the data are less discrete, i.e., the data are smoother.Comparing the three algorithms, the values pre-processed by double filtering have the least dispersion and the smoothest data.As shown in Fig. 6a and  b, the accelerometer and gyroscope values are smoother than the raw data after double filtering preprocessing in the stationary state.Taking the X-axis measurement data of accelerometer and gyroscope in dynamic hovering state as an example, the comparison results of three filtering preprocessing effects are shown in Fig. 6c and d.The data of the two sensors after double filtering are the smoothest.Further comparing the two preprocessing methods of Haar real-time wavelet filtering and IIR low-pass filtering, the data after IIR low-pass filtering in the accelerometer sensor is smoother; the data after Haar real-time wavelet filtering in the gyroscope sensor is smoother, which is consistent with the conclusions in Tables 2 and 3.
The smoothness of the data alone is not enough to illustrate the filtering effect, and a spectral analysis is needed to further determine the denoising situation.Taking the X-axis acquisition data of the accelerometer and gyroscope as an example, the Fourier transform analysis was performed on the pre-processed data under flight conditions, and the results are shown in Figs.7 and 8.It can be seen that the accelerometer is mainly affected by high-frequency noise and the gyroscope is mainly affected by low-frequency noise, and all three filtering algorithms are able to filter out the noise in the gyroscope and accelerometer measurements.Comparing the advantages and disadvantages of the three algorithms in accordance with Figs. 7 and 8, respectively, the same conclusions as in Tables 2 and 3 can be obtained.In addition, it can be clearly seen that the use of IIR low-pass filtering algorithm has obvious effect on highfrequency noise filtering, but its filtering effect on low-frequency noise is poor, which is especially obvious in the numerical denoising of gyroscopes, as shown in Fig. 7. Realtime filtering with Haar wavelets can denoise multiple frequency bands, and it is effective in filtering low frequency noise when preprocessing accelerometer and gyroscope data, but there is still some high frequency noise that cannot be filtered.The accelerometer and gyroscope data preprocessed by the dual filtering algorithm is the best denoising effect among the three algorithms.The experiment shows that the dual filtering preprocessing method of Haar realtime wavelet filtering + IIR low-pass filtering not only takes the advantage of Haar real-time wavelet filtering multi-band   4 and 5.In Figs. 9 and 10, the pitch and roll angle solutions fluctuate within a certain range, but there is still a drift in the heading angle, which indicates that to further improve the attitude solution accuracy of the heading angle, it is still necessary to add a magnetometer sensor to further correct.The attitude solution results of method 3 behave more smoothly both in the static state of Fig. 9 and in the dynamic hovering state of Fig. 10.By comparing the standard deviation data in Tables 4 and 5, it can be seen that the order of advantages and disadvantages of the three methods is: Method 3 > Method 2 > Method 1.Since the fusion of gyroscope and accelerometer data is achieved by correcting gyroscope values with accelerometer values, the weight of gyroscope values is relatively large, and the preprocessing results of gyroscope measurements are more influential to the final attitude angle calculation effect.Therefore, although Haar wavelet real-time filtering is not as effective as IIR low-pass filtering when preprocessing accelerometer measurements, the final calculated attitude angle shows that the Haar wavelet real-time filtering method 2 is still better than the IIR low-pass filtering method 1.The data in Table 4 show that in the stationary condition, Method 3 shows a 3.2% decrease in pitch angle standard deviation, a 3.9% decrease in crossroll angle standard deviation, and a 0.8% decrease in heading angle standard deviation compared to the currently widely used Method 1.The data in Table 5 show that in the dynamic hovering condition, the standard deviation of pitch angle decreases by 7.3%, the standard deviation of cross-roll angle decreases by 7.8%, and the standard deviation of heading angle decreases by 4.7% in Method 3 compared with Method 1.The experimental results show that the attitude angle solving method based on the complementary filtered attitude calculation with double filtering preprocessing is less discrete and can effectively improve the attitude solution accuracy.

Conclusions
For the quadrotor attitude solution problem, this paper describes the quaternion-based attitude description method, introduces two traditional filtering preprocessing methods, IIR low-pass filter denoising and real-time wavelet filter denoising, and gives the design steps of a real-time wavelet filtering algorithm based on Haar wavelets.Further, a double filtering preprocessing algorithm is proposed to remove the noise from the gyroscope and accelerometer measurements, and a complementary filtering algorithm is combined to fuse the gyroscope and accelerometer values to calculate the attitude angle.The UAV experimental platform is constructed, and the accelerometer and gyroscope data preprocessing comparison experiments and the complementary filtering attitude angle solution comparison experiments based on different preprocessing methods are designed respectively, the main conclusions are obtained as follows: 1.The double filtering preprocessing method has better noise filtering effect on accelerometer and gyroscope than IIR low-pass filtering preprocessing method and Haar real-time wavelet filtering preprocessing method.
2. In the process of sensor data fusion, the weight of gyroscope measurement values is relatively large, which can better affect the final attitude angle solution effect.Therefore, in the three attitude angle calculation methods, method 3 > method 2 > method 1.In the dynamic hovering state, the standard deviations of pitch angle, roll angle and heading angle calculated by the proposed method 3 are reduced by 7.3%, 7.8% and 4.7% respectively compared with the commonly used method 1, which can effectively improve the accuracy of attitude angle calculation.

RESEARCH ON COMPLEMENTARY FILTERED ATTITUDE SOLUTION METHOD FOR QUADCOPTER BASED ON DOUBLE FILTER PREPROCESSING S u m m a r y
In the attitude calculation process of quadrotor, the traditional pre-filtering method has degraded the attitude solving accuracy due to incomplete denoising of accelerometer and gyroscope measurements.Therefore, a complementary filtered attitude solution method based on double filter preprocessing is proposed to address this problem.First, the signal decomposition, hard threshold denoising, and signal reconstruction of the accelerometer and gyroscope acquired data are performed using the Haar real-time wavelet filtering algorithm.The reconstructed signal is then filtered by the infinite impulse response (IIR) low-pass filtering algorithm to remove the residual high-frequency noise and complete the dual filtering preprocessing.Next, the gyroscope data is corrected with accelerometer data according to the complementary filtering algorithm.Finally, the corrected data are used to solve the quaternion and thus the attitude angle by combining the Longacurta method.The results show that the dual filtering preprocessing method can further reduce the noise in the measurements of accelerometer and gyroscope.The attitude angle results calculated by the proposed method in the static and dynamic hovering states of the four-rotor aircraft have a small degree of dispersion, which can effectively improve the accuracy of attitude calculation.
Quadrotor body coordinates

1 NLX
− and high-pass filter 1 N H − of the Haar wavelet to obtain the low-frequency and highfrequency coefficients of the nth layer response data as 11 − of the (N- 1) layer, and decompose N k X into N layers.According to Eq. (

Fig. 4
Fig. 4 Complementary filtering attitude calculation method based on double filtering preprocessing

Fig. 6 Fig. 7 Fig. 8
Fig. 6 Double filtering preprocessing effect: aaccelerometer static state data, bgyroscope static state data, caccelerometer dynamic hovering data, dgyroscope dynamic hovering data; in the legend X-R,Y-R and Z-R means the raw data in longitudinal, lateral, and vertical directions respectively; X-DF,Y-DF and Z-DF means the data after double filtering in longitudinal, lateral, and vertical directions respectively; X-WF and X-LF means longitudinal data after Haar real-time wavelet filtering and IIR low-pass filtering, respectively

Fig. 9
Fig. 9 Attitude angle comparison in static state filter out low-frequency noise that has a large impact on the gyroscope sensor; real-time wavelet filtering uses the multi-resolution feature of wavelet transform to decompose the non-stationary signal in multiple layers, which can effectively eliminate the jump error and highfrequency noise of the low-frequency signal to achieve Fast and accurate initial calibration, and the noise of each frequency band can be filtered out, but there will still be part of the residual high frequency noise.It is expected to perform adaptive filtering of noise by Haar real-time wavelet filter firstly to solve the problem that the frequency domain filtering method cannot completely filter out low-frequency noise or the existence of characterization signal mutation points.Then the IIR low-pass filter is used to remove the high-frequency noise that remains, solving the problem of incomplete denoising of high-frequency noise by real-time wavelet filtering, thus obtaining more accurate values for subsequent calculations.The execution process is shown in Fig.3.The raw data of the sensor at time k is filtered by the Haar real-time wavelet filter to obtain the updated data at time k.The updated k-time data is further filtered by the IIR low-pass filter to obtain the double-filtered k-time data.

Table 2
Standard deviation of filtered data from three algorithms (accelerometer)

Table 4
Comparison of standard deviation of attitude angle in static state