IFOST

2010 Proceedings

Modeling and Control of Quadrotor MAV

Using Vision-based Measurement
Ly Dat Minh

Cheolkeun Ha

Department of Aerospace Engineering

University of Ulsan
Ulsan, Korea
Idm147@yahoo.com

Department of Aerospace Engineering

University of Ulsan
Ulsan, Korea
cha@mail.ulsan.ac.kr

Abstract- In this paper we review the mathematics model of

quadrotor using Lagrange's equation. We propose a vision-based
measurement to stabilize this model. A dual camera method is
used for estimating the pose of the quadrotor; positions and

attitudes of the quadrotor MAV. One of these cameras is located

on-board the quadrotor MAV, and the orther is located on the

ground. The control system is developed in Matlab/Simulink. In
this paper, we consider a linear controller for our purpose.
Linear Quadratic tracking controller with integral action, and
Optimal Linear Quadratic Gaussian (LQG) control with integral
action are designed for stabilization of the attitude of the

quadrotor MAV. Moreover, some measurement noises will be

considered in the controller design, too. Finally, this paper will
demonstrate how well this control works for a certain flight

mission : quadrotor MAV on the ground, and then starting

hover, sideway move and keeping the position with a pointing to
a certain object fixed in space.

Keywords- quadrotor
hovering, control

I.

MA V,

pose

estimation,

vision-based,

INTRODUCTION

The purpose of this study is to explore control

methodologies and vision algorithms to develop an
autonomous miniature aerial vehicle (MAV). MAVs are
important when it comes to performing desired tasks in
dangerous or inaccessible environments. An autonomous MAV
brings enormous benefits and is suitable for applications like
search and rescue, surveillance, remote inspection, military
applications; therefore saving time, reducing costs and keeping
human pilots away from dangerous flight conditions.
Many research groups are interesting in MAV. There are
two types of MAV: ultra-light fixed-wing planes and rotary
wing aerial vehicle such as helicopter, quad-rotor and etc.
Ultra-light fixed-wing planes cannot hover so they require
large space to navigate. Rotary wing, however, can take
off/land in limited space and easily hover above any target.
Furthermore, rotary wing aerial vehicles are highly
maneuverable. A quad-rotor MAV is a four-rotor helicopter. A
quad-rotor MAV has two pairs of propellers turning in opposite
directions in order to cancel out the resultant moment found in
single-rotor helicopter. This eliminates the need for a tail rotor
to stabilize the yaw motions of the quad-rotor. In this paper, the
quad-rotor MAV has fixed pitch angle rotors and the rotor
speeds are controlled to produce the desired lift forces.

The Draganflyer, a commercial aerial vehicle shown in Fig.

1, is a radio-controlled four-rotor helicopter available from RC
Toys. It is a unit for studying the problems of realizing
autonomous flight of MAV. In this paper, we model and
analyze the dynamics of the Draganflyer in order to design the
controller to stabilize the attitude in hover. We present some
control methods such as optimal control techniques of LQ
(Linear Quadratic) tracking controller with integral action to
stabilize and control the quad-rotor model.
In order to create an autonomous MAV, precise knowledge
of the helicopter position and orientation is needed. In the
previous work involving autonomous flying vehicles, this
information was obtained from Inertial Navigation Systems
(INS), Global Positioning Systems (GPS) or other sensors like
ultrasonic sensor. Typically, mUltiple sensors are used to
overcome limitations of individual sensors, thereby increasing
the reliability and decreasing the errors. Vision sensors are
primarily used for estimating the relative positions of some
target. Limited payload capacity may not permit the use of
heavy navigation systems or GPS. Moreover, GPS does not
work in indoor. Vision information can also be used to stabilize
and hover the helicopter, and also to track a moving object. In
this study, we introduce vision algorithms of pose and motion
estimation using two-camera system [1].
The pose estimation problem has been a subject of many
research projects for many years. Most of the pose estimation
techniques are image-based and they fall into these two
categories: point-based methods and model-based methods.
Point-based methods use the feature points identified on a 2D
image while model-based methods use the geometric models
(e.g. lines, curves) and its image to estimate the motion. We are
interested in point-based techniques in this paper.
II.

MATHEMATICAL MODEL

Unlike regular helicopters that have variable pitch angles, a

quadrotor MAV has fixed pitch angle rotors and its rotor
speeds are controlled to produce the desired lift forces. Basic
motion of a quad-rotor MAV can be described in Figure 1.
A quadrotor MAV has two pairs of rotor (1,3) and (2,4),
turning in opposite directions. By varying the rotor speed, one
can change the lift force and create motion. Vertical motion can
be achieved by simultaneous change in rotor speed. Moving

978-1-4244-9037-0/101$26.00

2010 IEEE

IFOST

2010 Proceedings

left/right motion can be obtained by converse change of rotor

speeds 2 and 4. Similarly, forwardlbackward motion can be
obtained by converse change of rotor speeds 1 and 3. The yaw
motion is obtained by rotor speed imbalance between the two
pairs. A good controller should be able to reach a desired yaw
angle while keeping the tilt angles and height constant. There is
no change in the direction of rotation of the rotors.

In hover, control of the quadrotor MAV can be thought of

as achieving force and torque balance. Using Euler angles
parameterization, the airframe orientation in space is given by a
rotation matrix RB2E from B to E, where RB2E E S03 is the
rotation matrix.
PHYSICAL PARAMETERS DEFINTION

TABLE!.

Parameter

oF
2
Rotate ten

Mass [kg]

Arm length [m]

Roll angle [rad]

Rotl1te rtghl

Going up

Figure 1.

Description
2
Gravity [m / S ]

Mowt1ght

()

Pitch angle [rad]

If/

Yaw angle [rad]

Basic motion of a quadrotor; the arrow width is proportional to

Rotor speed (i

1,2,3,4) [rad / s]

1,2,3,4) [N]

F;

Force (i

r;

Thrust force (i

1,2,3,4) [N]

Drag moment (i

1,2,3,4) [Nm]

propeller rotational speed

Di

The following are some assumptions that are used in

developing the mathematical model of the quadrotor MAV.

Thrust constant

Drag constant

RB2E

Rotaion matrix from Body to Earth

2
Rotor inertia [kg.m ]

The center of gravity and the body frame origin are

assumed to coincide.

Interaction with ground or other surfaces is neglected.

JR

The blade flapping is not modeled.

Or

Friction is only considered in yaw motion.

The carbon-fiber structure is supposed rigid.

The structure is supposed symmetric (i.e. diagonal

inertia matrix).

Thrust and drag are supposed proportional to the

square of the propellers speed.

Let E = {X, Y, Z} denote an inertial frame, and

B {x,y,z} denote a body fixed frame. The axes of the rotors
are parallel to the Z axis in E. The coordinate frames of inertial
and body frames are shown in Fig. 2.
=

F4

Y..J.--.X
Z

Figure 2.

-O[

+02 -03 +04 [rad / s]

The Lagrange-Euler approach is based on the concept of

kinetic and potential energy :

( J

!!... OL
dt oq,

qi

oL
=r '
oq,

(1)

L=T-V

: generalized coordinates

ri : generalized forces given by non-conservative forces

T: total kinetic energy
V: total potential energy
The total kinetic energy is defined as
2

1
1
l .. e2
A.
A.
=-1
('f'-lJIsm
) +... (2)
) +-1
( e cos'f'+lJIsm'f'cos
e2
F2

mg

1 IEv2 dm
T=-

Or

Quadrotor coordinates system

978-1-4244-9036-3/101$26.00 2010 IEEE

tt

1
A.
A.
)
+-1
(-e sm'f'+lJIcos'f'cos
e2
2

zz

And the potential energy can be expressed by

V= JF.dr=gJEzdm

(3)

= Jxdm( -gsinB)+ Jydm(gcosBsintft)+ Jzdm(gcosBcostft)

IFOST

2010 Proceedings
01 o 000 0 0 0 000
o 00000 0 o 9.81 0 o 0

Main forces and moments come from propellers. In this

paper the quadrotor MAV is in hover at trim, thus thrust and
drag are proportional to the squared propellers' rotational
speed, shown in Eq. 4:

T; =hQ;
Di=dQ;

=>

Thrust force

(4)

=>

A-

Drag moment
The non-conservative moments come from the propeller
thrusts, propeller drag, and the gyroscopic effects due to the
simultaneous rotation propeller and the quadrotor. After
calculating, the following full quadrotor dynamic model is
obtained as shown in Eq. 5 and 6:

mX =(cos{bsin8 coslf +sin {bsin If) LT;.

ax

X'o

'.

0 o 0000
0 o 0000
o 00000
o 00000
000000

0
-9.81
0
0
0
0
0
0
0

0
0
0
0

1
0
0
0
0

0
0
0
0

0
0
0
0
0

000
000
000
000

;=1

000
000
1 o 0
0 o 0
001

000000 0 0 0 000

mi =mg -(cos{bcos8) LT;

;=1

(10)

The input matrix, B, of the state-space representation is formed

by taking the Jacobian of the state equation with respect to the
input vector in Eq. 11.

mY =(cos{bsin 8 sin If -sin{bcos If) LT;

OFI

0001 o 0
o 00000
o 00001
o 0 o 000

(11)

(5)
where the input ui is defmed as the speed of the rotor

::

;=1

Uihover =

(12)

Also the matrix B for the state equation in Eq. 7 is given

0

Note that Eq. 5 means the translational motion and Eq. 6 the
attitude motion.
III.

In this section we present a control technique for attitude

stabilization of the quadrotor MAV in hover. We use the full
dynamic model in Eq. 3 and 4 to make the quad-rotor nonlinear
model in Matlab/Simulink, which will be used to analyze the
controller to be designed. In this paper a linear controller
design is considered so that the nonlinear model has to be
linearized about a trim in hover. Note that this trim is usually
unstable. The linearized state equation is expressed as

X=AX+B U
y=Cx+Du

with x(O) =Xo

(7)

where x E Rn, u E Rm, y E RI are the state vector, the control

input vector and the output vector, respectively. The state
vector is defmed as
x

X
[ X y Y z Z lVx thx lVy thy lVz thJ

(8)

The associated matrices A,B,C,D have proper dimension to

Eq. 7. Linearization of the nonlinear state equations at trim is
done by using Jacobians of the nonlinear state equations with
respect to the states and the inputs, evaluated at hovering
condition selected as
xo

[00000.10000000]

(9)

The Jacobian matrix A of the nonlinear state equation with

respect to all states evaluated at the given initial conditions in
Eq. 9 is shown in Eq. 10:

B= aF =
x,
au u.

CONTROLLER DESIGN

0
0

0
0
0

0
0
0

0
0
0

0.0248

0.0248

0.0248

0.0248

-0.3242

0.3242

-0.3242

0.3242

0.0207

-0.0207

0
0

-0.0207

0
0

0
0

(13)

0
0

0.0207

The output matrix C consists of four state variables, the

translation {X,Y,Z} in inertial frame E and the yaw angle If .
In this paper these variables are used as controlled variables by
commands. Also the controller design is based on these
variables. The direct transition matrix D is taken to be zero
( D=0 ) because there is no direct coupling between the input
and the output of the state equation in Eq. 7.
In this study, we choose Linear Quadratic Gaussian (LQG)
controller for our problem. In order to use LQG control, the
nonlinear system in Eq. 5 and 6 must be linearized around a
certain operating point, where that is hover. It is well-known
that LQG controller is designed from combination of Linear
Quadratic Regulator (LQR) and Linear Quadratic Estimator
(LQE), called as Kalman Filter, with preserving optimality of
the control. This idea is behind the separation principle,
meaning that optimal controller and optimal estimator can be
designed separately.

A.

Linear Quadratic Regulator (LQR)

Given a linear system defined in Eq. 7, LQR controller tries
to minimize the performance index is given as

978-1-4244-9037-0/10/$26.00 2010 IEEE

IFOST

2010 Proceedings
00

J(u)= f(xTQx+uTRu)dt

(14)

The state feedback for a linear system is given as

u=- Kx

(15)

(16)

The feedback gain Kr , related with the reference command

( r ), can be yielded as

Here the positive definite and symmetric matrix P satisfies the

following Riccati equation:
(17)
The positive defmite matrix Q weighs the size of the state
responses. Usually we choose it diagonal. If the entries of Q are
large numbers, then small deviation of the state from zero will
cause the cost to increase significantly, thus the control will
have high gain. The symmetric matrix R, which should be
positive matrix, weighs the control. It may also be chosen
diagonal. If the entries of this matrix are large, then the control
action will be small, opposite to what happens with matrix Q in
Eq. 14. If we want to increase the size of the control action, we
can reduce the entries of the control weighting matrix R.
R.

LQ tracking controller with integral action

A state feedback controller in Eq. 15 may not allow to

reject the effects of disturbances (particularly of input
disturbances or references). A very useful method of rejecting
the disturbance is to add an integral term to the feedback
control to ensure a unitary static closed-loop gain, i.e. gain
between the reference command and the output associated with
the reference. Integral action is used to eliminate steady state
errors when tracking constant signals. The objective is to keep
the output y close to a reference signal r .
Let us define the tracking error such that its time derivative
obeys the following differential equation:

ei=ret) - y t ) =ret) - Cx(t)

(18)

Defme an augmented state vector is also defmed as

x=[x eJ

(19)

[A 0] [B] [0]
0]X

Then the augmented state equation is realized as

x=

-C

x+ u+

(20)

y=[C

The feedback control to be designed is now structured as

u=-KX+Krr=-[Kx Ke1 [; ]+Krr

C.

(21)

model

with

978-1-4244-9036-3/10/$26.00 2010 IEEE

KI BTpeA - BKrT Q

Kr =

where K satisfies Eq. 16 and

(23)

is associated with in Eq. 17.

Next, let us defme the optimal estimator, called Kalman Filter,

with the estimator gain L :

i(t) =Ai( t ) +Ru(t) +L(y(t) - yet))

(24)

ji(t) =Ci(t)

where the estimator gain is obtained as

L=SCTg/

(25)

Note the positive defmite symmetric matrix S satisfies the

Riccati equation dual to Eq. 17.
So far we designed the optimal LQG controller to stabilize
the position and attitude motion of the quad-rotor MAV near
hover. In this design approach we assume that the output y in
Eq. 7 is available as measurement to the feedback controller.
However, in our problem the output shall be estimated by
vision camera. So in next section we will discuss the pose and
motion estimation algorithm[l] estimating the output in Eq. 7.
IV.

POSE ESTIMATION

In this paper, we will describe the two-camera pose

estimation method. This method is based on the point-based
techniques. In general, point-based method uses the feature
points identified on a 2D image. A pair of cameras is being
used to track the image features. These cameras track multi
color blobs located under the quad-rotor MAV and ground.
These blobs are located on a known geometric shape. A blob
tracking algorithm installed in the ground camera is used to
obtain the positions and areas of the blobs on the image planes.
Two cameras are set to see each other. One of the cameras
is located at the ground (CAM2) and the other is an on-board
camera (CAMl) looking downwards. Colored blobs are
attached to the bottom of the quad-rotor MAV and to the
ground camera. Tracking two blobs on the quad-rotor image
plane and one blob on the ground image frame is found to be
enough for accurate pose estimation. The blob tracking
algorithm tracks the blobs and returns image values (ui,Vi) .

Al

The cameras have matrices of intrinsic parameters

for CAM

2 and for CAMl. Let Wi be the vector from each camera to

the blobs.

Optimal LQG control with integral action

Consider the linear system

disturbances as shown in Eq. 22:

(22)

yet) =Cx(t) +vet)

where wet) and vet) are a process disturbance and a
measurement noise, respectively, and uncorrelated random
processes with covariance matrices Qf and Rf, respectively.

where the feedback gain is given as

K=KIBTp

x(t) =A x(t) +Ru(t) +Gw(t )

external

IFOST

2010 Proceedings
Wi= inv(AI).[ui
Wz= inv(Az).[uz

Vi

1]'

Vz

Eq. 31 becomes

for i=I,3,4,5,6
(26)

1]'

Wi = Wi I norm(wi )

Let

3 be

Moreover, Eq. 34 has a set of three equations in form of

the vector pointing from blob 1 to blob 3, and

Ai are the unknown scalars. Also the vector WI and

related by

W3= A, WI +RI\3
To simplify, let us take cross product of Eq. 27 by
0= A, (WI

2-

M L\3 +sinaM WI3 +(1-cosa)M .wI L\3 = 0 (34)

W3

are

(27)

W3 :

XW3)+(3x w31

(28)

(35)

Acosa+Bsina=C

A solution of Eq. 35 is yielded as

z
z
Z _ z
Z
_ sm -I B C B C2 (A +B )( C - A )
a2
2
A +B

(36)

Finally, the estimated rotation matrix R can be found from Eq.

29, meaning that Euler angles (f/J,B,ljI) are obtained.
In order to find the relative position of the quad-rotor MAV
with respect to the inertial frame located at the ground camera
frame, we need to fmd scalars Ai . First, fmd out A, from Eq. 28.
Then the rest of Ai are obtained from

(37)
where Ii is the position vector of the i-th blob in the body
frame. Hence the center of the quad-rotor MAV will be

[X y Z]'=[W3 +A4W4 +AsWs +A6Wd/4 (38)

V.

Figure 3.

Dual camera method

In order to solve Eq. 28, let the rotation matrix R assume to be

composed of two rotations: the rotation around the vector
formed the cross product of WI and wz, the rotation around
WI. This can be expressed in this form shown in Eq. 29:

R = Rot(wlx wz,B) Rot(wl,a)

where

Rot(a, b) means the rotation of b degrees around

a. Note the value of B can be obtained from
X

We consider full-state feedback in LQR with integral

action. The simulation performs the motion of the quadrotor
from position (O.2m, O.lm, 1m) to the position (0, 0, 3m),
while reducing the yaw angle from -20 to zero degrees. So the
initial state is selected as

(29)
the

unit vector

B = Ilwl W zII cos-I(WI.Wz}

Xo

(30)

o
5
10
time (seconds)

(31)

Using Rodrigues' formula given as

time (seconds)

(32)
Figure 4.

(33)

(39)

:K"' "' I '- I :rei

If.-I l,f;'-.lliffi,
:

Let us assume M = (w3x wl)x {W3x (Rot(wl x w2,B)}.

where

[0.200.1010000 -1T190]

[
x!i :: : . ..... : ..... >-:
f
J

Rewriting (28) gives

Rot(wpa)= I +WI sin a +wlz(1- cos a )

The simulation result using the linear state space model is

shown in Figure 5.

The only unknown left in (29) is the angle a .

(W3 x WI) X (W3 X (Rot(wl x wz,B)Rot(wl,a)I\3)= 0

SIMULATION

In this section we present the simulation result of the

controller design which is presented in section III. We make
the quadrotor nonlinear model and linear state space model in
Matlab/Simulink.

'

...........

0
5
10
time (seconds)

time (seconds)

f:
&.

" " " '

'"

0
5
10
time (seconds)

time (seconds)

Simulation result of linear state-space model in LQ

tracking with integral

Also the simulation result using the nonlinear state model is

shown in Figure 5.

978-1-4244-9037-0/101$26.00 2010 IEEE

IFOST

15 .
f5

2010 Proceedings

I!-J :'[-J m10

lV'S

time

. . T:

-4 0

psi vs time

theta vs time

..... ...

. { .

.. .

10

5 5

- 0

time (seconds)

lime (seconds)

lime (seconds)

phi \IS time

a. 2 ..

. " . . . . . . . .

. .

i ':rn ':EJ
0: ffij
lime (seconds)

! EEd
5 10 15 20 0 5 10 15
! ! EEd
5 10 15 20 0 5 10 15 20

10

g.

! .10 ..... l. ..... .

[
-20
.

10

10

lime (seconds)

lime (seconds)

20

Figure 5.

Simulation result of nonlinear state model

Figure 6 and Figure 7 show the simulation of LQG with

integral action controller design using the linear model and
rotor speed, respectively.

' 2 2 ' A
I ;ci] f ;i33, ! u."
- 'O EJ
i n : F\TJ

Figure 9.

wih integral action

02

lime (seconds)
phi V'S lime

1 .,: . . . i ...

Figure 6.

lime (seconds)

VI.

time (seconds)
psi vs lime

I,me (seconds)
theta vs time

-20

10

lime (seconds)

-20

10

lime (seconds)

10

Simulation of linear state model in LQG wih integral action

""
!lgl2g
'"
""" 3 loa
'oa rn
i

SOo

50

10

-soo

50

'

50

10

50

10

10

0
! .: !!:.--l !
n f UJ
D :D
i :O

: Kr J ! : t m l
1

0. 5

lime (seconds)

10

REFERENCES
[I]

Speed response of 4 rotors

Figure 8 and Figure 9 show the simulation of LQG with

integral action design and of the rotor speed in hover at 3[m]
height for the nonlinear quadrotor MAV, respectively.

20
- 0

ACKNOWLEDGEMENT
This research was supported by Basic Science Research
Program through the National Research Foundation of
Korea(NRF) funded by the Ministry of Education, Science and
Technology(No.2010-0016929)

Figure 7.

Figure 8.

CONCLUSION

In this paper we have reviewed the pose estimation

algorithm[l] and designed the controllers to keep the quadrotor
MAV at a specific position in hover. The three dimensional
quadrotor MAV model has been developed. LQ and LQG with
integral action controller have been used to stabilize and
performed the output-tracking objective. Simulations
performed on Matlab/Simulink showed the ability of the
controllers to perform output tracking control objective in
hover.

t:8 t:EEJ

-20
0

'o

Speed response of 4 rotors of nonlinear state model in LQG

20

lime (seconds)

Y" ,;me

time (seco nd s)

500

10

z " , ;m.

20

lime (seconds)

i 1:O
20

[2]

P.McKerrow, "Modelling the Draganf'yer four-rotor helicopter",

Proceedings of the IEEE International Conference on Robotics and
Automation, 26April -IMay 2004, 4, 3596-360.

[3]

Arda Ozgur Kivrak, "Design of control systems for a Quadrotor light

vehicle equipped with inertial sensors" , Master Thesis, in Mechatronics
Engineering , Atilim University, December 2006.

[4]

S. Boubdallah, A.Noth and R.Siegwart, "PID vs LQ Control Techniques

Applied to an Indoor Micro Quadrotor", IROS 2004, Sendai (Japan),
October 2004.

[5]

S. Boubdallah, P.Murrieri and R.Siegwart, "Design and Control of an

Indoor Micro Quadrotor", ICRA 2004, New Orleans (USA), April 2004.

time (seconds)

10

20

lime (seconds)

Altug, E.; Ostrowski, 1. P. & Taylor, C. 1. (2005), "Control of a

Quadrotor Helicopter Using Dual Camera Visual Feedback", The
International Journal of Robotics Research, Vol. 24, No. 5, May 2005,
pp. 329-341.

[6]
[7]

Amidi O. (1996) "An Autonomous Vision Guided Helicopter", Ph.D.

Thesis, Carnegie Mellon University, USA.
Christopher Kemp "Visual Control of a Miniature
Helicopter", PhD thesis, University of Cambridge, 2006.

Quadrotor

Simulation of nonlinear state model in LQG wih integral action

978-1-4244-9036-3/101$26.00 2010 IEEE