2 views

Uploaded by Ouafo Mandela Brel

- Estimation of Nonlinear Dynamic Systems
- Kalman Filterin
- Use It Free - Instantly Knowing Your Phone Attitude
- LKF2DyTransitMatrix_v5
- Robotics1_SS09.pdsd;sd;skd;f
- 10_chapter 2.pdf
- Quadrotor Unmanned Aerial Vehicle - Inertial Measurement Unit, Kalman Filter, PID controller
- 81634
- QuadCopter Flight
- CFD.pdf
- Easa Instrument Sys
- 3D Shooting Game
- General Motion of a Rigid Body, Motion of a Billiard Ball - Griffith
- Gesture Report
- Light Detection and Ranging_Paper
- 9
- 1
- MPU 9250 Registers
- 10.1.1.14
- incerteza.pdf

You are on page 1of 6

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

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.

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.

Acc = [ Acc y = R (g - a,. ) (5)

xyz 2

Accy + Acc 2

z

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

-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

= [ 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)

the state at time k-1.

0/

M,

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]

s(J)c(} c(J)c(}

x

Matrix R% : is rotation matrix from frame {E} to frame

{B}.

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)

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)

[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)

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

- Estimation of Nonlinear Dynamic SystemsUploaded byfaramarzsamani
- Kalman FilterinUploaded byMisty Brown
- Use It Free - Instantly Knowing Your Phone AttitudeUploaded byArdhiansyah Baskara
- LKF2DyTransitMatrix_v5Uploaded byFarhan
- Robotics1_SS09.pdsd;sd;skd;fUploaded bymangal_284
- 10_chapter 2.pdfUploaded byArøn Par
- Quadrotor Unmanned Aerial Vehicle - Inertial Measurement Unit, Kalman Filter, PID controllerUploaded bySagar Medikeri
- 81634Uploaded byJohnSmith
- QuadCopter FlightUploaded byasso2412
- CFD.pdfUploaded byAzam Roslan
- Easa Instrument SysUploaded byadityarana531
- 3D Shooting GameUploaded byVivek Chand
- General Motion of a Rigid Body, Motion of a Billiard Ball - GriffithUploaded byluis199555
- Gesture ReportUploaded byFernando Her R
- Light Detection and Ranging_PaperUploaded byEdwin
- 9Uploaded byArnav Guddu
- 1Uploaded byjakk
- MPU 9250 RegistersUploaded byAguaForte
- 10.1.1.14Uploaded bycarcava
- incerteza.pdfUploaded byRoger Danilo Figlie
- IMU Noise and CharacterizationUploaded byranchua1905

- Alternatives to Portland CementUploaded byPablo Moñino Lostalé
- HeliCoil Thread Repair KitsUploaded byAce Industrial Supplies
- High-Pressure, High-Temperature Well Logging, Perforating and TestingUploaded byDwi Apriliantono
- Kdenlive TutorialUploaded byalfikrinuralam8353
- Reference List - WA 2010Uploaded byidontlikeebooks
- EERDUploaded byHabtamu Hailemariam Asfaw
- OEE calc.xlsUploaded bychteo1976
- Oracle OPSM Creating Your Own PedigreeUploaded byJuNAiDSuAlEh
- Acids and Bases 09Uploaded byCikgu Anita
- (181536741) Panel 1-1 Briefing on ASEAN Power GridUploaded bylisaaliyo
- QT9_1SDC007108G0202Uploaded byJeyakumar
- Titan Medical Investor Presentation November 28 2018 Piper JaffrayUploaded bymedtechy
- Space Shuttle Mission STS-4Uploaded byAviation/Space History Library
- Hydraulic systems.docUploaded byJitendra Kumar
- How to Develop a Master Validation PlanUploaded bycompliance2go
- Www.vmware.com Tryvmware p Activate.php p=Vmware-workstation&Lp=1Uploaded byOnecio Araujo Ribeiro
- Bibliografia de Mecanica de FluidosUploaded byGerson Manuel Medina Mamani
- Latex Cheatsheet TemplateUploaded byHossam Nabil
- Cap DombooUploaded byTafadzwa Nyoni
- Radiator level alarmUploaded bytaaj77
- W310-A123-X-4A00_WS_SWT-2.3-82 VS.Uploaded byUhrin Imre
- Bentley Rm Bridge Advanced Detalii Module AditionaleUploaded byphanoanhgtvt
- Chapter 9 - Integrated Watershed ManagementUploaded bySudharsananPRS
- Total Gear Box SpecsUploaded byKoushik Saha
- Ric WhitepaperUploaded bylloyd6109
- ELEC 2607 Lab 3Uploaded byMehsen Kassem
- DX Sand Filter_techguidanceUploaded bycyruskulei
- John Ecklin - Free Energy MagnetismUploaded byHomers Simpson
- A. Inoue (Auth.), Professor Akihisa Inoue, Professor Koji Hashimoto (Eds.)-Amorphous and Nanocrystalline Materials_ Preparation, Properties, And Applications-SpringeUploaded byunknown1711
- AN ARCHITECTURE FOR SOFTWARE DEFINED WIRELESS NETWORKINGUploaded bydjpbowman