Attitude Solving Algorithm and FPGA Implementation of Four-Rotor UAV Based on Improved Mahony Complementary Filter

Abstract

With the development of modern industry, small UAVs have been widely used in agriculture, mapping, meteorology, and other fields. There is an increasing demand for the core attitude-solving algorithm of UAV flight control. In this paper, at first, a novel attitude solving algorithm is proposed by using quaternions to represent the attitude matrix and using Allan variance to analyze the gyroscope error and to quantify the trend of the error over time, so as to improve the traditional Mahony complementary filtering. Simulation results show that the six-axis data from the initial sensors (gyroscope and accelerometer) agree well with the measured nine-axis data with an extra magnetometer, which reduces the complexity of the system hardware. Second, based on the hardware platform, the six-axis data collected from MPU6050 are sent to FPGA for floating-point operation, transcendental function operation, and attitude solution module for processing through IIC communication, which effectively validates the attitude solution by using the proposed method. Finally, the proposed algorithm is applied to a practical scenario of a quadrotor UAV, and the test results show that the RMSE does not exceed 2° compared with the extended Kalman filter method. The proposed system simplifies the hardware but keeps the accuracy and speed of the solution, which may result in application in UAV flight control.