Dynamical analysis and validation of motion control by filtering performance for aerial robotic system

Although drone appears in different applications, such as environmental inspection, agriculture or transportation, some aspects require more studies to clarify the efficient outcomes. One of them is to investigate the filtering performance such as Kalman and Complementary filters when the autonomous aerial system (AAS) handles its mission. However, it lacks the systematic research about these filters to provide the proper evaluation. Therefore, in this paper, the research topic related to AAS model to indicate the filtering effects in the agricultural application for making an alternative solution is presented. Firstly, the mathematical representation of system model is established in order to describe the dynamical performance and motion constraints. Then, the theory of filter structure is implemented to estimate the system state. The proposed design is validated in both numerical simulation and experiments. The system parameters that are monitored, include angular values of roll, pitch and yaw in three axes, motion parameters and its trajectories. By utilizing various sensing devices such as gyroscope, accelerometer and compass in real-world hardware, the experimental results could evaluate more precise and efficient design. The findings of this study are to (1) propose the model of AAS and proper filters, (2) launch the verified process and calibration, and (3) demonstrate the competitive performance among filters. From these results of our work, it could be clearly seen that the AAS plays an important role in daily applications and the related topics are still attractive.


Introduction
The research of quadcopters has become an attractive topic in industrial applications and monitoring fields due to their quick development and easy execution [1]. It is stated that this system still plays a crucial role for investigators in many years. In [2], inspectors were considered to use a quadrotor as one of the important methods to verify the transportation infrastructures such as a tunnel or a bridge. This research emphases on innovating the position control without using the global positioning system (GPS) and offers a solution for detecting a tethered quadrotor's airframe location from the length and tension or force of the tether.
The problem of state estimation becomes one of the popular topics in the aerial vehicle system. Commonly, many researchers chose the filtering approach to give a solution for these problems. However, various filter technologies have been utilized to estimate the system state of aerial vehicle. Several enhanced filters, for example, particle filters, are difficult to be computed and have a high cost of labor. It requires a powerful microprocessor to be managed in the control board. Reversely, some filters, for instance Kalman filter and Complementary filter, reveal the effectiveness and are popular because of their frequency filtering performance in the linearization system [3]. Dissimilar studies developed the Kalman filter [4] in the flight control subject. To estimate the system state at specific attitude or hovering the air vehicle, authors [5] adjusted the typical Kalman filter to enhance the adaptive response. The output values are computed by using a multiple extended Kalman filter. The state observer obtains data from different sensors such as accelerometers, gyroscopes and GPS. The study in [6] introduced an unscented Kalman filter by using the three-axes attitude identification scheme as the system observer. It operates resourcefully, but needs computational capability considerably. Additionally, this approach is also useful to trace the motion [7-9]. The desired filtering data is provided from a small inertial/magnetic sensor. As well, the extended Kalman filter is proper for an uncertain system which contains the nonlinear components. It could be embedded into a pixhawk module that satisfies the further optimization operation of the attitude detection [10]. The px4 or pixhawk module is also combined with an open-source platform, for example gazebo, to develop the hardware-in-the-loop [11].
On the other hand, a method of filter techniques is to integrate a Complementary filter [10] which has little computational complexity in the aerial vehicle system. Owing to the simple framework and high efficiency, many investigators and developers are concerned to research the attitude estimation. In [11,12], researchers indicated that the high-order linear model of Complementary filter is globally asymptotically stable. Two kinds of filter, named as direct and passive, are completed as the work presented in [3]. Moreover, the proposed design is perfectly deterministic while it is based on the linear Complementary filter. This work is considered as the deterministic attitude estimation algorithm.
Kalman filter and Complementary filter are implemented correspondingly in the model of quadcopter to achieve the response in this work. The key contribution is to launch the performance of each filter in quadrotor to prove in which case it can be valuable. The rest of this article is structured as follows. Section 2 briefly reviews the previous works and describes the theoretical principles to estimate the orientation and acceleration in the real-world applications [12]. In Section 3, the mathematical models of both Kalman filter and Complementary filter are depicted basically. Sequentially, the motion analysis of aerial vehicle are demonstrated in Section 4. The theoretical results in Section 5 are simulated to visualize the effective design. Then, several experiments in Section 6 are established to verify our contributions. In Section 7, the conclusions are presented.

Problem statement
In detail, the Kalman filter is commonly known as a solution for state estimation in the unmanned aerial vehicle. The nonlinear observer [13], which produces an exogenous signal, ensures the globally asymptotical stability. This signal is to generate the linearized model for the Kalman filter to accurately estimate a quadcopter's attitude. Unlike many observers, the adaptive high-gain extended Kalman filter is validated to converge [14] globally. This is innovated by varying the high-gain parameter according to a metric. Although it presents several advantages in state estimation and good filtering properties, it still needs to increase the sensitivity to large perturbations. With the simpler design but the same structure, the extended Kalman filter model using the real sensor measurements to estimate correctly the measurement noise that is established [15]. In the complicated platform such as manipulating unmanned aerial vehicles, state estimation is a challenge due to inherent couplings, nonlinearities, and uncertainties, so that the extended Kalman filter might not be applicable. The other form of this filter that includes two options of the unscented Kalman filter, is employed to address the state estimation problem [16]. The improvement in estimation accuracy, overall control performance, and algorithm execution time compared to the other extended Kalman filters is solidly confirmed. In the other approach [17], a magnetic and inertial measurement unit provides raw data consisting of noise, bias, and drift, to feed into the motion controller of Quadcopter. The data fusion algorithms from the Madgwick, Mahony, and extended Kalman filter are competitive to significantly reduce these errors. Though, this research does not reveal the superior performance in real-world applications. Moreover, the magnetic measurement from low-cost sensors is inappropriate for precise control in autonomous aerial vehicles.
For a linear-like model, the Complementary filter is efficient concerning noise and the gyro bias estimation. This -order direct and passive filter combined with Three Axis Attitude Determination (TRIAD) scheme is proposed to launch an attitude estimation solution [18]. In the open-source platform [19,20], this kind of filter becomes the feasible method to determine the whole system's dynamics. Owing to the efficient characteristics, the built-in filter could be very easy to tune so the accurate measurement would be considerably cheap. For advanced applications, a novel attitude representation consisting of four-dimensional parameters is recommended for an intuitive interpretation [21]. Particularly, a complementary filter based on the new representation is rather natural than the filter that has magnetic separation property where pitch and roll angles are not affected by the magnetic disturbances. In summary, most of the previous works in this field are totally mentioned in Table 1. It is necessary to emphasize that the gyroscope provides inaccurate and less noisy measurement. Nevertheless, due to the integral factor, the cumulative error occurs, and the result tends to be drifted, even when the system locates in the original point. The gyroscope's results are only available in the short term. An accelerometer cannot evaluate the correct yaw angle and is sensitive to vibrate when external forces appear. Therefore, the utilization of filters that synthesize the signals from both gyroscope and accelerometer is to carry out the proper driving commands. In any mission, the attitude estimation is crucial for Quadcopter to maintain the tracking trajectory. Nevertheless, there is no research topic to discuss the comparative performance between Kalman filter and Complementary filter for such a case.

Angular estimation of aerial system
In order to use the BoosterPack module, it must be calibrated to the center of gravity (CoG) of the system. The rotation matrix of the system is described: (1) = cos cosΨ −cosΦsinΨ + sinΦsin cosΨ sinΦsinΨ + cosΦsin cosΨ cos sinΨ cosΦcosΨ + sinΦsin sinΨ −sinΦcosΨ + cosΦsin sinΨ −sin sinΦcos cosΦcos , where Φ, , Ψ are roll, pitch, and yaw angles, respectively. Assume that is fixed to the earth inertial coordinate, is parallel to and located at the CoG of system, is tied to the body of system and located at its CoG. Due to a small Euler angle, the angular velocity of system in the coordinate is as follows: (4)

Accelerating estimation of aerial system
Consider that the system has a linear acceleration and is put in gravitational acceleration. Assume that linear acceleration is very small ( ≈ 0), and BoosterPack has placed its -axis along with gravitational acceleration direction. The rotation matrix is called as : Hence, Euler angles can be evaluated from roll, pitch and yaw sensor readings: As a result, the rotation matrix is written as follows: The values of roll and pitch angle can be estimated as follows: = arctan −

Complementary filter design
To overcome the drifted problem in the gyroscope and to lessen high-frequency noise from an accelerometer, a Complementary filter is recommended to evaluate angles based on the variation of speed from a gyroscope. Later, it is added with angles from the accelerometer as Fig. 1(a). It is considered to have small values, and the estimation of roll value Φ and pitch value are known, respectively: The superiority of the Complementary filter mainly concentrates on the parameter . In this filter, a delay time happens in the output signal (regularly because of the low-pass filter of accelerometer and high-pass filter from gyroscope). The more value is near to 1, the better quality this filter has. However, the estimation of the sensor readings, which are varied fastly, takes a longer convergent time. It requires to take much effort to estimate the yaw angle directly from an accelerometer, so presently, we still apply values inferred from a gyroscope and accept the drifted phenomenon. By using more magnetic sensors, this problem will be solved.

Kalman filter design
The Kalman filter's basic block diagram, which is based on the Complementary filtering principle, is given in Fig. 1b. Vector = is in the input state, including angle ( ) and angular velocity ( ). The system state is described below: where is the vector control inputs. is the state transition matrix applied to each system state on the system state . is the control input matrix that applied each control input parameter in the vector .
is the process noise terms for each parameter in the state vector. Furthermore, and ̂ are the sensor output and its estimation. is the Kalman gain matrix. The process noise is assumed to be drawn from a zero-mean multivariate normal distribution with covariance given by ( )~(0, ).
By several measured approaches, at time , an observation of the true state is made according to the below formula: where is the transformation matrix that maps the state vector parameters into the measurement domain. is the observed noise assumed to be zero-mean Gaussian white noise with covariance given by ( )~(0, ).

Motion analysis for aerial vehicle
It is considered that Quadcopter is a solid object to be abided by forces such as the total force of motors and moments. In Fig. 2, a model of a quadrotor is analyzed. Each motor produces a force to lift up and a moment to turn the Quadcopter. The gravitational force is usually located at the origin and is directed towards the center of the earth.
Matrix : is the rotation matrix from frame to frame : Position vector in frame and linear velocity vector in frame are related as follows: Matrix is defined to perform the relationship between the angular velocity vector in frame and frame : Using the Newton's 2nd rule to the translational motion, the dynamic equation of system is analyzed [19]: where = ( , , ) , = ( , , ) and applied torque = . From the Newton's 2nd rule to rotational motion, the following can be gotten: Assume that aerial vehicle has symmetric architecture and axes in coordinate are the same with inertial axes of Quadcopter: Generally, the gravitational force is determined as = 0 0 . Similarly, we can have gravitational force in frame as = , = −sin sin cos cos cos . The quadrotor suffers forces from 4 Brushless DC (BLDC) motor to form total upward thrust force in frame as = 0 0 − with as the thrust factor: Therefore, the total force that acts on Quadcopter is summarized: Then, torque value is achieved by applying the Newton's 2nd rule to rotational motion: where = is the angular moment. The Coriolis equation is applied to evaluate the time derivative of the angular moment in the body coordinate.
From Eq. (23), the torque that demonstrates a model of the quadrotor is specified as follows: The length from to BLDC motor, d is a drag element, and Ω , Ω , Ω , Ω are angular velocities of motor 1, motor 2, motor 3, and motor 4, respectively.
To unify, the total force is applied to frame as follows: Later, from Eq.
where , , , are control signals: There is air resistance, which affects the quadrotor in the outside environment. Thus, the air drag force can be counted as: where , , and are drag coefficient, the mass density of the air flow, reference area of the object, and the object's speed relative to the air flow correspondingly. In reality, the actual speed endures the limited barriers that cannot be overcome as follows:

Results of numerical simulation
In this section, all simulations are verified with parameters as in Table 2. In the Complementary filter, varies from 0.96 to 0.98. Also, is 0.6~0.8 for Kalman filter. The motion of the Quadcopter in the z-axis is illustrated as follows: To track the motion on the -axis, the roll angle and pitch angle must equalize to zero. The force which is provided by each motor is computed as = 3, 2.9, 81/4 = 7,848 N. Hence, a force to lift up must be larger than . In this case, = 4.8 = 32 N is chosen. This force should be smaller than = 50 N in each motor. The simulation result in the -axis is shown in Fig. 3(a). If pitch angle and yaw angle are zero, roll angle is approximately 5 degrees; then Quadcopter moves along the -axis. The motion of the quadrotor in Fig. 3(b) and Fig. 4(a) is demonstrated below. From the results, settling time is 1,73 s approximately, and overshoot is 32,6 % which must be less than ±3 o ): In this case, the system tries to achieve the predetermined attitude on the -axis during the first 10 s. Therefore, the slope of the position graph on the -axis is low. Similarly, the roll angle and yaw angle are zero. Pitch angle equals -5 degrees, and then the drone moves along the -axis. The Trajectory of Quadcopter in Fig. 4(b) and Fig. 3(c) is computed as follows. It is measured that settling time is 1,59 s roughly and overshoot is 27,3 %: With out best knowledge, to achieve the better performance, the tuning process of filter is reasonably based on our experiences. For the pitch angle estimation, the component in high frequency is from the integrating pitch rate of gyro output while using the sensitivity of accelerometer to gravity direction is for the one in low frequency. The gain is tuned so that the estimated angle is wholly compensated in steady state. Similarly, the trial-and-error approach is appropriate to adjust the proper values for covariance matrix in Kalman filter. Using the above constraints, it is programmed in Matlab environment to mimic the performance of aerial vehicle in 3D space as Fig. 5(a). To imitate the real-world circumstance, the Gaussian function that plays as noise, is added to the system model. The output responses of velocity, acceleration and compass are gained in Fig. 6 in respect to data from gyroscope, accelerometer and compass measured sensor. Consequently, it can be understood clearly that these results of proposed design are reasonable and feasible.

Results of experimental validation
In these experiments, the previous section's design is employed to verify as follows: open-loop control scheme in , , and -axis, PID control scheme in roll angle to drive left or right, and PID control scheme in pitch angle to drive forward or backward. The gains , and of PID controller are 1.3, 0.004 and 18 respectively. To control the exact yaw angle is not a topic in this research. BoosterPack module is attached to the drone and is supervised by mobile and host through GUI. , for Complementary filter, varies from 0.96 to 0.98 and , for Kalman filter, is 0.6~0.8 so that the sensor achieves good performance. As a result, they are selected as the best suitable values ( = 0.9725, = 0.6137). In the initializing stage, the CPU sets predetermined values for inputs, outputs, and peripheral devices when power is on. The main program will run in a loop as order; i.e. (1) Read raw data from the attached sensors; (2) Process data via sensor fusion filter (Kalman filter or Complementary filter); (3) Read RF signal from a remote controller, combine with handling values from the filter, these signals are inputted in the PID scheme to compute pulses for motors; (4) After delivering pulses to ESC, CPU transfers feedback data to a ground station.
In Fig. 7(a) and Fig. 7(b), the Complementary filter gets values of the gyro as major factors and accelerometer values as minor sources. Hence, the fluctuation which is created by both motors and propellers, indicates the accelerometer's angles. The evaluated angle from Complementary filter is more constant than that of the Kalman filter. Nonetheless, the drift phenomenon of about ±0,5° still remains. On the other hand, the Kalman filter's performance in Fig. 8(a) and Fig. 8(b) obtains the angles from an accelerometer to calculate the practical angle. It mentions the fact that this filter carries out more precise values than it was expected. However, the pulse modulation signal to each driving motor belongs to the vibration of the system. In order to examine the balancing capability on one axis, the experiment in Fig. 9(a) is performed to investigate the hardware of the system. This aerial system is suspended on a stick, and the controller tries to keep the fixed angle of 10, 30 and -35 degrees. In Fig. 10, several angular values are obtained by testing on one axis. The system testing is started in 1.5 s and the system is balanced in 5 s. Later, a stick is turned to the left, the point is balanced during 3 s. The AAS retains the predetermined angle within 8 s. a) b) Fig. 9. Experimental balancing test on a) one-axis and b) six-axes In the test scenario of six-axes motion shown in Fig. 9(b), four wings are free, and the body frame is hung by a shaft in order to evaluate the balancing ability. In Fig. 11, the experimental results of tracking angles are completed by Complementary filter correspondingly whilst Fig. 12 reveals the errors of angular values. At the sequence of 3 s, the whole system is turned on. After a few seconds, the aerial vehicle is elevated from the ground and reached to a stable position. Again, in Fig. 13 and Fig. 14 autonomous aerial vehicle can slightly fluctuate around the equilibrium point with ±3 degrees, though it bears the external disturbances in the outdoor surrounding. From the above results, it can be understood easily that theComplementary filter is more advantageous than the Kalman filter in the view of the simple implementation and easy understanding. However, the filtering results from the Kalman filter produce better precision and stable performance. There is a trade-off between the Complementary filter and Kalman filter when they are integrated into the autonomous system. In Fig. 5(b), quadrotor performs well while the PID controller adapts with excellent tuning parameters based on the experiences of operator.  The robustness and accuracy of filter are ensured The rapid response of this method should be considered due to vision data Zhao L. et al [32] Using both data fusion technique and Kalman filter to estimate the nonlinear state with delayed measurements is the aim of this study The faults in speed and accuracy in the nonlinear polymerization process is compensated It can not adapt with rapid response and complex high-dimensional process Our approach Validation of simulation performance as well as experimental outcomes between two well-known filters has been presented The output results might be a solid evidence for learners and practitioners to explore the aerial system It requires more investments on gain-varying using intelligent scheme To summarize the technical specifications, the competitive descriptions among related researches have been indicated in Table 3. The outcomes of each approach were noted to illuminate the main advantages and the applied fields.

Conclusions
In this paper, the comparative investigation of Complementary filter and Kalman filter for the aerial vehicle control is made. Firstly, a theoretical model is applied to estimate the orientation and acceleration. Then, the developmental theory of two filters is launched to realize the innovative idea. These works are later employed in the platform of AAS to insist on major factors in both filters while the kinematics and dynamics of the quadrotor are analyzed in terms of implementation. Based on the numerical simulation and experimental tests, the proper selection of effective filters for low-cost, feasibility, and practical applications in the environment, automation, and agriculture has been validated.
Future perspectives shall be mentioned. From these results from both simulation and experiment, the data fusion has a potential to be investigated among different sensing instruments such as digital camera, inertial sensor and positioning sensor in order to enhance the system performance. In the other approach, the advanced control scheme that automatically adjusts the tuning gains of filter should be noticed. Moreover, the integrating design of both Kalman and Complementary filter into one platform may be more advantageous.