IEEE Robotics & Automation Magazine - December 2013 - 35

We rewrite (2) using b V (i.e., V in " B , frame) to facilitate
the estimator design. After neglecting the second-order terms
that appear because of coordinate frame transformation,
the first two components of b Vo d " b vo x, b vo y, b vo z , can be
written as
b

b
vo x . -g sin i - km1 v x

b

b
vo x . g cos i sin z - km1 v y

4,

(3)

where
4

k1 = m1 / ~i .
i =1

In what follows, we assume that k 1 is a positive constant considering the fact that the summation of propeller rotational
rates are fairly constant during smooth flight.
Inertial Sensors in Quadrotors
This article is concerned with the quadrotor state estimators
based on inertial sensors and specifically with the accelerometers and the gyroscopes. For simplicity, we assume that
a triad of the accelerometers and the gyroscopes are
mounted at the center of mass of the quadrotor body. For
both types of sensors, we adhere to standard MEMS error
models [15].
The gyroscopes measure the instantaneous rotational rate
of the body with respect to the inertial frame, and their measurements can be modeled independently of the equations of
motion of the moving platform to which they are attached.
gi = X i + b gi + w gi,
ob gi = - 1 b gi + w bgi,

(4)
(5)

x gi

where b gi is the bias of ith gyroscope and x gi is the time constant of ith gyroscope bias. w gi and w bgi are zero-mean white
Gaussian noise (WGN) terms.
In contrast, accelerometers measure a combination of
inertial and gravitational acceleration, and their measurements can be expressed using the equations of motion governing the body they are mounted on. Perhaps one of the best
example of the value of this strategy is the case of a triad of
accelerometers mounted on a quadrotor platform. Denoting
by au i the acceleration that would be measured by an ideal
accelerometer, we combine the accelerometer measurement
model with (2) to arrive at
au = Vo - g = - k T

4

/~

i=1

2
i

4

u,
b3 - m1 / ~i V
i=1

(6)

which describes the readings obtained from an on-board
triad of accelerometer, is unique to quadrotors, and is of critical importance to a state estimator in that context. As stated
in the previous section, (6) shows that the accelerometers
along the b 1 and b 2 coordinate axes are only sensitive to a
force that is dependant on the projection of the quadrotor
translational velocity onto the b 1, b 2 plane. Furthermore,

the component of the gravitational acceleration in the body
frame (which is typically large when compared to inertial
accelerations of slowmoving vehicles) no lonThrust force is
ger influences the accelerometer measurement. In
perpendicular to the
the next section, we will
exploit this unique proppropeller plane and thus
erty to design a better
s t at e e s t i m at or for
has no effect on the motion
quadrotors.

along that plane.
Estimator Design
The goal here is to design
a state estimator for the
quadrotor, with regard to the dynamic and kinematic equations presented in the previous sections. For this, we propose
a six-state extended Kalman filter (EKF)-based state estimator. The filter states are:
● z , the roll angle in current orientation estimate
● i , the pitch angle in current orientation estimate
● b gx , the bias in X axis gyroscope
● b gy , the bias in Y axis gyroscope
b
●
v x , the X velocity component of quadrotor in body frame
b
v y , the Y velocity component of quadrotor in body frame.
●
Process Model
The EKF process equations are formed by (1) and (3)-(5).
Out of the three Euler angles, we can only estimate z and i ,
as the process and measurement equations are expressed in a
form independent of the yaw angle }.
= (g x - b gx + w gx) + tan i cos z (g z - b gz)
+ tan i sin z (g y - b gy + w gy)
4,
oi = cos z (g y - b gy + w gy) - sin z (g z - b gz)

zo

bo gx
bo gy

_
b
x gx
`,
= - 1 b gy + w bgy bb
x gy
a
=- 1

b gx + w bgx b

(7)

(8)

_
vo x =-g sin i - k 1 b v x + w ax
b
m
`,
b
vo y =-g cos i sin z - k 1 b v y + w ay b
m
a
b

(9)

where w ax and w ay are WGN terms included to account for
the model imperfections in (3).
Equations (7)-(9) together describe the process dynamics
of the estimator. The resulting system can be represented as a
nonlinear function of states, control inputs, and noise terms.
xo = f (x, u, w) .
Measurement Model
The observations of the EKF are the measurements from the
X and Y accelerometers, which are aligned with b1 and b2,
DECEMBER 2013

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

35
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - December 2013
