You are on page 1of 6

2017 International Conference on System Science and Engineering (ICSSE)

Experimental Comparison of Complementary Filter and Kalman


Filter Design for Low-Cost Sensor in Quadcopter
H.-Q.-T. Ngo, T.-P. Nguyen, V.-N.-S. Huynh, T.-S. Le and C.-T. Nguyen

Abstract- Most of quadcopter operate in the outdoor implementation. Unfortunately, data from accelerometer is
environment where is often complex and unpredicted. In order not excellent to estimate the gravitational direction if
to control the quadcopter in unknown outdoor environment, it dynamics of air vehicle is large enough.
should be designed an excellent filter to estimate complete state
vector which illustrates the movement of rigid body. In this In this paper, Kalman filter and Complementary filter are
paper, two filters such as Complementary and Kalman are implemented in quadcopter to visualize the performance. The
investigated to compare the performance. Quadcopter collect key contribution is to build a routine of filtering selection
only measurements from a low-cost inertial measurement unit, technique. The rest of this article is structured as following.
IMU-MPU6050. The raw data is put into filter to estimate the Section II performs the implementation of low-cost IMU-
state vector in system. In the simulations, these filters are MPU6050 sensor. In Section III, the contents focus on theory
studied to implement in the model of quadrotor. Later, the of Kalman filter and Complementary filter basically. Later,
hardware platform of quadcopter is built to denote modeling analysis of quadcopter is presented in Section IV.
experimental results in order to validate the effective design of Then, several experiments in Section V is setup to validate
Complementary filter and Kalman filter in quadcopter. our contributions. In Section VI, conclusions are carried out.
Keywords- Quadcopter, Complementary, Kalman II. IMPLEMENTATION OF IMU-MPU6050
I. INTRODUCTION A. Estimated Orientation
Although quadcopters become more popular not only in MPU6050 is calibrated to center of gravity (CoG) of
affordable applications but also in academic research because system. The rotation matrix of system:
of its easy and cheap design, it is still active topic for
researchers during many years. State estimation is addressed
as one of traditional problems in quadcopter. There are large
literatures on filtering techniques to estimate parameters of
quadcopter. Many advanced filter, such as particle filter, are
CBC\f -c<l>s\f + s<l>sBc\f s<l>s\f + c<l>sBc\f 1
computational burden and suitable for powerfull processor in =[ cBs\f c<l>c\f + s<l>sBs\f -s<l>c\f + c<l>sBs\f
mainboard. Two common methods that are often used are
- sB s<l>cB c<l>cB
Kalman filter and Complementary filter due to its frequency
filtering characteristics in linear system [1]. Various studies
focus on Kalman filter [2, 3,4] in the flight control field. For
Where <D, e, 'P are roll, pitch and yaw angles respectively
the air vehicle attitude estimation, authors [5] modify the Consider that RJ is attached to earth inertial frame, R2
standard Kalman filter to update measurements. The parallel to RJ and is placed at the CoG of system, R3 is
estimated values are calculated using a multiplicative attached to the body of system and also placed at CoG of it.
extended Kalman filter. The estimation receive data from
The angular velocity of system (R3):
accelerometers, gyroscopes and GPS. Researchers in [6]
proposed an unscented Kalman filter using the three-axis gyroRate = n = (nx3 , n y3, n z3 ) (2)
attitude detennination algorithm as the observer. It works
efficiently, but needs computational power considerably. This where
filter is also used to track the motion [7, 8, 9] . The filter
o
processes data from small inertial/magnetic sensor.
However, it is difficult to apply this filter robustly [10]. In cos<l>
-sine
sin <l> cos e ~
][<D] (3)
recent work, many practitioners use two complementary -sin<l> cos <l> cos e \Jf
filters, i.e. direct complementary filter and passive
complementary filter, to obtain high quality attitude Euler angle IS small, therefore, equation (3) can be
extraction and gyros bias estimation. In this approach, both rewriten as:
filters can be explicitly expressed in quaternion form for easy

Ha Quang Thinh Ngo is with Bach Khoa University (BKU), Ho Chi


Minh city, Vietnam (phone: +84-8-3868-8611 ; fax: +84-8-3865-3823, (4)
e-mail: nhqthinh@hcmut.edu.vn).
Thanh Phuong Nguyen is with Ho Chi Minh University of Technology
(HUTECH), Ho Chi Minh city, Vietnam.
Tan Sang Le, Van Ngoc Son Huynh and Chi Tong Nguyen are students
in Bach Khoa University (BKU), Ho Chi Minh city, Vietnam.

978-1-5386-3422-6/17/$31.00 ©2017 IEEE 488


2017 International Conference on System Science and Engineering (ICSSE)

B. Estimated Acceleration
ACC
<D xvz = arctan ( - ) (13)
Assume that the system is attached by a linear - y
acceleration ar and placed in gravitational acceleration. R is . Accz
rotation matrix.

ACCX ] o = arctan [ ~ -Accx J (14)


Acc = [ Acc y = R (g - a,. ) (5)
xyz 2
Accy + Acc 2
z

Accz III. FILTERS DESIGN FOR MPU6050


Consider that linear acceleration is small (ar ;::: 0). The role of gyroscope is to measure accurately and less
MPU6050 is placed its z axis along with gravitational noisy if external forces impact to system. However, based on
acceleration. the calculation by integral factor, error is cumulative over
time and measurements tend to be drifted, not return to 0
even system is in its original point. Hence, gyroscope's
ACC ] = R x g = R 0
Acc = [ Acc y
X
[0] (6)
values are only reliable in short-term. Due to MEMS
structure of accelerometer, it is sensitive to vibrate when
Accz 1 external forces appear. Accelerometer cannot calculate yaw
angle and results aren't drifted over time because of lacking
Euler angles can be estimated from roll, pitch and yaw of integration. These values are suitable for a long-time
value of sensor. period. Before using the filter, sensor is needed to reduce its
maximum vibration by placing additional spacers separating
0 the device from frame for damping and vibrationless. To

R.($)=[~ cos <D


-sin <D
Si~$
cos<D
] (7)
lessen the amplitude of noise at high frequency which effects
to the raw data, it is necessary to apply the built-in low-pass
filter in MPU6050 with cutoff frequency of accelerometer
and gyroscope that are 44Hz and 42Hz correspondingly.

Ry(0)
[ cosO
= 0
0
1
-S:O] (8)
A. Design of Complimentary filter
To handle the drifted phenomenon in gyroscope and
reduce high frequency noise from accelerometer,
sinO 0 cosO Complementary filter is applied to estimate the angle based
on the change of speed from gyroscope. Then, it is combined
with angle from accelerometer as Fig. 1. Assume that with
sin 'I'

~]
small angle, the estimated roll value (<I» and the estimated
cos 'I' (9) pitch value (9) are determined respectively.

o Low-Pass Fi~er

Then, from equation (6), we obtain the following.

Figure I. Scheme of Complementary filter

-sinO ] <Destimate(n) = <D(n -1) + ~<D :=:; <D(n -1) + Q x3 (15)


= [ cos 0 sin <D (11)
cosOcos<D <D( n) = }'<l> estimate (n ) + (1 - r) <D accelerometer (n ) (16)

It means that the values from sensor can be achieved by 0estimate (n) = O(n -1) + ~O :=:; O(n -1) + Q y3 (17)
these parameters.
O( n) = rOestimate (n) + (1- r) 0accelerometer (n) (18)
-sinO ]
0.5::; r::; 1
II~~~II [
= cos 0 sin <D (12)
cosOcos<D It can be seen clearly that quality of Complementary filter
depends mainly on parameter y. Output signal from this filter
From above equation, roll and pitch angle are calculated. has a delay (mostly due to low-pass filter of accelerometer
and high-pass filter from gyroscope). The more y value is

489
2017 International Conference on System Science and Engineering (ICSSE)

close to 1, the better quality this filter has. But the convergent IV. KINEMATICS AND DYNAMICS OF
time is long when sensor is quickly changed. Yaw angle QUADCOPTER
cannot infer directly from accelerometer, so currently we will
use value inferred from gyroscope and accept the drifted A. Kinematics
phenomenon. This problem will be solved when using Assume that quadcopter is a solid to be suffered by a
additional magnetic sensor. force, that is total force of motors, and three moments, that
turn it in roll, pitch and yaw angle. In Fig. 3, a model of
B. Design of Kalman Filter quad-rotor is analyzed. Each motor provides a force Ti to lift
Kalman filter, as they are applied in navigation systems, up and a moment Mi to rotate quadcopter. The gravitational
are based on Complementary filtering principle. The basic force is often toward to center of the earth.
block diagram is given in Fig. 2. The inputs of this filter
include angle (x) and angular velocity (x) which are
described as following.

x, =[:1 (19)

Therefore, the system state at time k is determined from


the state at time k-1.

0/
M,

Uk is the vector containing control inputs. A is the state


transition matrix which is applied the effect of each system
state X k-1 on the system state X k. B is the control input matrix
l~
which is applied the effect of each control input parameter in
Figure 3. Analysis of Quadcopter Model
the vector Uk. O)k is the process noise terms for each
parameter in the state vector. The process noise is assumed to
be drawn from a zero mean multivariate normal distribution
N with covariance given by P(O)) ~ N(O, Qk). {CSv} = [Xv,Y v' zv] are the inertial frame, body frame and
By some measured methods, at time k, an observation Zk vehicle frame correspondingly. x,y,z which are alternately
of the true state Xk is made according to. coordinates in centroid of quadcopter model, correspond
alternately XE'YE,ZE axes in coordinate system {E}.
(21)

H is the transformation matrix that maps the state vector Matrix R%: is rotation matrix from frame {B} to
parameters into the measurement domain. V kis the observed frame{E}
noise which is assumed to be zero mean Gaussian white
noise with covariance given by P(V) ~ N(O, Rk).
S(J)S(}CljI- c(J)sljI c(J)s(}cljI + S(J)SljI]

Xaccel s(J)s(}sljI + c(J)cljI c(J)s(}cljI- s(J)cljI (22)


s(J)c(} c(J)c(}
x
Matrix R% : is rotation matrix from frame {E} to frame
{B}.

Figure 2. Scheme of Kalman filter


R%((J),B,IjI) = R%((J) , B, 1jI f (23)

Qk and Rk are half-defmed positive symmetric matrix The relationship between position vector ~E in frame
representing for the process and measurement noise. The {E} and linear velocity vector VB in frame {B} is shown as
quality of Kalman filter depends on Qk and Rk . However, the
follows.
ratio Qk / Rk plays more important role in the filter. The
smaller the ratio is (Rk --+ Qk), the later the fluctuation
amplitude of filter signal is. Otherwise, the larger the ratio is
(Rk --+ 0), the greater the fluctuation amplitude is and the (24)
shorter the delay time as well as the convergent time is.

S(f:JSOClf! - c(f:JsljJ
X] [ COOf!
c(f:JsOcljJ + S(f:JS1f! ] [U ]
[ ~ = cOSIjJ s(f:JsOSIjJ + c(f:JCIjJ c(f:JsOCIjJ - S(f:JClf! v (25)
Z - sO s(f:JcO c(f:JcO w

490
2017 International Conference on System Science and Engineering (ICSSE)

Matrix T: is computed to show relationship between I o: 0


angular velocity vector VB in frame {B} and frame {E}. J = [ 0 I yy (38)
o 0

(39)

V. EXPERIMENTAL LABORATORY
In these experiments, MPU6050 is set-up on the drone
and is supervised by host through GUI in Fig. 4. y, for
Complementary filter, varies from 0,95 to 0,99 and R, for
From that, matrix T can be obtained. Kalman filter, is 0 , 5 ~0 , 7 so that sensor achieves good
performance. As a result, they are selected as the best suitable
seD to ceDtO ] value (y=0,9996, R=0,5721).
ceD - seD (28) ~ COMPortTerminal - ...!!!!!!
seD / cO ceD / cO COIolPorts.tlinga

B. Dynamics
In the Newton's second rule to the translational motion,
we have equations [11].
lIottery 5 15 1.1_ 2

(29)

(30)
. B) = R.F B
m. ( R.v'B + R.v (31)

(32) Figure 4. Graphical User Interface on host

[ill~ =
[rv-qW] [I,]
pw-ru + ~ 1;, (34)
QJ

g> -0.05
<t
w qu- pv fz '().1

-0.15
vB=(u,v,wf and {j)B=(p,q,rf From
'().2 ' - - - - ' - - - " ' - - - - - - ' - - - " ' - - - - - - ' - - - " " - - - - - - - '
Newton' s law to rotational motion, we get: o 5 10 15 20 25 30 35
Time (sec)
TB = J.til + 0/ x ( J.OJ B) (35) Figure 5. Performance of Roll and Pitch Angle by Complementary Filter

15~.---r--~--r--~--r--~-_,
(36)
1515 - - Molorl
- - Molor2
1510 MOlor3
Consider that structure of quadcopter is symmetry and - - MOlor4
axes in {B} coincide with inertial axes of quadcopter. ~ 1505
~1500 ~"v
Iyy - I zz T<I> &1495
qr .
In In 1490

m=
I
pr zz -Ixx To 1485
+ (37) 1480 L..-_--'-_ _'--_--'-_ _'--_--'-_ _' - - _ - l
IyV Iyy o 10 15 ~ 25 30 35
Time (sec)
IU -Iyy 3:...
pq .
I zz I zz Figure 6. Performance of Speed by Complementary Filter

491
2017 International Conference on System Science and Engineering (ICSSE)

In Fig. 5 and Fig. 6, Complementary filter gets values of From these results, it can be seen easily that Complementary
gyro as major factors and values of accelerometer as minor filter is more superior than Kalman filter in this test case. In
factors. Hence, the oscillation (caused by motors, propellers) Fig. 17, quadcopter works well when the PID controller runs
that reflects into angles of accelerometer would not impact on with the good tuning parameters based on the experiences
the system so much. The estimated angle of Complementary such as K p= I,3; K,=0,004 and K D=18.
filter is more stable than that of Kalman filter. Actually, it
still exists the drift phenomenon (approximate ±0,5°). In
contrary, the performance of Kalman filter in Fig. 7 and Fig.
8 receives angles of accelerometer to estimate the real angle.
It leads to the fact that this filter appreciates more precise
values. However, the pulse modulation to each motor
depends on the vibration of system.

0. 15 1--,..--...----.---.---..--;::=:::;~

0.1
I == :~~ I

e.
Cl
0.05 Figure 9. One-axis Balancing Test Case
~
OJ
c:
«
~~~~--~--~--~--~==~
-0.05 30
20
I== :I~~ I
-0.1 ' - - - - - ' - - - - ' - - - - ' - - - ' - - - - ' - - - - ' - - - ' en 10
o 10 15 20 25 30 35 8
Time (sec) -; 0
c.
Figure 7. Perfonnance of Roll and Pitch Angle by Kalman Filter ~ -10

-20

-30
-40'-----'---~--~---'-----'---~
o
20 ~ 60 80 100 120
Time (sec)

Figure 10. Angular Results by Kalman Filter on One Axis

Speed Moto r
1700
- - Molor l
1600 - - Molor 2
MOlor 3
1500 - - Molor 4
::?:
5 10 15 20 25 30 35 ~ 1400
Time (sec) ell
-3 1300
0..
Figure 8. Performance of Speed by Kalman Fil ter 1200

1100
In order to examine the balancing capability on one axis,
the experiment in Fig. 9 is built to investigate the hardware of 1000
0 20 60 80 100 120
system. This quadcopter is suspended on a stick and the ~
Time (sec)
controller try to keep the fixed angle such as 10, 30 and -35
degree. In Fig. 10 and Fig. 11, several angular and speed
Figure 11. Speed Results by Kalman Filter For 4 Motors
results are obtained by Kalman filter. The system is started
testing at the 17s and balancing at the 40s. Later, a stick is
turned to left the balancing point during 5s. Quadcopter
retains the pre-determined angle from 45s.
Later, quadcopter is hung up by four ropes to assess the
balancing ability on 6 DOF as shown in Fig. 12. The angular
and speed test results are done by Complementary filter in
Fig. 13 and Fig. 14. The overall system is activated at the 5s.
After a few seconds, quadcopter is lifted from the ground and
stabilized in this position. The similar test conditions are
applied for Kalman filter in Fig. 15 and Fig. 16. Quad-rotor is
able to hold the equilibrium with ±3 degree although it
Figure 12. 6 DOF Balancing Test Case
suffers the external disturbances in outdoor environment.

492
2017 International Conference on System Science and Engineering (ICSSE)

-3

-4~----~----~----~----~----~----~----J
Figure 17. Experimental Test in Outdoor Environment
o 10 20 30 40 50 60 70
Time (sec)
VI. CONCLUSION
Figure 13. Angular Results on 6 DOF by Complementary Filter
In this paper, an application of Complementary filter and
Kalman filter in quadcopter is studied. The method to work
and compute the output for IMU-MPU6050 is performed in
1800
order to insist on major factors in both filters. The
kinematics and dynamics of quadrotor are analyzed to
implement. Hardware and software design are completed to
~ 1600
carry out several experimental results. From these outcomes,
5l the effective filter is proposed for low-cost IMU-MPU6050
~ 1400
in the flight control technique.
1200
REFERENCES
10 00~~--~----~----L-----L-----~--~~----J [I] R. Mahony, T. Hamel and J.-M. Ptlimlin, "Nonlinear complementary
o 10 20 30 40 50 60 70 filters on the special orthogonal group," in IEEE Trans. on Automatic
Time (sec) Control, vol. 53 , no. 5, 2008, pp. 1203- 1218.
Figure 14. Speed Results on 6 DOF by Complementary Filter [2] D. Hetenyi, M. G6tzy and L. Blazovics, "Sensor fusion with enhaned
Kalman Filter for altitude control of quadrotors," in iEEE ii th
Tnternational Symposium on Applied Computational intelligence and
6 r---~--~--~---r--~--~--~----~--~--' Informatics (SACI) , 2016, pp. 413-418.
[3] H. Beck, 1. Lesueur, G.-c. Arcand, O. Akhrif, S. Gagne, F. Gagnon
4
and D. Couillard, "Autonomous takeoff and landing of a quadcopter,"
2
in In ternational Conference on Unmanned Aircraji Systems (lCUAS) ,
c; 2016, pp. 475-484.
~ O~----~~~~~ INIII [4] K.-D. Sebesta and N. Boizot, "A real-time adaptive high-gain EKF,
Applied to a quadcopter inertial navigation system," in International
'"
c, -2 IEEE Trans. On Tndustrial Electronics, vol. 61 , no. 1, 2014, pp. 495-503.
~ [5] J. K. Hall, N . B. Knoebel and T. W. McLain, "Quatemion attitude
-4
estimation for miniature air vehicles using a multiplicative extended
-6 Kalman filter," in IEEE on Position, Location and Na vigation
Symposium, 2008, pp. 1230- 1237.
[6] H. G. De Marina, F. J. Pereda, J. M. Giron-Sierra and F. Espinosa,
5 10 15 20 25 30 35 40 45 50
" UA V attitude estimation using unscented kalman filter and triad ,"
Time (sec)
in IEEE Transactions on Tndustrial Electronics, vol. 59, no. 11 , 2012,
Figure 15. Angular Results on 6 DOF by Kalman Filter pp.4465-4474.
[7] X. Yun and E. R. Bachmann, " Design, Implementation and
Experimental Results ofa Quatemion-based Kalman Filter for Human
Body Motion Tracking," in IEEE Transactions on Robotics, vol. 22,
no. 6, 2006, pp. 1216-1227.
[8] H. Q. T. Ngo, Q. C. N guyen and W. H. Kim, "Implementation of
1800
input shaping control to reduce residu al vibration in industrial network
motion system," International Conference on Control, Automation
~ 1600 and Systems, Busan, Korea, 2015 , pp. 1693-1698.
11.
[9] H. Q. T. Ngo, Q. C. Nguyen and T. P. Nguyen, " Design and
~
11.
1400 Impl ementation of High Performance Motion Controller for 2-D Delta
Robot," Seventh international Conference on information Science and
1200 Technology, Da Nang, Vietnam, 2017, pp. 129-134.
[10] R. Mahony, T. Hamel and J.-M. Ptlimlin, "Complementary filter
1000 '--__-'-'__-'-__-'-__-'-__-'-__--'__ ---''--.l.-~ __~___J design on the special orthogonal group SO(3)," in Proc. Of 44th IEEE
o 10 15 20 25 30 35 40 45 50 Con! On Decision and Control and the European Control
Time (sec) Conference, 2005, pp. 1477-1484.
[II] V. K. Tripathi, L. Behera and N. Verma, "Design of sliding mode and
Figure 16. Speed Results on 6 DOF by Kalman Filter backstepping controllers for a quadcopter", in 39th National Systems
Conference, 2015.

493