See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/239787836

Nonlinear complementary filters on the special linear group

Article in International Journal of Control · October 2012

DOI: 10.1080/00207179.2012.693951

CITATIONS READS
55 357

4 authors:

Robert E. Mahony T. Hamel

Australian National University University Côte d'Azur
360 PUBLICATIONS 17,016 CITATIONS 233 PUBLICATIONS 10,426 CITATIONS

SEE PROFILE SEE PROFILE

P. Morin Ezio Malis

Sorbonne Université Robocortex
106 PUBLICATIONS 3,422 CITATIONS 109 PUBLICATIONS 6,713 CITATIONS

SEE PROFILE SEE PROFILE

Some of the authors of this publication are also working on these related projects:

HoverEye View project

Aircraft Flight Control View project

All content following this page was uploaded by Ezio Malis on 21 January 2021.

The user has requested enhancement of the downloaded file.

IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008 1203

Nonlinear Complementary Filters on the

Special Orthogonal Group
Robert Mahony, Senior Member, IEEE, Tarek Hamel, Member, IEEE, and Jean-Michel Pflimlin

Abstract—This paper considers the problem of obtaining good need to run on embedded processors with low memory and pro-
attitude estimates from measurements obtained from typical low cessing resources.
cost inertial measurement units. The outputs of such systems are There is a considerable body of work on attitude reconstruc-
characterized by high noise levels and time varying additive bi- tion for robotics and control applications (for example [1]–[4]). A
ases. We formulate the filtering problem as deterministic observer
kinematics posed directly on the special orthogonal group (3)
standard approach is to use extended stochastic linear estimation
driven by reconstructed attitude and angular velocity measure- techniques [5], [6]. An alternative is to use deterministic comple-
ments. Lyapunov analysis results for the proposed observers are mentary filter and nonlinear observer design techniques [7]–[9].
derived that ensure almost global stability of the observer error. Recent work has focused on some of the issues encountered for
The approach taken leads to an observer that we term the direct low cost IMU systems [10]–[12] as well as observer design for
complementary filter. By exploiting the geometry of the special or- partial attitude estimation [13]–[15]. It is also worth mentioning
thogonal group a related observer, termed the passive complemen- the related problem of fusing IMU and vision data that is receiving
tary filter, is derived that decouples the gyro measurements from
the reconstructed attitude in the observer inputs. Both the direct
recent attention [16]–[19] and the problem of fusing IMU and
and passive filters can be extended to estimate gyro bias online. The GPS data [20], [9]. Parallel to the work in robotics and control
passive filter is further developed to provide a formulation in terms there is a significant literature on attitude heading reference sys-
of the measurement error that avoids any algebraic reconstruction tems (AHRS) for aerospace applications [21]. An excellent re-
of the attitude. This leads to an observer on (3), termed the view of attitude filters is given by Crassidis et al. [22]. The re-
explicit complementary filter, that requires only accelerometer and cent interest in small low-cost aerial robotic vehicles has lead to
gyro outputs; is suitable for implementation on embedded hard- a renewed interest in lightweight embedded IMU systems [8],
ware; and provides good attitude estimates as well as estimating the
gyro biases online. The performance of the observers are demon-
[23][24].Forthelow-costlight-weightsystemsconsidered,linear
strated with a set of experiments performed on a robotic test-bed filtering techniques have proved extremely difficult to apply ro-
and a radio controlled unmanned aerial vehicle. bustly [25] and linear single-input single-output complementary
filters are often used in practice [24], [26]. A key issue is online
Index Terms—Attitude estimates, complementary filter, non-
linear observer, special orthogonal group.
identification of gyro bias terms. This problem is also important
in IMU callibration for satellite systems [5], [21], [27]–[30]. An
important development that came from early work on estimation
and control of satellites was the use of the quaternion representa-
tionfortheattitudekinematics[31],[32],[29],[33].Thenonlinear
I. INTRODUCTION
observer designs that are based on this work have strong robust-
HE recent proliferation of microelectromechanical sys- ness properties and deal well with the bias estimation problem
T tems (MEMS) components has lead to the development of
a range of low cost and light weight inertial measurement units.
[9], [29]. However, apart from the earlier work of the authors [14],
[34], [35] and some recent work on invariant observers [36], [37]
The low power, light weight and potential for low cost manu- there appears to be almost no work that considers the formulation
facture of these units opens up a wide range of applications in of nonlinear attitude observers directly on the matrix Lie-group
areas such as virtual reality and gaming systems, robotic toys, representation of .
and low cost mini-aerial-vehicles (MAVs) such as the Hovereye In this paper we study the design of nonlinear attitude ob-
(Fig. 1). The signal output of low cost IMU systems, however, servers on in a general setting. We term the proposed
is characterized by low-resolution signals subject to high noise observers complementary filters because of the similarity of
levels as well as general time-varying bias terms. The raw sig- the architecture to that of linear complementary filters (cf.
nals must be processed to reconstruct smoothed attitude esti- Appendix A), although, for the nonlinear case we do not have
mates and bias-corrected angular velocity measurements. For a frequency domain interpretation. A general formulation of
many of the low cost applications considered the algorithms the error criterion and observer structure is proposed based on
the Lie-group structure of . This formulation leads us to
Manuscript received November 8, 2006; revised August 3, 2007. Published propose two nonlinear observers on , termed the direct
August 27, 2008 (projected). Recommended by Associate Editor S. Celikovsky. complementary filter and passive complementary filter. The
R. Mahony is with Department of Engineering, Australian National Univer- direct complementary filter is closely related to recent work
sity, ACT, 0200, Australia (e-mail: [email protected]).
T. Hamel is with the I3S-CNRS, 06903 Sophia Antipolis, France (e-mail:
on invariant observers [36], [37] and corresponds (up to some
[email protected]). minor technical differences) to nonlinear observers proposed
J.-M. Pflimlin is with the Department of Navigation, Dassault Aviation, using the quaternion representation [9], [29], [31]. We do not
92210 Saint Cloud, Paris, France (e-mail: Jean-Michel.Pflimlin@dassault-avi- know of a prior reference for the passive complementary filter.
ation.com).
Color versions of one or more of the figures in this paper are available online The passive complementary filter has several practical advan-
at http://ieeexplore.ieee.org. tages associated with implementation and low-sensitivity to
Digital Object Identifier 10.1109/TAC.2008.923738 noise. In particular, we show that the filter can be reformulated
0018-9286/$25.00 © 2008 IEEE
1204 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

Fig. 1. VTOL MAV HoverEye of Bertin Technologies.

in terms of vectorial direction measurements such as those ob- II. PROBLEM FORMULATION AND NOTATION
tained directly from an IMU system; a formulation that we term
the explicit complementary filter. The explicit complementary A. Notation and Mathematical Identities
filter does not require online algebraic reconstruction of atti- The special orthogonal group is denoted . The associ-
tude, an implicit weakness in prior work on nonlinear attitude ated Lie-algebra is the set of anti-symmetric matrices
observers [22] due to the computational overhead of the calcula-
tion and poor error characterization of the constructed attitude.
As a result the observer is ideally suited for implementation
on embedded hardware platforms. Furthermore, the relative For any two matrices then the Lie-bracket (or
contribution of different data can be preferentially weighted matrix commutator) is . Let then
in the observer response, a property that allows the designer we define
to adjust for application specific noise characteristics. Finally,
the explicit complementary filter remains well defined even if
the data provided is insufficient to algebraically reconstruct the
attitude. This is the case, for example, for an IMU with only
accelerometer and rate gyro sensors. A comprehensive stability For any then is the vector cross product.
analysis is provided for all three observers that proves local The operator denotes the inverse of the
exponential and almost global stability of the observer error operator
dynamics, that is, a stable linearization for zero error along
with global convergence of the observer error for all initial con-
ditions and system trajectories other than on a set of measure
zero. Although the principal results of the paper are presented
in the matrix Lie group representation of , the equivalent For any two matrices the Euclidean matrix
quaternion representation of the observers are presented in inner product and Frobenius norm are defined
an Appendix. The authors recommend that the quaternion
representations are used for hardware implementation.
The body of paper consists of five sections followed by a
conclusion and two Appendices. Section II provides a quick
overview of the sensor model, geometry of and intro-
duces the notation used. Section III details the derivation of the
direct and passive complementary filters. The development here
is deliberately kept simple to be clear. Section IV integrates on- The following identities are used in the paper:
line bias estimation into the observer design and provides a de-
tailed stability analysis. Section V develops the explicit comple-
mentary filter, a reformulation of the passive complementary
filter directly in terms of error measurements. A suite of ex-
perimental results, obtained during flight tests of the Hovereye
(Fig. 1), are provided in Section VI that demonstrate the perfor-
mance of the proposed observers. In addition to the conclusion
(Section VII) there is a short Appendix on linear complementary
filter design and a second Appendix that provides the equivalent
quaternion formulation of the proposed observers.
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1205

The following notation for frames of reference is used: where is a bias term and denotes additive measure-
• denotes an inertial (fixed) frame of reference; ment noise. Normally, the gravitational field
• denotes a body-fixed-frame of reference; where dominates the value of for sufficiently
• denotes the estimator frame of reference. low frequency response. Thus, it is common to use
Let denote, respectively, the anti-symmetric and sym-
metric projection operators in square matrix space

as a low-frequency estimate of the inertial axis expressed

in the body-fixed-frame.
Let denote the angle axis coordinates of Magnetometer: The magnetometers provide measure-
. One has [38] ments of the magnetic field

where is the Earths magnetic field (expressed in the in-

ertial frame), is a body-fixed-frame expression for the
For any then . If then local magnetic disturbance and denotes measurement
in angle axis coordinates and . If then noise. The noise is usually quite low for magnetometer
has real eigenvalues , and there exists an readings, however, the local magnetic disturbance can be
orthogonal diagonalizing transformation such that very significant, especially if the IMU is strapped down
. to an MAV with electric motors. Only the direction of the
For any two signals are magnetometer output is relevant for attitude estimation and
termed asymptotically dependent if there exists a nondegenerate we will use a vectorial measurement
function and a time such that for any

in the following development.

The measured vectors and can be used to construct an
By the term nondegenerate we mean that the Hessian of at instantaneous algebraic measurement, , of the rotation
any point is full rank. The two signals are termed asymp-
totically independent if they are not asymptotically dependent.

B. Measurements where is the inertial direction of the magnetic field in the

The measurements available from a typical inertial measure- locality where data is acquired. The weights and are
ment unit are 3 axis rate gyros, 3 axis accelerometers and 3 axis chosen depending on the relative confidence in the sensor
magnetometers. The reference frame of the strap down IMU is outputs. Due to the computational complexity of solving an
termed the body-fixed-frame . The inertial frame is denoted optimization problem the reconstructed rotation is often ob-
. The rotation denotes the relative orientation of tained in a suboptimal manner where the constraints are applied
with respect to . in sequence; that is, two degrees of freedom in the rotation
Rate Gyros: The rate gyro measures angular velocity of matrix are resolved using the accelerometer readings and the
relative to expressed in the body-fixed-frame of final degree of freedom is resolved using the magnetometer.
reference . The error model used in this paper is As a consequence, the error properties of the reconstructed
attitude can be difficult to characterize. Moreover, if either
magnetometer or accelerometer readings are unavailable (due
to local magnetic disturbance or high acceleration manoeuvres)
where denotes the true value, denotes additive then it is impossible to resolve the vectorial measurements into
measurement noise and denotes a constant (or slowly a unique instantaneous algebraic measurement of attitude.
time-varying) gyro bias.
Accelerometer: Denote the instantaneous linear accelera- C. Error Criteria for Estimation on
tion of relative to , expressed in , by . An Let denote an estimate of the body-fixed rotation matrix
ideal accelerometer, ‘strapped down’ to the body-fixed- . The rotation can be considered as coordinates for
frame , measures the instantaneous linear accelera- the estimator frame of reference . It is also associated with
tion of minus the (conservative) gravitational accel- the frame transformation
eration field (where we consider expressed in the in-
ertial frame ), and provides a measurement expressed
in the body-fixed-frame . In practice, the output from
a MEMS component accelerometer has added bias and The goal of attitude estimate is to drive . The estimation
noise, error used is the relative rotation from body-fixed-frame to
the estimator frame

(1)
1206 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

The proposed observer design is based on Lyapunov stability

analysis. The Lyapunov functions used are inspired by the cost
function

(2)

One has that

(3) Fig. 2. Block diagram of the general form of a complementary filter on SO(3).

where is the angle associated with the rotation from to This choice leads to an elegant Lyapunov analysis of the filter
frame . Thus, driving (2) to zero ensures that . stability. Differentiating the storage function (2) along trajecto-
ries of (5) yields

III. COMPLEMENTARY FILTERS ON

In this section, a general framework for nonlinear comple-
mentary filtering on the special orthogonal group is introduced.
The theory is first developed for the idealized case where
and are assumed to be known and used to drive the filter (8)
dynamics. Filter design for real world signals is considered in
later sections. In Mahony et al. [34] a local stability analysis of the filter dy-
The goal of attitude estimation is to provide a set of dynamics namics (5) is provided based on this derivation. In Section IV a
for an estimate to drive the error rotation (1) global stability analysis for these dynamics is provided.
. The kinematics of the true system are We term the filter (5) a complementary filter on since
it recaptures the block diagram structure of a classical comple-
(4) mentary filter (cf. Appendix A). In Fig. 2: The “ ” operation
is an inverse operation on and is equivalent to a “-” op-
where . The proposed observer equation is posed eration for a linear complementary filter. The “ ” oper-
directly as a kinematic system for an attitude estimate on ation is equivalent to generating the error term “ ”. The
. The observer kinematics include a prediction term two operations and are maps from error space
based on the measurement and an innovation or correction and velocity space into the tangent space of ; operations
term derived from the error . The general form that are unnecessary on Euclidean space due to the identification
proposed for the observer is . The kinematic model is the Lie-group equivalent
of a first order integrator.
(5) To implement the complementary filter it is necessary to map
the body-fixed-frame velocity into the inertial frame. In prac-
where is a positive gain. The term tice, the “true” rotation is not available and an estimate of the
is expressed in the inertial frame. The body-fixed-frame angular rotation must be used. Two possibilities are considered.
velocity is mapped back into the inertial frame . If no Direct Complementary Filter: The constructed attitude
correction term is used then the error rotation is is used to map the velocity into the inertial frame
constant

A block diagram of this filter design is shown in Fig. 3. This

(6) approach can be linked to observers documented in earlier work
[31], [29] (cf. Appendix B). The approach has the advantage that
The correction term is considered to be in the it does not introduce an additional feedback loop in the filter
estimator frame of reference. It can be thought of as a nonlinear dynamics, however, high frequency noise in the reconstructed
approximation of the error between and as measured from attitude will enter into the feed-forward term of the filter.
the frame of reference associated with . In practice, it will be Passive Complementary Filter: The filtered attitude is
implemented as an error between a measured estimate of used in the predictive velocity term
and the estimate .
The goal of the observer design is to find a simple expression (9)
for that leads to robust convergence of . In prior work
[34], [35] the authors introduced the following correction term A block diagram of this architecture is shown in Fig. 4. The
advantage lies in avoiding corrupting the predictive angular ve-
(7) locity term with the noise in the reconstructed pose. However,
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1207

Fig. 5. Block diagram of the simplified form of the passive complementary

filter.

Fig. 3. Block diagram of the direct complementary filter on SO(3).

For the following work it is assumed that a reconstructed ro-
tation and a biased measure of angular velocity are avail-
able

valid for low frequencies (11a)

for constant bias (11b)

The approach taken is to add an integrator to the compensator

term in the feedback equation of the complementary filter.
Let be positive gains and define
Direct Complementary Filter With Bias Correction:
Fig. 4. Block diagram of the passive complementary filter on SO(3).
(12a)
the approach introduces a secondary feedback loop in the filter
and stability needs to be proved. (12b)
A key observation is that the Lyapunov stability analysis in (12c)
(8) is still valid for (9), since
Passive Complementary Filter With Bias Correction:

(13a)

(13b)
using the fact that the trace of a commutator is zero, (13c)
. The filter is termed a passive compli-
mentary filter since the cross coupling between and does The nonlinear stability analysis is based on the idea of an adap-
not contribute to the derivative of the Lyapunov function. A tive estimate for the unknown bias value.
global stability analysis is provided in Section IV. Theorem 4.1 [Direct Complementary Filter With Bias
There is no particular theoretical advantage to either the direct Correction]: Consider the rotation kinematics (4) for a
or the passive filter architecture in the case where exact measure- time-varying and with measurements given by
ments are assumed. However, it is straightforward to see that the (11). Let denote the solution of (12). Define error
passive filter (9) can be written variables and . Define by

(10) (14)
This formulation suppresses entirely the requirement to repre- Then:
sent and in the inertial frame and leads to 1) The set is forward invariant and unstable with respect to
the architecture shown in Fig. 5. The passive complementary the dynamic system (12).
filter avoids coupling the reconstructed attitude noise into the 2) The error is locally exponentially stable to
predictive velocity term of the observer, has a strong Lyapunov .
stability analysis, and provides a simple and elegant realization 3) For almost all initial conditions the trajectory
that will lead to the results in Section V. converges to the trajectory .
Proof: Substituting for the error model (11), (12a) becomes
IV. STABILITY ANALYSIS
In this section, the direct and passive complementary filters
on are extended to provide online estimation of time-
varying bias terms in the gyroscope measurements and global Differentiating it is straightforward to verify that
stability results are derived. Preliminary results were published
in [34] and [35]. (15)
1208 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

Define a candidate Lyapunov function by Applying LaSalle’s principle to the solutions of (12) it fol-
lows that either asymptotically or
(16) where is a solution of (18).
To determine the local stability properties of the invariant
Differentiating one obtains sets we compute the linearization of the error dynamics. We
will prove exponential stability of the isolated equilibrium point
first and then return to prove instability of the set . De-
fine as the first order approximations of and
around

(20a)
(20b)

The sign change in (20b) simplifies the analysis of the lineariza-

tion. Substituting into (15), computing and discarding all terms
of quadratic or higher order in yields
Substituting for and [(12b) and (12c)] one obtains
(21)
(17)
For positive gains the linearized error system is
Lyapunov’s direct method ensures that converges asymptoti- strictly stable. This proves part ii) of the theorem statement.
cally to zero [39]. Recalling that , where To prove that is unstable, we use the quaternion formula-
denotes the angle axis coordinates of . It follows that tion (see Appendix B). Using (49), the error dynamics of the
implies either , or for . In quaternion associated to the rotation is given by
the second case one has the condition . Note that
is also equivalent to requiring to be symmetric. (22a)
It is easily verified that is an isolated equilibrium of the
error dynamics (18). (22b)
From the definition of one has that on . We will
prove that is forward invariant under the filter dynamics (12). (22c)
Setting in (15) and (12b) yields
It is straightforward to verify that the invariant set associated to
(18) the error dynamics is characterized by
For initial conditions the solution of
(18) is given by

(19) Define , then an equivalent characterization of is

given by . We study the stability properties of the
We verify that (19) is also a general solution of (15) and (12b). equilibrium of evolving under the filter dynamics
Differentiating yields (12). Combining (22a) and (22c), one obtains the following dy-
namics for :

Linearizing around small values of one obtains

where the second line follows since commutes with

and the final equality is due to the fact that ,a
consequence of the choice of initial conditions . Since and are positive gains it follows that the lineariza-
It follows that on solution of (19) and hence tion is unstable around the point and this completes the
. Classical uniqueness results verify that (19) is a solution proof of part i).
of (15) and (12b). It remains to show that such solutions remain The linearization of the dynamics around the unstable set is
in for all time. The condition on is proved above. To see either strongly unstable (for large values of ) or hyperbolic
that we compute (both positive and negative eigenvalues). Since depends on
the initial condition then there will be trajectories that converge
to along the stable center manifold [39] associated with the
stable direction of the linearization. From classical center mani-
as . This proves that is forward invariant. fold theory it is known that such trajectories are measure zero in
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1209

the overall space. Observing in addition that is measure zero where is given by (24). This bounds , and it fol-
in proves part iii) and the full proof is complete. lows is bounded. LaSalle’s principle cannot be applied directly
The direct complimentary filter is closely related to quater- since the dynamics (24a) are not autonomous. The function
nion based attitude filters published over the last fifteen years is uniformly continuous since the derivative
[31], [9], [29]. Details of the similarities and differences is given
in Appendix B where we present quaternion versions of the
filters we propose in this paper. Apart from the formulation
directly on , the present paper extends earlier work by
proposing globally defined observer dynamics and a full global is uniformly bounded. Applying Barbalat’s lemma proves
analysis. To the authors best understanding, all prior published asymptotic convergence of to zero.
algorithms depend on a term that is discontinuous on Direct substitution shows that is an equilib-
(14). Given that the observers are not well defined on the set rium point of (24). Note that (14) and hence on
the analysis for prior work is necessarily nonglobal. However, (Th. 4.1). For the error dynamics (24) become
having noted this, the recent work of Thienel et al. [29] pro-
vides an elegant powerful analysis that transforms the observer
error dynamics into a linear time-varying system (the transfor-
mation is only valid on a domain on ) for which The solution of this ordinary differential equation is given by
global asymptotic stability is proved. This analysis provides a
global exponential stability under the assumption that the ob-
server error trajectory does not intersect . In all practical situ-
ations the two approaches are equivalent.
The remainder of the section is devoted to proving an Since is anti-symmetric for all time then is
analogous result to Theorem 4.1 for the passive complemen- orthogonal and since it follows
tary filter dynamics. In this case, it is necessary to deal with is symmetric for all time. It follows that is forward in-
nonautonomous terms in the error dynamics due to passive variant under the filter dynamics (13). We prove by contradic-
coupling of the driving term into the filter error dynamics. tion that is the largest forward invariant set of the
Interestingly, the nonautonomous term acts in our favour to closed-loop dynamics (13) such that . Assume that there
disturb the forward invariance properties of the set (14) and exits such that remains in for
reduce the size of the unstable invariant set. all time. One has that on this trajectory. Conse-
Theorem 4.2 [Passive complementary filter with bias quently,
correction]: Consider the rotation kinematics (4) for a
time-varying and with measurements given
by (11). Let denote the solution of (13). Define
error variables and . Assume that
is a bounded, absolutely continuous signal and that the
pair of signals are asymptotically independent (see (25)
Section II-A). Define by
where we have used
(23)
(26)
Then:
1) The set is forward invariant and unstable with respect several times in simplifying expressions. Since are
to the dynamic system 13. asymptotically independent then the relationship (25) must be
2) The error is locally exponentially stable to degenerate. This implies that there exists a time such that for
. all then and contradicts the assumption.
3) For almost all initial conditions the trajec- It follows that either asymptotically or
tory converges to the trajectory . .
Proof: Substituting for the error model (11) in (13) and Analogously to Theorem 4.1 the linearization of the error dy-
differentiating , it is straightforward to verify that namics (24) at is computed. Let and
for . The linearized dynamics are the time-varying
linear system
(24a)
(24b)

The proof proceeds by differentiating the Lyapunov-like func- Let denote the magnitude bound on and choose
tion (16) for solutions of (13). Following an analogous deriva-
tion to that in Theorem 4.1, but additionally exploiting the can-
cellation , it may be verified that
1210 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

Set to be matrices where is a noise process. Since only the direction of the mea-
surement is relevant to the observer we assume that
and normalize all measurements to ensure .
Let be an estimate of . Define

(27)

to be the associated estimate of . For a single direction , the

It is straightforward to verify that and are positive def- error considered is
inite matrices given the constraints on . Consider
the cost function , with . Differ-
entiating yields
which yields

(28)

It is straightforward to verify that For multiple measures the following cost function is consid-
ered

(30)

This proves exponential stability of the linearized system at

. where
The linearization of the error dynamics on a trajectory in
are also time varying and it is not possible to use the argu- (31)
ment from Theorem 4.1 to prove instability. However, note that
for all . Moreover, any neigh-
borhood of a point within the set Assume linearly independent inertial direction then the
contains points such the . Trajectories with matrix is positive definite if . For
these initial conditions cannot converge to due to the de- then is positive semi-definite with one eigenvalue zero. The
crease condition derived earlier, and it follows that is un- weights are chosen depending on the relative confidence
stable. Analogous to Theorem 4.1 it is still possible that a set of in the measurements . For technical reasons in the proof of
measure zero initial conditions, along with very specific trajec- Theorem 5.1 we assume additionally that the weights are
tories , such that the resulting trajectories converge to . chosen such that has three distinct eigenvalues
This proves part iii) and completes the proof. .
Apart from the expected conditions inherited from Theorem Theorem 5.1 [Explicit complementary filter with bias
4.1 the key assumption in Theorem 4.2 is the independence of correction]: Consider the rotation kinematics (4) for a
from the error signal . The perturbation of the passive time-varying and with measurements given by
dynamics by the independent driving term provides a distur- (29) and (11b). Assume that there are two or more,
bance that ensures that the adaptive bias estimate converges to vectorial measurements available. Choose such that
the true gyroscopes’ bias, a particularly useful property in prac- (defined by (31)) has three distinct eigenvalues. Consider
tical applications. the filter kinematics given by

(32a)
V. EXPLICIT ERROR FORMULATION OF THE PASSIVE
COMPLEMENTARY FILTER (32b)

A weakness of the formulation of both the direct and pas- (32c)

sive and complementary filters is the requirement to reconstruct
an estimate of the attitude, , to use as the driving term for
the error dynamics. The reconstruction cannot be avoided in the and let denote the solution of (32). Assume that
direct filter implementation because the reconstructed attitude is a bounded, absolutely continuous signal and that the
is also used to map the velocity into the inertial frame. In this pair of signals are asymptotically independent (see
section, we show how the passive complementary filter may be Section II-A). Then:
reformulated in terms of direct measurements from the inertial 1) There are three unstable equilibria of the filter character-
unit. ized by
Let , denote a set of known inertial
directions. The measurements considered are body-fixed-frame
observations of the fixed inertial directions
where and
(29) are diagonal matrices with entries
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1211

as shown and such that where real since complex eigenvalues must come in complex conju-
is a diagonal matrix. gate pairs. The eigenvalues of an orthogonal matrix are of the
2) The error is locally exponentially stable to form
.
3) For almost all initial conditions
, the trajectory converges to the trajec-
where is the angle from the angle axis representation. Given
tory .
that all the eigenvalues are real it follows that or .
Proof: Define a candidate Lyapunov-like function by
The first possibility is the desired case . The
second possibility is the case where .
When then (32) and (13) lead to identical error dy-
namics. Thus, we use the same argument as in Theorem 4.2 to
prove that on the invariant set. To see that the only forward
The derivative of is given by
invariant subsets are the unstable equilibria as characterized in
part i) of the theorem statement we introduce . Ob-
serve that

Recalling that the trace of a commutator is zero, the derivative Analogous to (35), this implies or on the
of the candidate Lyapunov function can be simplified to obtain set and . Set . Then

As has three distinct eigenvalues, it follows that

(33)
for all and thus is diagonal. Therefore, there are
four isolated equilibrium points
Recalling the identities in Section II-A, one may write (where are specified in part i) of the theorem statement)
as and that satisfy the condition . The case
(where ) corresponds to the equi-
(34) librium while we will show that the other three
equilibria are unstable.
We proceed by computing the dynamics of the filter in the
Introducing the expressions of into the time derivative new variable and using these dynamics to prove the stability
of the Lyapunov-like function , (33), one obtains properties of the equilibria. The dynamics associated to are

The Lyapunov-like function derivative is negative semi-definite

ensuring that is bounded. Analogous to the proof of The-
orem 4.2, Barbalat’s lemma is invoked to show that
tends to zero asymptotically. Thus, for one has

(35)

We prove next (35) implies either or .

Since is a real matrix, the eigenvalues and eigenvectors of Setting , one obtains
verify
(37)
and (36)
The dynamics of the new estimation error on the bias are
where (for ) represents the complex conjugate of
the eigenvalue and represents the Hermitian transpose of
the eigenvector associated to . Combining (35) and (36),
one obtains
(38)

The dynamics of (37) and (38) are an alternative formu-

lation of the error dynamics to .
Note that for is positive definite and Consider a first order approximation of (37) and (38)
. One has for all . In the case around an equilibrium point
when , it is simple to verify that two of the three eigen-
values are real. It follows that all three eigenvalues of are
1212 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

The linearization of (37) is given by It is straightforward to verify that its time derivative is always
positive

and thus
Note that for then has at least one element of
the diagonal positive. For each and , define

and finally
and note that is non-null for all . Let such
that . A trajectory initialized at will
diverge from the compact set since on . How-
ever, the trajectory cannot exit through the surface
since along the trajectory. Restricting such
for and where is specified in part i) of the theorem that the linearization is valid, then the trajectory must exit
statement. Define through the sphere . Consequently, trajectories initially
arbitrarily close to will diverge. This proves that the point
is locally unstable.
To prove local exponential stability of we
consider the linearization (39) for . Note that and
. Set and . Then
are positive definite and (39) may be written as

Setting and one may write the lineariza-

tion (37) as

Consider a cost function with given by (27).

Analogous to (28), the time derivative of is given by
We continue by computing the linearization of . Equation
(37) may be approximated to a first order by

Once again, it is straightforward to verify that

and thus

where is defined in (27) and this proves local exponential

Finally, for stability of .
The final statement of the theorem follows directly from the
above results along with classical dynamical systems theory and
the proof is complete.
Remark: If , the weights , and the measured
Rewriting in terms of the variables and setting directions are orthogonal then .
one obtains The cost function becomes
for

The combined error dynamic linearization in the primed coor- In this case, the explicit complementary filter (32) and the pas-
dinates is sive complementary filter (13) are identical.
Remark: It is possible to weaken the assumptions in Theorem
(39) 5.1 to allow any choice of gains and any structure of the ma-
trix and obtain analogous results. The case where all three
To complete the proof of part i) of the theorem statement we eigenvalues of are equal is equivalent to the passive comple-
will prove that the three equilibria associated with mentary filter scaled by a constant. The only other case, where
for are unstable. The demonstration is analogous , has
to the proof of the Chetaev’s Theorem (see [39, pp. 111–112]).
Consider the following cost function:
for . (Note that the situation where is
considered in Corollary 5.2.) It can be shown that any sym-
metry with satisfies
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1213

and it is relatively straightforward to verify that this Define the following storage function:
set is forward invariant under the closed-loop filter dynamics.
This invalidates part i) of Theorem 5.1 as stated, however, it
can be shown that the new forward invariant points are unstable
as expected. To see this, note that any in this set cor- The derivative of is given by
responds to the minimal cost of on . Consequently,
any neighborhood of contains points such that
and the Lyapunov decrease condition en-
sures instability. There is still a separate isolated unstable equi- The Lyapunov-like function derivative is negative semi-def-
librium in , and the stable equilibrium, that must be treated inite ensuring that is bounded and . The set
in the same manner as undertaken in the formal proof of The- is characterized by and thus
orem 5.1. Following through the proof yields analogous results
to Theorem 5.1 for arbitrary choice of gains .
The two typical measurements obtained from an IMU unit are
estimates of the gravitational, , and magnetic, , vector fields Consider a trajectory that satisfies the filter dy-
namics and for which for all time. One has

In this case, the cost function becomes

The weights and are introduced to weight the confidence

in each measure. In situations where the IMU is subject to high Differentiating this expression again one obtains
magnitude accelerations (such as during takeoff or landing ma-
noeuvres) it may be wise to reduce the relative weighting of the
accelerometer data compared to the magnetometer Since the signals and are asymptotically independent it
data. Conversely, in many applications the IMU is mounted in follows that the functional expression on the left-hand side is
the proximity to powerful electric motors and their power supply
degenerate. This can only hold if . For , this
busses leading to low confidence in the magnetometer readings
set of trajectories is characterized by the definition of . It is
(choose ). This is a very common situation in the case
straightforward to adapt the arguments in Theorems 4.1 and 4.2
of mini aerial vehicles with electric motors. In extreme cases
to see that this set is forward invariant. Note that for then
the magnetometer data is unusable and provides motivation for
. It is direct to see that lies on a local
a filter based solely on accelerometer data.
maximum of and that any neighborhood contains points
such that the full Lyapunov function is strictly less than its
A. Estimation From the Measurements of a Single Direction
value on the set . This proves instability of and completes
Let be a measured body fixed frame direction associated part i) of the corollary.
with a single inertial direction . Let be an The proof of part ii) and part iii) is analogous to the proof of
estimate . The error considered is Theorem 5.1 (see also [15]).
An important aspect of Corollary 5.2 is the convergence of
the bias terms in all degrees of freedom. This ensures that, for
a real world system, the drift in the attitude estimate around the
Corollary 5.2: Consider the rotation kinematics (4) for a unmeasured axis will be driven asymptotically by a zero
time-varying and with measurements given mean noise process rather than a constant bias term. This makes
by (29) (for a single measurement ) and (11b). Let the proposed filter a practical algorithm for a wide range of MAV
denote the solution of (32). Assume that is applications.
a bounded, absolutely continuous signal and are
asymptotically independent (see Section II-A). Define VI. EXPERIMENTAL RESULTS
In this section, we present experimental results to demon-
strate the performance of the proposed observers.
Then: Experiments were undertaken on two real platforms to
1) The set is forward invariant and unstable under the demonstrate the convergence of the attitude and gyro bias
closed-loop filter dynamics. estimates.
2) The estimate is locally exponentially stable to 1) The first experiment was undertaken on a robotic ma-
. nipulator with an IMU mounted on the end effector and
3) For almost all initial conditions then supplied with synthetic estimates of the magnetic field
converges to the trajectory . measurement. The robotic manipulator was programmed
Proof: The dynamics of are given by to simulate the movement of a flying vehicle in hov-
ering flight regime. The filter estimates are compared to
(40) orientation measurements computed from the forward
1214 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

Fig. 7. Bias estimation from direct and passive complementary filters.

Fig. 6. Euler angles from direct and passive complementary filters.

kinematics of the manipulator. Only the passive and direct Fig. 8. Estimation results of the Pitch and roll angles.
complimentary filters were run on this test bed.
2) The second experiment was undertaken on the VTOL MAV filter displays slightly less noise in the bias estimates than for
HoverEye developed by Bertin Technologies (Fig. 1). The the direct filter (note the different scales in the -axis).
VTOL belongs to the class of “sit on tail” ducted fan Figs. 8 and 9 relate to the second experiment. The experi-
VTOL MAV, like the iSTAR9 and Kestrel developed mental flight of the MAV was undertaken under remote control
respectively by Allied Aerospace [40] and Honeywell by an operator. The experimental flight plan used was: First,
[41]. It was equipped with a low-cost IMU that consists the vehicle was located on the ground, initially headed toward
of three axis accelerometers and three axis gyroscopes. . After take off, the vehicle was stabilized in hovering
Magnetometers were not integrated in the MAV due to condition, around a fixed heading which remains close the initial
perturbations caused by electrical motors. The explicit heading of the vehicle on the ground. Then, the operator engages
complementary filter was used in this experiment. a 90 -left turn manoeuvre, returns to the initial heading, and
For both experiments the gains of the proposed filters were follows with a 90 -right turn manoeuvre, before returning to
chosen to be: and . The iner- the initial heading and landing the vehicle. After landing, the
tial data was acquired at rates of 25 Hz for the first experiment vehicle is placed by hand at its initial pose such that final and
and 50 Hz for the second experiment. The quaternion version of initial attitudes are the identical.
the filters (Appendix B) were implemented with first order Euler Fig. 8 plots the pitch and roll angles estimated di-
numerical integration followed by rescaling to preserve the unit rectly from the accelerometer measurements against the esti-
norm condition. mated values from the explicit complementary filter. Note the
Experimental results for the direct and passive versions of the large amounts of high frequency noise in the raw attitude esti-
filter are shown in Figs. 6 and 7. In Fig. 6, the only significant mates. The plots demonstrate that the filter is highly successful
difference between the two responses lies in the initial transient in reconstructing the pitch and roll estimates.
responses. This is to be expected, since both filters will have the Fig. 9 presents the gyros bias estimation verses the pre-
same theoretical asymptotic performance. In practice, however, dicted yaw angle based on open loop integration of the
the increased sensitivity of the direct filter to noise introduced gyroscopes. Note that the explicit complementary filter here is
in the computation of the measured rotation is expected to based solely on estimation of the gravitational direction. Con-
contribute to slightly higher noise in this filter compared to the sequently, the yaw angle is the indeterminate angle that is not
passive. directly stabilized in Corollary 5.2. Fig. 9 demonstrates that the
The response of the bias estimates is shown in Fig. 7. Once proposed filter has successfully identified the bias of the yaw
again the asymptotic performance of the filters is similar after axis gyro. The final error in yaw orientation of the microdrone
an initial transient. From this figure it is clear that the passive after landing is less than 5 over a two minute flight. Much of
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1215

where is predominantly high frequency noise and is a pre-

dominantly low frequency disturbance. Choosing a pair of com-
plementary transfer functions with
low pass and high pass, the filtered estimate is given by

The signal is all pass in the filter output while noise com-
ponents are high and low pass filtered as desired. This type of
filter is also known as distorsionless filtering since the signal
is not distorted by the filter [42]. Complementary filters
are particularly well suited to fusing low bandwidth position
measurements with high band width rate measurements for first
order kinematic systems. Consider the linear kinematics
(41)
with typical measurement characteristics
Fig. 9. Gyros bias estimation and influence of the observer on yaw angle.
(42)
this error would be due to the initial transient when the bias
estimate was converging. Note that the second part of the figure where is low pass filter associated with sensor character-
indicates that the bias estimates are not constant. Although istics, represents noise in both measurements and is a
some of this effect may be numerical, it is also to be expected deterministic perturbation that is dominated by low-frequency
that the gyro bias on low cost IMU systems are highly suscep- content. Normally the low pass filter over the fre-
tible to vibration effects and changes in temperature. Under quency range on which the measurement is of interest. The
flight conditions changing engine speeds and aerodynamic rate measurement is integrated to obtain an estimate of
conditions can cause quite fast changes in gyro bias. the state and the noise and bias characteristics of the integrated
signal are dominantly low frequency effects. Choosing
VII. CONCLUSION
This paper presents a general analysis of attitude observer de-
sign posed directly on the special orthogonal group. Three non-
linear observers, ensuring almost global stability of the observer
error, are proposed:
Direct Complementary Filter: A nonlinear observer posed with all pass such that over the bandwidth
on that is related to previously published nonlinear ob- of . Then
servers derived using the quaternion representation of .
Passive Complementary Filter: A nonlinear filter equation
that takes advantage of the symmetry of to avoid trans-
formation of the predictive angular velocity term into the esti-
mator frame of reference. The resulting observer kinematics are Note that even though is high pass the noise
considerably simplified and avoid coupling of constructed atti- is low pass filtered. In practice, the filter structure is imple-
tude error into the predictive velocity update. mented by exploiting the complementary sensitivity structure of
Explicit Complementary Filter: A reformulation of the pas- a linear feedback system subject to load disturbance. Consider
sive complementary filter in terms of direct vectorial measure- the block diagram in Fig. 10. The output can be written
ments, such as gravitational or magnetic field directions ob-
tained for an IMU. This observer does not require online al-
gebraic reconstruction of attitude and is ideally suited for im-
plementation on embedded hardware platforms. Moreover, the
filter remains well conditioned in the case where only a single
vector direction is measured. where is the sensitivity function of the closed-loop system
The performance of the observers was demonstrated in a suite and is the complementary sensitivity. This architecture is
of experiments. The explicit complementary filter is now im- easy to implement efficiently and allows one to use classical
plemented as the primary attitude estimation system on several control design techniques for in the filter design. The sim-
MAV vehicles world wide. plest choice is a proportional feedback . In this case
the closed-loop dynamics of the filter are given by
APPENDIX A
A REVIEW OF COMPLEMENTARY FILTERING (43)
Complementary filters provide a means to fuse multiple inde- The frequency domain complementary filters associ-
pendent noisy measurements of the same signal that have com- ated with this choice are and
plementary spectral characteristics [11]. For example, consider . Note that the crossover frequency for
two measurements and of a signal the filter is at . The gain is typically chosen
1216 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

via . Given such that

then Let de-
note a body-fixed frame velocity, then the pure quaternion
is associated with a quaternion velocity. Con-
sider the rotation kinematics on Equation (4), then the
associated quaternion kinematics are given by
Fig. 10. Block diagram of a classical complementary filter.

based on the low pass characteristics of and the low fre- (45)
quency noise characteristics of to choose the best crossover
frequency to tradeoff between the two measurements. If the rate Let be a low frequency measure of , and (for
measurement bias, , is a constant then it is natural to constant bias ) be the angular velocity measure. Let denote
add an integrator to the compensator to make the system type I the observer estimate and quaternion error

(44)

A type I system will reject the constant load disturbance Note that
from the output. Gain design for and is typically based
on classical frequency design methods. The nonlinear develop-
ment in the body of the paper requires a Lyapunov analysis of
closed-loop system (43). Applying the PI compensator, (44),
one obtains state space filter with dynamics where is the angle axis representation of .
The quaternion representations of the observers proposed in this
paper are as follows.
Direct Complementary Filter (12):
The negative sign in the integrator state is introduced to indicate
that the state will cancel the bias in . Consider the Lyapunov
function (46a)

(46b)

Passive Complementary Filter (13):

Abusing notation for the noise processes, and using ,
and , one has (47a)

(47b)

In the absence of noise one may apply Lyapunov’s direct Explicit Complementary Filter (32):
method to prove convergence of the state estimate. LaSalle’s
principal of invariance may be used to show that . When (48a)
the underlying system is linear, then the linear form of the
feedback and adaptation law ensure that the closed-loop system
is linear and stability implies exponential stability. (48b)

APPENDIX B (48c)
QUATERNION REPRESENTATIONS OF OBSERVERS
The error dynamics associated with the direct filter expressed in
The unit quaternion representation of rotations is commonly the quaternion formulation are
used for the realization of algorithms on since it of-
fers considerable efficiency in code implementation. The set of (49)
quaternions is denoted .
The set is a group under the operation The error dynamics associated with the passive filter are

(50)

with identity element . The group of quaternions There is a 15-year history of using the quaternion representation
are homomorphic to via the map and Lyapunov design methodology for filtering on (for
example, cf. [9], [29], and [31]). To the authors’ knowledge the
Lyapunov analysis in all cases has been based around the cost
function
This map is a two to one mapping of onto with kernel
, thus, is locally isomorphic to
MAHONY et al.: NONLINEAR COMPLEMENTARY FILTERS ON THE SPECIAL ORTHOGONAL GROUP 1217

Due to the unit norm condition it is straightforward to show that the correction term scales to a factor of the correction
term . A simple scaling factor like this is compensated for the
in choice of filter gains and and makes no difference to
the performance of the filter.
The cost function proposed in this paper is
(3). It is straightforward to see that the quadratic approximation
of both cost functions around the point is the quadratic REFERENCES
. The quaternion cost function , however, is nondiffer-
entiable at the point while the cost has a [1] J. Vaganay, M. Aldon, and A. Fournier, “Mobile robot attitude esti-
smooth local maxima at this point. To the authors understanding, mation by fusion of inertial data,” in Proc. IEEE Int. Conf. Robotics
Automation (ICRA), 1993, vol. 1, pp. 277–282.
all quaternion filters in the published literature have a similar [2] E. Foxlin, M. Harrington, and Y. Altshuler, “Miniature 6-DOF iner-
flavour that dates back to the seminal work of Salcudean [31]. tial system for tracking HMD,” in Proc. SPIE, Orlando, FL, 1998, vol.
The closest published work to that undertaken in the present 3362, pp. 214–228.
paper was published by Thienel in her doctoral dissertation [43] [3] J. Balaram, “Kinematic observers for articulated robers,” in Proc. IEEE
Int. Conf. Robotics Automation, 2000, pp. 2597–2604.
and transactions paper [29]. The filter considered by Thienel et [4] J. L. Marins, X. Yun, E. R. Backmann, R. B. McGhee, and M. Zyda,
al. is given by “An extended kalman filter for quaternion-based orientation estimation
using marg sensors,” in IEEE/RSJ Int. Conf. Intelligent Robots Systems,
2001, pp. 2003–2011.
(51a) [5] E. Lefferts, F. Markley, and M. Shuster, “Kalman filtering for space-
craft attitude estimation,” AIAA J. Guidance, Control, Navig., vol. 5,
no. 5, pp. 417–429, Sep. 1982.
(51b) [6] B. Barshan and H. Durrant-Whyte, “Inertial navigation systems for mo-
bile robots,” IEEE Trans. Robot. Autom., vol. 44, no. 4, pp. 751–760,
The term enters naturally in the filter design from the 1995.
[7] M. Zimmerman and W. Sulzer, “High bandwidth orientation measure-
differential, , of the absolute value term in ment and control based on complementary filtering,” presented at the
the cost function , during the Lyapunov design process. Con- Symp. Robotics Control (SYROCO), Vienna, Austria, 1991.
sider the observer obtained by replacing in (51) by . [8] A.-J. Baerveldt and R. Klang, “A low-cost and low-weight attitude esti-
Note that with this substitution, (51b) is transformed into (46b). mation system for an autonomous helicopter,” in Proc. IEEE Int. Conf.
Intell. Eng. Syst., 1997, pp. 391–395.
To show that (51a) transforms to (46a) it is sufficient to show [9] B. Vik and T. Fossen, “A nonlinear observer for GPS and ins integra-
that . This is straightforward from tion,” in Proc. IEEE Conf. Decision and Control, Orlando, FL, Dec.
2001, pp. 2956–2961.
[10] H. Rehbinder and X. Hu, “Nonlinear state estimation for rigid body
motion with low-pass sensors,” Syst. Control Lett., vol. 40, no. 3, pp.
183–190, 2000.
[11] E. R. Bachmann, J. L. Marins, M. J. Zyda, R. B. Mcghee, and X. Yun,
“An extended Kalman filter for quaternion-based orientation estima-
This demonstrates that the quaternion filter (51) is obtained from tion using MARG sensors,” in Proc. IEEE/RSJ Intelligent Robots and
Systems, Maoui, HI, 2001, pp. 2003–2011, .
the standard form of the complimentary filter proposed (12) with [12] J.-M. Pflimlin, T. Hamel, P. Soueeres, and N. Metni, “Nonlinear atti-
the correction term (12c) replaced by tude and gyroscoples bias estimation for a VTOL UAV,” in Proc. IFAC
World Congr., 2005.
[13] H. Rehbinder and X. Hu, “Drift-free attitude estimation for accelerated
rigid bodies,” Automatica, 2004.
[14] N. Metni, J.-M. Pflimlin, T. Hamel, and P. Soueeres, “Attitude and
Note that the correction term defined in (12c) can be written gyro bias estimation for a flying UAV,” in Proc. IEEE/RSJ Int. Conf.
. It follows that Intelligent Robots Systems, Aug. 2005, pp. 295–301.
[15] N. Metni, J.-M. Pflimlin, T. Hamel, and P. Soueeres, “Attitude and gyro
bias estimation for a VTOL UAV,” Control Eng. Practice, vol. 14, no.
12, pp. 1511–1520, 2006.
[16] J. Lobo and J. Dias, “Vision and inertial sensor cooperation using
gravity as a vertical reference,” IEEE Trans. Pattern Anal. Mach.
The correction term for the two filters varies only by the posi- Intell., vol. 25, no. 12, pp. 1597–1608, Dec. 2003.
tive scaling factor . The quaternion correction term [17] H. Rehbinder and B. Ghosh, “Pose estimation using line-based dy-
namic vision and inertial sensors,” IEEE Trans. Autom. Control, vol.
is not well defined for (where ) and these 48, no. 2, pp. 186–199, Feb. 2003.
points are not well defined in the filter dynamics (51). It should [18] J.-H. Kim and S. Sukkarieh, “Airborne simultaneous localisation and
be noted, however, that is bounded at and, apart from map building,” in Proc. IEEE Int. Conf. Robotics Automation, Taipei,
possible switching behavior, the filter can still be implemented Taiwan, R.O.C., Sep. 2003, pp. 406–411.
[19] P. Corke, J. Dias, M. Vincze, and J. Lobo, “Integration of vision and
on the remainder of . An argument for the use of the inertial sensors,” presented at the IEEE Int. Conf. Robotics Automation
correction term is that the resulting error dynamics strongly (ICRA), Barcelona, Spain, Apr. 2004, ser. W-M04 full-day workshop.
force the estimate away from the unstable set [cf. (14)]. An [20] R. Phillips and G. Schmidt, in System Implications and Innovative Ap-
argument against its use is that, in practice, such situations will plications of Satellite Navigation, 1996, vol. 207, pp. 0.1–0.18 [On-
line]. Available: [email protected], ser. AGARD Lecture Series 207,
only occur due to extreme transients that would overwhelm the NASA Center for Aerospace Information ch. GPS/INS Integration
bounded correction term in any case, and cause the numer- [21] D. Gebre-Egziabher, R. Hayward, and J. Powell, “Design of multi-
ical implementation of the filter to deal with a discontinuous sensor attitude determination systems,” IEEE Trans. Aerosp. Electron.
Syst., vol. 40, no. 2, pp. 627–649, Apr. 2004.
argument. In practice, it is an issue of little significance since [22] J. L. Crassidis, F. L. Markley, and Y. Cheng, “Nonlinear attitude fil-
the filter will general work sufficiently well to avoid any issues tering methods,” J. Guidance, Control, Dynam., vol. 30, no. 1, pp.
with the unstable set . For , corresponding to , 12–28, Jan. 2007.
1218 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. 53, NO. 5, JUNE 2008

[23] M. Jun, S. Roumeliotis, and G. Sukhatme, “State estimation of an au- [41] J. Fleming, T. Jones, P. Gelhausen, and D. Enns, “Improving control
tonomous helicopter using Kalman filtering,” in Proc. IEEE/RSJ Int. system effectiveness for ducted fan VTOL UAVs operating in cross-
Conf. Intelligent Robots and Systems, Kyongju, Korea, Oct. 17–21, winds,” in Proc. AIAA 2nd “Unmanned Unlimited” System, San Diego,
1999, pp. 1346–1353. CA, Sep. 2003.
[24] G. S. Sukhatme, G. Buskey, J. M. Roberts, P. I. Corke, and S. Saripalli, [42] R. G. Brown and P. Y. C. Hwang, Introduction to Random Signals and
“A tale of two helicopters,” in Proc. IEEE/RSJ Int. Robots Systems, Las Applied Kalman Filtering, 2nd ed. New York, NY: Wiley, 1992.
Vegas, NV, Oct. 2003, pp. 805–810 [Online]. Available: http://www- [43] J. Thienel, “Nonlinear observer/controller designs for spacecraft atti-
robotics.usc.edu/srik/papers/iros2003.pdf tude control systems with uncalibrated gyros,” Ph.D. dissertation, Dept.
[25] J. M. Roberts, P. I. Corke, and G. Buskey, “Low-cost flight control Aerosp. Eng., Faculty of the Graduate School, Univ. of Maryland, MD,
system for small autonomous helicopter,” in Proc. Australian Conf. 2004.
Robotics Automation, Auckland, New Zealand, Nov. 27–29, 2002, pp.
71–76.
[26] P. Corke, “An inertial and visual sensing system for a small au- Robert Mahony (S’92–M’95–SM’08) received the
tonomous helicopter,” J. Robot. Syst., vol. 21, no. 2, pp. 43–51, Feb. B.Sc. degree in applied mathematics and geology and
2004. the Ph.D. degree in systems engineering both from
[27] G. Creamer, “Spacecraft attitude determination using gyros and quater- the Australian National University (ANU) in 1989
nion measurements,” J. Astronaut. Sci., vol. 44, no. 3, pp. 357–371, Jul. and 1995, respectively.
1996. He worked as a marine seismic geophysicist and an
[28] D. Bayard, “Fast observers for spacecraft pointing control,” in Proc. industrial research scientist before completing a two-
IEEE Conf. Decision Control, Tampa, FL, 1998, pp. 4702–4707. year postdoctoral fellowship in France and a two-year
[29] J. Thienel and R. M. Sanner, “A coupled nonlinear spacecraft attitude Logan Fellowship at Monash University, Australia.
controller and observer with an unknow constant gyro bias and gyro He is currently a Reader in the Department of En-
noise,” IEEE Trans. Autom. Control, vol. 48, no. 11, pp. 2011–2015, gineering at the Australian National University. His
Nov. 2003. research interests are in nonlinear control theory with applications in robotics
[30] G.-F. Ma and X.-Y. Jiang, “Spacecraft attitude estimation from vector and geometric optimization techniques.
measurements using particle filter,” in Proc. 4th Int. Conf. Machine
Learning Cybernetics, Guangzhou, China, Aug. 2005, pp. 682–687.
[31] S. Salcudean, “A globally convergent angular velocity observer for
rigid body motion,” IEEE Trans. Autom. Cont., vol. 36, no. 12, pp.
1493–1497, Dec. 1991. Tarek Hamel (M’07) received the B.Eng. degree
[32] O. Egland and J. Godhaven, “Passivity-based adaptive attitude con- from the University of Annaba, Algeria, in 1991 and
trol of a rigid spacecraft,” IEEE Trans. Autom. Control, vol. 39, pp. the Ph.D. degree in robotics from the University of
842–846, Apr. 1994. Technology Compiégne (UTC), France, in 1995.
[33] A. Tayebi and S. McGilvray, “Attitude stabilization of a four-rotor After two years as a Research Assistant at the
aerial robot: Theory and experiments,” IEEE Trans. Control Syst. UTC, he joined the Centre d’Etudes de Mècanique
Technol., vol. 14, no. 3, pp. 562–571, May 2006. d’Iles de France in 1997 as an Associate Pro-
[34] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Complimentary filter design fessor. Since 2003, he has been a Professor at the
on the special orthogonal group SO(3),” presented at the IEEE Conf. I3S UNSA-CNRS laboratory of the University of
Decision Control (CDC), Seville, Spain, Dec. 2005. Nice-Sophia Antipolis, France. His research interests
[35] T. Hamel and R. Mahony, “Attitude estimation on SO(3) based on di- include nonlinear control theory, estimation and
rect inertial measurements,” in Proc. Int. Conf. Robotics Automation vision-based control with applications to unmanned aerial vehicles and mobile
(ICRA), Orlando, FL, 2006, pp. 2170–2175. robots.
[36] S. Bonnabel and P. Rouchon, Control and Observer Design for Non-
linear Finite and Infinite Dimensional Systems. New York: Springer-
Verlag, 2005, pp. 53–67, Vol. 322 ch. On Invariant Observers ser. Lec-
ture Notes in Control and Information Sciences. Jean-Michel Pflimlin received the B.Eng. degree
[37] S. Bonnabel, P. Martin, and P. Rouchon, “A non-linear symmetry-pre- from Supaero, the French Engineering school in
serving observer for velocity-aided inertial navigation,” in Proc. Amer. Aeronautics and Space, in 2003 and the Ph.D.
Control Conf., Jun. 2006, pp. 2910–2914. degree in automatic control from Supaero in 2006.
[38] R. Murray, Z. Li, and S. Sastry, A Mathematical Introduction to Robotic His Ph.D. research was conducted at the Labo-
Manipulation. Boca Raton, FL: CRC Press, 1994. ratory for Analysis and Architecture of Systems,
[39] H. Khalil, Nonlinear Systems, 2nd ed. Englewood Cliffs, NJ: Pren- LAAS-CNRS, Toulouse, France.
tice-Hall, 1996. He is currently working at the Navigation De-
[40] L. Lipera, J. Colbourne, M. Tischler, M. Mansur, M. Rotkowitz, and partment of Dassault Aviation, Saint Cloud, Paris,
P. Patangui, “The micro craft istar micro-air vehicle: Control system France. His research interests include nonlinear
design and testing,” in Proc. 57th Annu. Forum Amer. Helicopter Soc., control and filtering, advanced navigation systems,
Washington, DC, May 2001, pp. 1–11. and their applications to unmanned aerial vehicles.

View publication stats