IEEE Robotics & Automation Magazine - September 2012 - 39

where wk ¼ ½wx, k , wa, k , wb, k T is process noise. The nonlinear measurement model is (we omit time index k for
notational simplicity)
3 2
3
p
p
6 v 7 6
7
v
7 6
7
zk ¼ h(xk ) þ vk ¼ 6
4 mb 5 ¼ 4 RT (q)me 5,
eb
hb
ÀPz

∧
θ

yu

Gyroscope

2

where h^ denotes the estimate of h and kp is a gain
that determines crossover frequency. The complementary
filter described in (5) assumes that there is no steady-state
estimation error. However, in practice, the gyro bias varies
over time. To compensate for this, an integrator
[Figure 5(b)] is added to obtain the following:
_
^
h^ ¼ yu À ^b þ kp (yx À h),
^
^b_ ¼ ÀkI (yx À h):

(6)

~ ¼ 1=2(R
~ ÀR
~ T ) and
where pa (R)
^
~
R, R 2 SO(3) are attitude estimate
and estimate error, respectively, and
the vex operator is the inverse operation of a skew-symmetric matrix.

Gyroscope

yu

∧
θ
Angle

-

∧
θ

+
∧
- b
1
s
∧
b
Ki -

+
Kp

C(s)

yx +

Accelerometer

-

Figure 5. Complementary filter blends two different sensors that
have different frequency responses [21]. (a) Complementary filter
without bias compensation and (b) complementary filter with
bias compensation.

Ry is the rotation matrix reconstructed using roll and pitch
measured from the accelerometer. Xy is the measurement
from the three-axis gyroscope. Because R has to satisfy the
constraint RT R ¼ I, the computation load becomes an
issue in implementing this on an embedded system. For
this, in [22], the filter based on quaternion is provided.
Arducopter, Multiwii, and Aeroquad have implemented
this algorithm with the rotation matrix representation, and
the Paparazzi project provides both rotation matrix and
quaternion representations.
We evaluated the nonlinear complementary filters (NCFs)
on the SO(3) group using a Vicon motion capture system,
which gives accurate ground-truth measurements. The attitude computed by the flight controller was sent to the GCS
by an XBee 2.4 GHz transceiver. Attitude estimates of the
quadrotor are shown in Figure 7, which suggests that the attitude computed by the algorithm is accurate and reliable.

(7)

Nonlinear Complementary Filters on the SO(3) Group
An LCF is extended to the nonlinear SO(3) group [22] (Figure 6). The final form of the filter with bias estimate is given by
^_ ¼ R(X
^ y À ^b þ k) 3
R
(8)
^b_ ¼ ÀkI k, ^b(0) ¼ ^b0
(9)
T
~
~ ¼R
^ Ry , (10)
R
k ¼ vex(pa (R)),

yx +

Accelerometer

(5)

1
s

C(s)

where mb is the measurement of the magnetic field of the
earth me in body frame, hb is the height measured by the barometric sensor reading Pz , and vk is the measurement noise.
The states are estimated by the standard EKF algorithm
and measurements from accelerometers, gyroscopes,
magnetometers, GPS, and barometer are fused to estimate
the states.

_
^
h^ ¼ yu þ kp (yx À h),

∧
θ
Angle

Kp

(4)

Linear Complementary Filter
The Mikrokopter project implements the linear complementary filter (LCF) and is shown in Figure 5 on each axis
of the accelerometer and gyroscope. It is designed to fuse
multiple independent noisy measurements of the same
signal that have complementary spectral characteristics.
The details of complementary filters can be found in [21]
and [22].
Let yu be the rate measurement of the angle h and yx the
angle measured by accelerometer. The complementary filter to estimate the angle h is given by

∧
θ
1
s

Controllers
It is well known that the open-loop rotational dynamics of
a quadrotor are unstable as studied in [23]. The identified
model reveals that poles are located in the right-half plane

Ωy
Ry

R^ TRy

∼

R

∼

∼

π(R)

k

-kpπ(R)

^
R^ = RA

R^

R^ T
Figure 6. NCF on SO(3) group [22].

SEPTEMBER 2012

*

IEEE ROBOTICS & AUTOMATION MAGAZINE

*

39
Table of Contents for the Digital Edition of IEEE Robotics & Automation Magazine - September 2012
