Design and implementation of a low-cost observer-based attitude and heading reference system

ARTICLE IN PRESS Control Engineering Practice 18 (2010) 712–722

Contents lists available at ScienceDirect

Control Engineering Practice journal homepage: www.elsevier.com/locate/conengprac

Design and implementation of a low-cost observer-based attitude and heading reference system ¨ b Philippe Martin a,, Erwan Salaun a b

Centre Automatique et Syste mes, Mines ParisTech, 60 boulevard Saint-Michel, 75272 Paris Cedex 06, France School of Aerospace Engineering, Georgia Institute of Technology, Atlanta, GA 30332-0150, USA

a r t i c l e in f o

a b s t r a c t

Article history: Received 31 March 2009 Accepted 27 January 2010 Available online 11 February 2010

A nonlinear observer (i.e. a ‘‘filter’’) is proposed for estimating the attitude of a flying rigid body, using measurements from low-cost inertial and magnetic sensors. It has by design a nice geometrical structure appealing from an engineering viewpoint; it is easy to tune, computationally very thrifty, and with guaranteed (at least local) convergence around every trajectory. Moreover it behaves sensibly in the presence of acceleration and magnetic disturbances. Experimental comparisons with a commercial device illustrate its good performance; an implementation on an 8-bit microcontroller with very limited processing power demonstrates its computational simplicity. & 2010 Elsevier Ltd. All rights reserved.

Keywords: Attitude estimation Strapdown systems Nonlinear observers Extended Kalman filter Inertial navigation Symmetries

1. Introduction Aircraft, especially unmanned aerial vehicles (UAV), commonly need to know their attitude to be operated, whether manually or with computer assistance. When cost or weight is an issue, using very accurate inertial sensors for ‘‘true’’ (i.e. based on the Schuler effect due to a non-flat rotating Earth) inertial navigation is excluded. Instead, low-cost systems—often called attitude and heading reference systems (AHRS)—rely on light and cheap strapdown gyroscopes, accelerometers and magnetometers. The various measurements are ‘‘merged’’ according to the motion equations of the aircraft assuming a flat non-rotating Earth, usually with some kind of ‘‘filter’’; for more details about avionics, various inertial navigation systems and sensor fusion, see for instance Kayton and Fried (1997) and Grewal, Weill, and Andrews (2007). The attitude estimation problem has received a lot of attention especially in the aerospace engineering community, see the recent survey Crassidis, Markley, and Cheng (2007) and the references therein. By far the most widely used approach is the extended Kalman filter (EKF) and its variants, see e.g. Shuster and Oh (1981), Lefferts, Markley, and Shuster (1982) and Markley (2003). While it is a general method capable of good performance when properly tuned, the EKF suffers from several drawbacks: it is not easy to choose the numerous parameters; it is computationally

 Corresponding author.

E-mail addresses: [email protected] (P. Martin), ¨ [email protected] (E. Salaun). 0967-0661/$ - see front matter & 2010 Elsevier Ltd. All rights reserved. doi:10.1016/j.conengprac.2010.01.012

expensive, which is a problem in low-cost embedded systems; it is usually difficult to prove the convergence, and the designer has to rely on extensive simulations. An alternative route is to use a dedicated nonlinear observer as proposed in Thienel and Sanner (2003) and Mahony, Hamel, and Pflimlin (2008). The present paper follows the same lines; it uses the rich geometric structure of the attitude-heading problem to derive an observer by the method developed in Bonnabel, Martin, and Rouchon (2008), building up on the preliminary work Martin ¨ (2007, 2008a). The proposed observer has by design a and Salaun nice geometrical structure appealing from an engineering viewpoint; it is easy to tune, computationally very thrifty, and with guaranteed (at least local) convergence around every trajectory. Moreover it behaves sensibly in the presence of acceleration and magnetic disturbances. Experimental comparisons with a commercial device illustrate its good performance; an implementation on an 8-bit microcontroller with very limited processing power demonstrates its computational simplicity. As any other AHRS the proposed observer assumes the linear acceleration is small so that the accelerometers measurements are close to the gravity vector, which limits its use to ‘‘quasihover’’ situations. The relevance of this assumption in the context ¨ (2010) and of a rotary wing UAV is discussed in Martin and Salaun Pflimlin, Binetti, Soue res, Hamel, and Trouchet (2010). When velocity measurements are available, observers based on the same ¨ (2008c, approach can also be designed, see Martin and Salaun 2008b). The paper first presents the physical model used and proceeds with the construction of the observer. The choice of the tuning

ARTICLE IN PRESS P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

parameters, taking into account possible magnetic disturbance, and the ensuing convergence is then studied. Finally, experimental results on a very low-cost implementation are reported.

2. The physical system 2.1. Motion equations

713

eigenvalue. The conclusion is the same when linearizing around any other equilibrium point. For that reason, it is customary to assume the linear acceleration V_ small, hence to approximate the specific acceleration vector by a =  q  1A q using (2). This yields the new output yA =  a =q  1A q (the sign is reversed for convenience). Therefore the physical system (1)–(2) is seen as q_ ¼ 12q  o;

The motion of a flying rigid body (assuming the Earth is flat and defines an inertial frame) is described by q_ ¼ 12q  o;

ð1Þ

V_ ¼ A þ q  a  q1 ;

ð2Þ

ð3Þ

with output measurements ! ! yA q1  A  q ¼ : yB q1  B  q

ð4Þ

where:

 q is the unit quaternion representing the orientation of the body-fixed frame with respect to the Earth-fixed frame;

 o is the angular velocity vector expressed in the body-fixed frame;

 A= ge3 is the (constant) gravity vector expressed in the Earth 

fixed frame (the unit vectors e1,e2,e3 point, respectively, North, East, Down); V is the velocity vector of the center of mass expressed in the Earth-fixed frame; a is the specific acceleration vector, in this case the aerodynamic forces divided by the body mass, expressed in the body-fixed frame.

The first equation describes the kinematics of the body, the second is Newton’s force law. It is customary to use quaternions instead of Euler angles since they provide a global parametrization of the body orientation, and are well-suited for calculations and computer simulations. For more details see Stevens and Lewis (2003) or any other good textbook on aircraft modeling, and Appendix A for useful formulas used in this paper. 2.2. Measurements In an AHRS there are no velocity measurements. Three triaxial sensors providing nine scalar measurements are used: three gyroscopes measure o; three magnetometers measure the magnetic field in the body-fixed frame yB = q  1B q, where B =B1e1 + B3e3 is the Earth magnetic field in the Earth-fixed frame; three accelerometers measure a. Clearly, the velocity V is not observable. A simple first-order analysis shows it is moreover not detectable. Indeed, linearizing (1)–(2) around the equilibrium point ðV ; q; o ; aÞ ¼ ð0; 1; 0; AÞ yields

dq_ ¼ 12do dV_ ¼ da þ 2A  dq; dyB ¼ 2B  dq; where dq ¼ dq1 e1 þ dq2 e2 þ dq3 e3 ; but no observer 1 ! 0 1 dq_^ d o @ A þLðdyB dy^ B Þ; 2 _ ¼ dV^ da þ2A  dq^ ^ dy^ B ¼ 2B  dq; where L is a freely chosen 6  3 matrix, is able to estimate dV: there will be a linearly growing error due to a double zero

2.3. Sensor imperfections The sensors are of course not perfect, in particular they are usually biased. A reasonable assumption is to consider these biases constant but otherwise unknown. It would then be desirable to estimate them online together with the attitude and heading. While this is doable for the gyro biases, this is impossible for the accelero biases (though up to six unknown constants can be estimated since there are six output measurements). Indeed, assume the accelerometers measure in fact am = a+ ab, where ab is a constant vector bias; (3)–(4) then becomes q_ ¼ 12q  o; a_ b ¼ 0; yA yB

! ¼

q1  A  qþ ab q1  B  q

! :

But this system is clearly unobservable: a first-order analysis as in the previous section reveals one combination of the components of ab cannot be estimated. In a similar way, it is also impossible to completely estimate a bias vector on the magnetic measurements. Another issue is the possible local perturbation of the magnetic field B. Once again a linear analysis shows it is not possible to estimate the three components of the magnetic field B (hence the perturbation), but only the North and Down components. Moreover only one imperfection on am can be estimated without relying on the possibly disturbed magnetic measurements. In an AHRS it is usually desirable to use the magnetic measurements to estimate only the heading, so that a magnetic disturbance does not affect the estimated attitude. As seen later this decoupling can be achieved by considering yC :¼ yA  yB ¼ q1  C  q, where C :¼ A  B ¼ gB1 e2 , rather than the direct measurement yB. Notice that /yA ; yC S ¼ /A; CS ¼ 0, hence only eight independent measurements out of nine are left; as a consequence only five unknown constants can now be estimated. This is not a drawback and is even beneficial since the observer will then not depend on the latitude-varying B3. Finally, the sensors are modeled as follows: the three gyros measure om ¼ o þ ob , where ob is a constant vector bias; the three accelerometers measure am = asa, where as 40 is a constant scaling factor; the three magnetometers measure yB = bsq  1B q, where bs 4 0 is a constant scaling factor, which implies yC :¼ cs q1  C  q, where cs :¼ as bs 4 0. There are therefore five unknown constants, which can all be estimated, see next section. Noise also corrupts all the measurements; it is dealt with indirectly through the tuning of the observer gains.

ARTICLE IN PRESS 714

P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

2.4. The design model From the previous section, the model chosen for designing the observer is

By construction fg is a diffeomorphism on S for all g. The transformation group is local if fg ðxÞ is defined only for g around e. In this case the transformation law fg2 3fg1 ðxÞ ¼ fg2 g1 ðxÞ is imposed only when it makes sense; ‘‘for all g’’ thus means ‘‘for all g around e’’, and ‘‘for all x’’ means ‘‘for all x in some neighborhood’’.

q_ ¼ 12q  ðom ob Þ;

ð5Þ

o_ b ¼ 0;

ð6Þ

a_ s ¼ 0;

ð7Þ

x_ ¼ f ðx; uÞ;

ð10Þ

c_ s ¼ 0;

ð8Þ

y ¼ hðx; uÞ;

ð11Þ

with the output ! ! yA as q1  A  q ¼ : yC cs q1  C  q

ð9Þ

This system is observable since all the state variables can be recovered from the known quantities om ; yA ; yC and their derivatives: from (9), as ¼ JyA J=g and cs ¼ JyC J=B1 g; hence the action of q on the two independent vectors A and C is known, which completely defines q in function of yA,yC,as,cs. Finally (5) _ yields ob ¼ om 2q1 q.

3. The nonlinear observer There is no general method for designing a nonlinear observer for a given system. When the system has a rich geometric structure the recent paper Bonnabel et al. (2008) proposes a constructive solution to this problem. In short, when a system of state dimension n is invariant by a transformation group of dimension r rn, the method yields the structure of the general symmetry-preserving (or invariant) observer. More importantly the error between the estimated and actual states is described in suitable invariant coordinates by a system of dimension 2n  r; in particular the error system has dimension n when the group has dimension n. This result greatly simplifies the convergence analysis. The system (5)–(8) turns out to be invariant by a rotation in the body-fixed frame and a translation on the gyro bias. From a physical and engineering viewpoint, it is also perfectly sensible for an observer using measurements expressed in the body-fixed frame not to be affected by the actual choice of coordinates, i.e. by a rotation in the body-fixed frame. Similarly, a translation on the gyro bias and a scaling on the outputs should not affect the observer. Therefore, the invariance properties of the system are used not only as a means to build an observer, but also to fulfill a rather natural requirement. Section 3.1 briefly recalls the main ideas of Bonnabel et al. (2008) and Section 3.2 applies them to produce an invariant observer and an invariant state error. The reader not interested in those mathematical aspects can safely skip them and proceed directly to Section 3.3. 3.1. Invariant observers

Definition 1. Let G be a Lie Group with identity e and S an open set (or more generally a manifold). A transformation group ðfg Þg A G on S is a smooth map ðg; xÞ A G  S/fg ðxÞ A S such that:

 fe ðxÞ ¼ x for all x;  fg 3fg ðxÞ ¼ fg g ðxÞ for all g1 ; g2 ; x. 2

1

2 1

Consider now the smooth output system

where (x,u,y) belongs to an open subset X  Rn  U  Rm  Y  Rp (or more generally a manifold). The signals u(t), y(t) are assumed known (y is measured, and u is measured or known as a control input). Consider also the local group of transformations on X  U  Y defined by ðX; U; YÞ :¼ ðjg ðxÞ; cg ðuÞ; rg ðyÞÞ, where jg , cg and rg are local diffeomorphisms. Definition 2. The system (10)–(11) is invariant with equivariant output if for all g,x,u f ðjg ðxÞ; cg ðuÞÞ ¼ Djg ðxÞ  f ðx; uÞ; hðjg ðxÞ; cg ðuÞÞ ¼ rg ðhðx; uÞÞ: The property also reads X_ ¼ f ðX; UÞ and Y= h(X,U), i.e. the system is left unchanged by the transformation. ^ u; yÞ is invariant Definition 3. The observer x_^ ¼ Fðx; ^ cg ðuÞ; rg ðyÞÞ ¼ Djg ðxÞ ^  Fðx; ^ u; yÞ for all g; x; ^ u; y. Fðjg ðxÞ;

if

_ The property also reads X^ ¼ FðX^ ; U; YÞ, i.e. the system is left unchanged by the transformation. The key idea to build an invariant observer is to use an invariant output error. ^ u; yÞ/Eðx; ^ u; yÞ A Y is an Definition 4. The smooth map ðx; invariant output error if ^ u; yÞ is invertible for all x; ^ u;  the map y/Eðx; ^ u; hðx; ^ uÞÞ ¼ 0 for all x; ^ u;  Eðx; ^ cg ðuÞ; rg ðyÞÞ ¼ Eðx; ^ u; yÞ for all x; ^ u; y.  Eðjg ðxÞ; The first and second properties mean E is an ‘‘output error’’, i.e. ^ uÞ ¼ y; the third property, which also it is zero if and only if hðx; ^ u; yÞ, defines invariance. reads EðX^ ; U; YÞ ¼ Eðx; Similarly, the key idea to study the convergence of an invariant observer is to use an invariant state error. ^ xÞ A X is an invariant ^ xÞ/Zðx; Definition 5. The smooth map ðx; state error if

 it is a diffeomorphism on X  X ;  Zðx; xÞ ¼ 0 for all x; ^ jg ðxÞÞ ¼ Zðx; ^ xÞ for all x; ^ x.  Zðjg ðxÞ; The two main results—based on the Cartan moving frame method—are now stated in the special case where g/jg ðxÞ is invertible (i.e. when G is of dimension n), see Bonnabel et al. (2008) for the general case; this corresponds to the case where G acts on itself, see Bonnabel, Martin, and Rouchon (2009). The moving frame x/gðxÞ is obtained by solving for g the so-called normalization equation jg ðxÞ ¼ c for some arbitrary constant c; in other words jgðxÞ ðxÞ ¼ c.

ARTICLE IN PRESS P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

whereas the output (9) is equivariant since ! ! a0 q1 ðq  q0 Þ1  ða0 as ÞA  ðq  q0 Þ 0  yA  q0 ¼ : c0 q1 ðq  q0 Þ1  ðc0 cs ÞC  ðq  q0 Þ 0  yC  q0

Theorem 1. Every invariant observer reads ^ uÞ þ x_^ ¼ f ðx;

n X

715

^ u; yÞ; Iðx; ^ uÞÞ  Eðx; ^ u; yÞÞwi ðxÞ; ^ ðLi ðEðx;

i¼1

Solving for q0 ; o0 ; a0 ; c0 the normalization equations

where:

q  q0 ¼ 1; ^ :¼ ½DjgðxÞ ^ 1  @=@xi is an invariant vector field, i= 1,y,n  wi ðxÞ ^ ðxÞ

  

(@=@xi is the ith canonical vector field on X ); the w0i s are by construction independent; ^ u; yÞ :¼ rgðxÞ ^ uÞÞrgðxÞ Eðx; ^ ðhðx; ^ ðyÞ is an invariant output error; ^ uÞ :¼ cgðxÞ Iðx; ^ ðuÞ is a (complete) invariant; Li, i=1,y,n, is a 1  p freely chosen matrix with entries possibly depending on E and I.

Theorem 2. The error system reads Z_ ¼ UðZ; IÞ, ^ xÞ ¼ jgðxÞ ^ jgðxÞ Zðx; ^ ðxÞ ^ ðxÞ is an invariant state error.

where

This result greatly simplifies the convergence analysis of the pre-observer, since the error equation is autonomous but for the ‘‘free’’ known invariant I. For a general nonlinear (not invariant) observer the error equation depends on the trajectory t/ðxðtÞ; uðtÞÞ of the system, hence is in fact of dimension 2n.

3.2. Building the observer The transformations consisting of constant rotations in the body-fixed frame, constant translations on the gyro bias, and constant scalings on the outputs, i.e. 1 1 0 0 q  q0 q C B ob C B q1 0  ob  q0 þ o0 C C B C; jðq0 ;o0 ;a0 ;c0 Þ B C¼B B C B a0 as @ as A @ A c0 cs cs

cðq0 ;o0 ;a0 ;c0 Þ ðom Þ ¼ q1 0  om  q0 þ o0 ;

rðq0 ;o0 ;a0 ;c0 Þ

yA

!

yC

¼

a0 q1 0  yA  q0

!

c0 q1 0  yC  q0

;

are easily seen to form a transformation group of the same dimension as the system (5)–(8); q0 is a unit quaternion, o0 a vector in R3 and a0 ; c0 4 0. The underlying group law is defined by 1 0 1 1 0 0 q0 q1  q0 q1 C B C C B B 1 B o1 C B o0 C B q  o0  q1 þ o1 C C B C :¼ B 1 C B C B a1 C B a0 C B a1 a0 A @ A A @ @ c1 c0 c1 c0 (in fact the state space is here a group, acting on itself by the group law). The system (5)–(8) is invariant by the transformation group since zfflffl}|fflffl{ _ q  q0 ¼ q_  q0 ¼ 12ðq  q0 Þ  ððq1 0  om  q0 þ o0 Þ ðq1 0  ob  q0 þ o0 ÞÞ;

q1 0  ob  q0 þ o0 ¼ 0; a0 as ¼ 1; c0 cs ¼ 1; yields the so-called moving frame 1 0 q1 C B B q  ob  q1 C C B C B 1 C: gðq; ob ; as ; cs Þ :¼ B C B as C B C B 1 A @ cs This leads to the six-dimensional invariant error E ! ! ! yA EA a^ s q^ 1  A  q^ :¼ rgðq; rgðq; ^ o ^ o ^ b ;a^ s ;c^ s Þ ^ b ;a^ s ;c^ s Þ yC EC c^ s q^ 1  C  q^ 1 0 1 ^ 1 ^ B A a^ s q  yA  q C C ¼B A @ 1 C q^  yC  q^ 1 c^ s and the three-dimensional complete invariant ^  ðom o ^ b Þ  q^ I :¼ cgðq; ^ o ^ b ;a^ s ;c^ s Þ ðom Þ ¼ q

1

:

It is easy to check EA, EC and I are indeed invariant. The invariant vector fields wðq; ob ; as ; cs Þ are found by solving the eight eight-dimensional 2 13 0 0 1 0 1 0 1 0 1 q ei 0 0 0 6 B ob C7 B 0 C Be C B0C B0C i 6 C7 B B C B C B C B C 6Djgqob as cs B C7  w ¼ B C; B C; B C; B C 4 @ as A5 @ 0 A @ 0 A @1A @0A cs 1 0 0 0 for i =1,2,3; (e1,e2,e3) is the canonical basis of R3 (recall the tangent space of the unit quaternion space is also R3 ). Since 1 0 1 2 13 0 0 q dq  q0 dq C B B 6 B ob C7 B dob C B q1  dob  q0 C C 6 C7 B B C¼B 0 C; 6Djgðq;ob ;as ;cs Þ B C7  C B C 4 @ as A5 B a0 das A @ das A @ cs c0 dcs dcs this yields the eight independent invariant vector fields 1 0 1 0 1 1 0 0 0 ei  q 0 0 B 0 C B q1  e  q C B 0 C B 0 C i C B C B C C B B C; B C; B C; i ¼ 1; 2; 3: C; B B A @ as A @ 0 A @ 0 A @ 0 cs 0 0 0 Every invariant observer then reads 3 X 1 ^ ^ bÞ þ q_^ ¼ q^  ðom o ðLi EÞei  q; 2 i¼1 3 X

zfflfflfflfflfflfflfflfflfflfflfflfflfflfflfflffl}|fflfflfflfflfflfflfflfflfflfflfflfflfflfflffl ffl{ _ 1 _ q1 0  ob  q0 þ o0 ¼ q0  o b  q0 ¼ 0;

o_^ b ¼

zffl}|ffl{ _ a0 as ¼ a0 a_ s ¼ 0;

a_^ s ¼ a^ s ðNEÞ;

z}|{ _ c0 cs ¼ c0 c_ s ¼ 0;

c_^ s ¼ c^ s ðOEÞ;

1 ^ ðMi EÞq^  ei  q;

i¼1

ARTICLE IN PRESS 716

P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

where the Li,Mi’s and N,O are arbitrary 1  6 matrices with entries possibly depending on E and I. Noticing 0 1 L1 3 X B C ðLi EÞei ¼ @ L2 AE ¼: LE; i¼1 L3 where L is the 3  6 matrix whose rows are the Li’s, and defining M in the same way, the observer can finally be rewritten under the form (12)–(15). Last but not least, the observer convergence will be analyzed thanks to the invariant state error 1 0 1 0 1 0 Z q q^ B C B ^ C B ob C BbC Bo C C B C :¼ j ^ ^ ^ ^ B b Cj ^ ^ ^ ^ B C gðq;o b ;a s ;c s Þ B a gðq;o b ;a s ;c s Þ B C BaC ^ @ as A @ A @ s A g cs c^ s 1 0 1 1q  q^ B^ ^ b ob Þ  q^ 1 C C B q  ðo C B as C B ¼B 1 C: ^s C B a C B A @ cs 1 c^ s As the state space is a group, it is more natural to use 1 0 q^  q1 0 1 Z B^ ^ b ob Þ  q^ 1 C C B q  ðo B C C B BbC as C B B C :¼ B C; BaC ^s C B a @ A C B A @ cs g ^c s

components of E except when /EA ; e3 S ¼ g, in which case /EC ; e3 S vanishes from (16). It can be checked this observer is invariant. Notice also the two ^ ^ built-in desirable geometric features: JqðtÞJ ¼ Jqð0ÞJ ¼ 1 since LE is a vector of R3 (see Appendix A); a^ s ðtÞ; c^ s ðtÞ 4 0 provided a^ s ð0Þ; c^ s ð0Þ 4 0.

3.4. The invariant error system From Section 3.2, an invariant state error is given by 1 0 q^  q1 0 1 Z B^ ^ b ob Þ  q^ 1 C q  ðo C B C B C BbC B as C B B C¼B C: BaC B C a^ s @ A B C A @ cs g c^ s Therefore,

Z_ ¼ q_^  q1 q^  ðq1  q_  q1 Þ ¼ ðLEÞ  Z12b  Z; _^ o _ b Þ  q^ 1 þ q_^  ðo ^ b ob Þ  q^ 1 b_ ¼ q^  ðo b ^ b ob Þ  ðq^ 1  q_^  q^ 1 Þ þ q^  ðo ¼ ME þðLEÞ  bb  ðLEÞ þ 12 I  b12b  I;

a_ ¼ 

g_ ¼ 

as a_^ s 2 a^ s

cs c_^ s

¼ aNE;

¼ gOE:

^ o ^ b ; a^ s ; c^ s Þ ¼ ðq; ob ; as ; cs Þ the state error equals so that when ðq; (1,0,1,1), the identity element of this group (instead of (0,0,0,0) which has no intrinsic meaning). This is completely equivalent since any invertible transformation of an invariant state error is also an invariant state error.

EA ¼ A

3.3. The general invariant observer

EC ¼ CgZ  C  Z1 ;

2 c^ s

Since E is obtained from as 1 q^  ðq1  A  qÞ  q^ ¼ AaZ  A  Z1 ; a^ s

it is found as expected that the error system From Section 3.2 every invariant observer reads ^ ^ b Þ þ ðLEÞ  q; q_^ ¼ 12q^  ðom o

ð12Þ

^ o_^ b ¼ q^ 1  ðMEÞ  q;

ð13Þ

a_^ s ¼ a^ s NE;

ð14Þ

c_^ s ¼ c^ s OE;

ð15Þ

where E is the 6  1 invariant output 1 0 1 ! ^ 1 ^ EA B A a^ s q  yA  q C C; E :¼ :¼ B A @ EC 1 C q^  yC  q^ 1 c^ s L,M are 3  6 matrices, and N,O are 1  6 matrices. These matrices can be freely chosen with entries possibly depending on the components of E and of the 3  1 (complete) invariant ^ b Þ  q^ I :¼ q^  ðom o

1

:

In fact only five of the six components of E are independent; indeed /A; CS ¼ 0 and /yA ; yC S ¼ 0 imply /EC ; AS ¼ /EA ; CS þ/EA ; EC S:

ð16Þ

This means the sixth column of L,M,N,O can be taken to 0 without loss of generality in a large domain around E= 0; indeed the sixth component /EC ; e3 S can be rewritten in function of the five other

Z_ ¼ ðLE12bÞ  Z;

ð17Þ

b_ ¼ ME þð2LE þIÞ  b;

ð18Þ

a_ ¼ aNE;

ð19Þ

g_ ¼ gOE

ð20Þ

depends only on the invariant state error ðZ; b; a; gÞ and the ‘‘free’’ known invariant I, but not on the trajectory of the observed system (5)–(8). This property greatly simplifies the convergence analysis of the observer. The linearized error system around the no-error equilibrium point ðZ ; b ; a ; g Þ ¼ ð1; 0; 1; 1Þ then reads

dZ_ ¼ 12db þLdE;

ð21Þ

db_ ¼ I  db þM dE;

ð22Þ

da_ ¼ N dE;

ð23Þ

dg_ ¼ OdE;

ð24Þ

where dE is the 6  1 vector ! ! A  dZdZ  AdaA dEA ¼ ¼ C  dZdZ  CdgC dEC

2A  dZdaA 2C  dZdgC

! :

ARTICLE IN PRESS P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

 the heading subsystem:

4. Tuning the observer So far only the structure of the observer has been investigated. Next, the gain matrices L,M,N,O must be chosen so as to meet the following requirements:

 the error must converge to zero, at least locally;  the local error behavior should be easily tunable, if possible  

with a clear physical interpretation; the magnetic measurements should not affect the attitude estimate, but only the heading; the behavior in the face of acceleration and/or magnetic disturbances should be sensible and understandable.

4.1. Local design It turns out that the previous locally around every trajectory by 0 0 l1 0 0 0 0 0 0 0 1 B B l2 L :¼ B 1 2g @ 0 0 0  l3 0 B1 0 M :¼

0

1 B B m2 B 2g @ 0

requirements can easily be met choosing 1 0 0C C C; A 0 1

m1

0

0

0

0

0

0

0

0

0

0 1 m3 B1

0C C C; A 0

N :¼

1 ð0 0 n 0 0 0Þ; g

O :¼

1 ð0 0 0 0 o 0Þ; B1 g

0

with l1 ; l2 ; l3 ; m1 ; m2 ; m3 ; n; o 4 0. This will follow from the very simple form of the linearized error system (21)–(24). Notice it is not obvious to achieve a similar result with an EKF. Indeed, (21)– (24) now read

dZ_ ¼ Dl dZ12db;

ð25Þ

db_ ¼ Dm dZ þ I  db;

ð26Þ

da_ ¼ nda;

ð27Þ

dg_ ¼ odg;

ð28Þ

where 0

0

l1 B Dl :¼ @ 0 0

0

l2 0

1

0 C A l3

0

and

Dm

m1 B :¼ @ 0 0

0 m2 0

0

1

0 C A: m3

dZ_ 3

 the longitudinal subsystem: !

db_ 1

0 l1 ¼@ m1

1 ! 1  A dZ1 2 ; db1 0

 the lateral subsystem: dZ_ 2 db_ 2

0

! ¼

@ l2 m2

1 ! 1  A dZ2 ; 2 db2 0

!

db_ 3

0 l3 ¼@ m3

1 ! 1  A dZ3 ; 2 db3 0

 the scaling subsystem: da_ ¼ nda; dg_ ¼ odg: When I a 0 the longitudinal, lateral and heading subsystems are slightly coupled by the biases errors db. It can be proved that ðdZ; db; da; dgÞ-ð0; 0; 0; 0Þ provided l1 ; l2 ; l3 ; m1 ; m2 ; m3 ; n; o 4 0 and I bounded. Indeed, the scaling subsystem obviously converges. For the other variables consider the Lyapunov function V :¼ JdbJ2 þ 2m1 dZ21 þ 2m2 dZ22 þ 2m3 dZ23 : Time-differentiating V and using /db; I  dbS ¼ 0 yields V_ ¼ 4ðl1 m1 dZ21 þl2 m2 dZ22 þl3 m3 dZ23 Þ r0: Since V is lower-bounded and V_ r0, V(t) is bounded and reaches a limit as t-1. On the other hand V€ is a polynomial function in the components of dZ; db. As V is bounded, so are dZ; db, hence V€ . V reaches a finite limit and V_ is uniformly continuous (because V€ is bounded): by Barbalat’s lemma V_ ðtÞ tends to 0 as t-1, and so does dZðtÞ. By (25)–(26) dZ€ is a polynomial function in the components of dZ; db; I. As dZ; db; I are bounded, so is dZ€ . dZðtÞ reaches a finite limit, namely 0, and Z_ is uniformly continuous (because Z€ is bounded): by Barbalat’s lemma Z_ ðtÞ tends to 0 as t-1. In other words Dl dZðtÞ 12 dbðtÞ tends to 0, and so does dbðtÞ, which ends the proof.

4.2. Global design The tuning in the previous section ensures local convergence around every trajectory of the system, which is already a very strong property. It is possible to further improve the convergence domain by modifying the correction terms at higher-order. Consider the new vector yD :¼ yC  yA ¼ q1  D  q, where D :¼ C  A ¼ g 2 B1 e1 , and the associated invariant error ED :¼ D

1 1 q^  yD  q^ : a^ s c^ s

Of course ED carries no new information since by construction DED ¼ ðCEC Þ  ðAEA Þ. It is but a convenient means to express the observer matrices; a related trick is used in Metni, Pfimlin, Hamel, and Soue res (2006). Define now L,M,N,O by LE :¼

When I= 0 (i.e. at rest) this system decouples into:

dZ_ 1

717

la lc ld A  EA þ C  EC þ D  ED ; g2 ðB1 gÞ2 ðB1 g 2 Þ2

ME :¼ sLE;

NE :¼

n la þld

OE :¼

o lc þ ld

! la /EA ; EA AS ld /ED ; ED DS ; þ g2 ðB1 g 2 Þ2 lc /EC ; EC CS ðB1 gÞ2

þ

! ld /ED ; ED DS ; ðB1 g 2 Þ2

with la ; lc ; ld ; s; n; o 4 0. LE is at first-order the same as in Section so is ME with 4.1 with (l1,l2,l3)=2(la +lc,la + ld,lc + ld);

ARTICLE IN PRESS 718

P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

ðm1 ; m2 ; m3 Þ ¼ 2sðla þlc ; la þld ; lc þld Þ. Indeed, after using (16), 1 0 0 0 0 0 la lc 0 0 0 0 0 0C 1B C B la þld L¼ B C A lc þld g@ 0 0 0 0 0  B1 1 0 lc lc lc 0 0 0 /E ; e S /E ; e S /E ; e S 1 2 3 A A A C B B1 g 2 B1 g 2 B1 g 2 C B C B C B ld ld C: B /EA ; e2 S /EA ; e1 S 0 þB 0 0 0 2 2 C B g B g 1 1 C B C B ld lc A @ /E ; e S 0 /E ; e S 0 0 0 3 1 A A B1 g 2 B1 g 2 At first order the new tuning thus gives a special case of (25)–(26). As for (27)–(28), they get coupled since   n ld N dE ¼  0 dE; 0 0 1 0 g ðla þ ld ÞB1   o ld 1 0 0 dE: OdE ¼  0 0 g ðlc þ ld ÞB1 B1

W :¼ JbJ2 þ

g2

JEA J2 þ

slc ðB1 gÞ2

JEC J2 þ

with the corresponding output error E* made up from EA :¼ A

1 1 q^  yA  q^ ; a^ s

EC :¼ C

1 1 q^  yC   q^ : c^ s

Let Z0 be the unit quaternion uniquely defined by

Z0  A  Z1 0 ¼

JA J A; JAJ

Z0  C   Z1 0 ¼

JC  J C; JCJ

and consider the coordinate change

Z~ :¼ Z  Z1 b~ :¼ b; 0

This choice of matrices provides a Lyapunov function that guarantees a large domain of convergence, while essentially preserving the nice local behavior of the previous section. Indeed, time differentiating the function

sla

measured outputs are then ! ! yA as q1  A  q :¼ ; yC  cs q1  C   q

sld ðB1 g 2 Þ2

JED J2

and using E_ A ¼ ðAEA ÞNE þðAEA Þ  ð2LEbÞ and similar expressions for E_ C ; E_ D yields _ ¼ 2ðLEÞ2 2 la þ ld ðNEÞ2 2 lc þ ld ðOEÞ2 r 0: W n o Therefore W is a Lyapunov function which globally decreases for all la ; lc ; ld ; s; n; o 40. Though it ensures a large domain of convergence, it is not clear it is enough for global convergence; see Mahony et al. (2008) for a convergence result in a simpler case (only gyro biases). Notice the Lyapunov function V used in Section 4.1 can be seen as a low-order approximation of W since ma mc md JdEA J2 þ JdEC J2 þ JdED J2 þ JdbJ2 g2 ðB1 gÞ2 ðB1 g 2 Þ2 ¼ V þma da2 þ mc dg2 þ md ðda þ dgÞ2 : The tuning proposed in Section 4.1 is very simple and ensures the observer converges locally around every trajectory. This is already a very strong property, but there is no guaranty regarding a larger convergence domain. The tuning proposed in this section is computationally slightly more complicated. It also ensures a nice local behavior, though with less freedom: since there are now only four parameters (instead of six) for (25)–(26), the longitudinal, lateral and heading subsystems may have different settling times but must have the same damping; the scaling subsystem (27)–(28) is now coupled but still has two tuning parameters. The guaranteed domain of convergence is much larger. Also notice the observer structure is flexible enough to accommodate yet other tunings.

5. Effects of disturbances Two main disturbances may affect the model. When V_ a0, the 1 A*q (recall the sign is accelerometers measure in fact asq  reversed for convenience) where A :¼ AV_ ; magnetic disturbances will change B into some B*. Accordingly, C  :¼ A  B and D :¼ C   A . For simplicity A*, B* are assumed constant. The

a~ :¼

JA J a JAJ

g~ :¼

JC  J g: JCJ

The error system (17)–(20) for these new variables reads

Z_~ ¼ ðLE Þ  Z~ 12b~  Z~ ; _

b~ ¼ ME þ ð2LE þIÞ  b~ ;

a_~ ¼ a~ NE ; g_~ ¼ g~ OE : Since  1 1 EA ¼ AaZ  Z1 ¼ Aa~Z~  A  Z~ 1 0  Z0  A  Z0  Z

EC ¼ Cg~ Z~  C  Z~ 1 ; ðZ~ ; b~ ; a~ ; g~ Þ verify the same error system as ðZ; b; a; gÞ. All the observer properties are preserved in the new coordinates, with (A*,C*,D*) playing the role of (A,C,D). An important case is when only the magnetic field is perturbed, i.e. A* =A and C* = C*1e1 + C*2e2 (instead of C =gB1e2). The disturbed equilibrium point of the error system is then   JA J JC  J ðZ ; b ; a ; g Þ ¼ Z0 ; 0; ; ; JAJ JCJ where Z0 is the quaternion ðcosc0 =2; 0; 0; sinc0 =2ÞT corresponding to a rotation of angle c0 :¼ arctanC1 =C2 . This means the observer converges to the correct roll and pitch angles, while only the estimated heading is offset because of the magnetic disturbance.

6. Experimental validation The observer is now compared with the commercial device MIDG2.1 The MIDG2 consists of a triaxial accelerometer, a triaxial gyroscope, a triaxial magnetometer, a GPS engine and an on-board computer. It can work in two different modes: in ‘‘Vertical Gyro mode’’ it estimates the attitude and heading without relying on the GPS information; in ‘‘INS mode’’ it moreover estimates the velocity vector, heavily relying on the GPS information. The algorithm is according to the user manual some EKF. The estimates and sensors raw data can be output at a pace up to 50 Hz. 1

www.microboticsinc.com

ARTICLE IN PRESS P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

la :¼ 0:06;

lc :¼ 0:10;

ld :¼ 0:06;

s :¼ 0:05;

n :¼ 0:25;

o :¼ 0:50:

Estimated Euler angles 200

φ MIDGII estimated φ

(°)

The observer (12)–(15), implemented in Simulink, is fed with the MIDG2 raw measurements. Its estimations are compared with the ones computed by the MIDG2 in Vertical Gyro Mode from the same data. To have similar behaviors, the observer parameters were set to

719

0 −200 240

250

260

270

280

290

100

300

310

θ MIDGII

(°)

estimated θ

0

On the other hand Eq. (12) was modified to −100 240

^ 2 Þq: ^ ^ b Þ þ ðLEÞ  q^ þ kð1JqJ q_^ ¼ 12q^  ðom o

270

280

290

estimated ψ

0 −200 240

250

260

270

280

290

120

140

160

(°)

60 80 100 Estimated biases

310

Estimated Euler angles with magnetic disturbance 20

40

300

time (s)

Roll Pitch Yaw

20

310

Fig. 2. Estimations during motion.

80 60 40 20 0 0

300 ψ MIDGII

Estimated Euler angles

(°)

260

200 (°)

^ which is also invariant, is a well^ 2 Þq, The extra term kð1JqJ ^ known trick to enforce JqðtÞJ ¼ 1 despite numerical errors; k :¼ 1 is used (this value is not critical). In the sequel Euler angles are displayed for convenience; both the observer and the MIDG2 compute with quaternions, eventually converted to angles.

250

φ MIDGII estimated φ

10

Roll bias Pitch bias Yaw bias

0

20

40

60 80 100 Estimated scalings

120

140

380

160

410

420

430

440

450

390

400

410

420

430

440

450

60

80

100

120

140

470

ψ MIDGII

(°) 40

460

100

Scaling as

20

470

estimated θ

Scaling cs

0

460 θ MIDGII

10

380

0.8

estimated ψ

50

160

0 380

time (s) Measured roll angular rate 0.5 (°/s)

400

0

1 0.9

390

20 (°)

(°/s)

0 0 −0.5 −1 −1.5

390

400

410

420 430 time (s)

440

450

460

470

Fig. 3. Effect of a magnetic disturbance.

0 −0.5

6.1. Comparison with commercial device (Figs. 1–3)

−1 0

20

40 60 80 Estimated roll angular rate bias

100

(°/s)

0

A long experiment (500 s) comprising three parts illustrates several nice features of the observer:

−0.1

 for t o 240 s the system is left at rest until the biases reach

−0.2 0

20

40 60 Estimated roll angle

80

100

(°)

10 With correction

0

Without correction

−10 0

20

40

60

80

100



time (s) Fig. 1. Estimations at rest. (a) Estimated biases, scaling factors and Euler angles. (b) Roll data.



constant values. Fig. 1(a) shows the observer converges despite the very wrong initial values. Fig. 1(b) further highlights the importance of the correction term for the angle estimation. Without correction the estimated roll angle diverges with a slope of  0.181/s (bottom plot); this is indeed the sensor bias value (top plot), correctly estimated by the observer (middle plot); for 240 s ot o 293 s the system is moved in all directions, then put back to rest. The observer and the MIDG2 give very similar results, Fig. 2; at t= 385 s a magnet is brought nearer for about 10 s. As expected, only the estimated yaw angle is affected by this

ARTICLE IN PRESS 720

P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

 at t= 1300 s the ‘‘accelerometers correction term’’ are also

Estimated Euler angles

switched off, i.e. la,ma and n are set to 0. All the estimated angles now diverge.

(°)

0 −10

φ MIDGII estimated φ

−20 0

1000

1500

2000

1000

1500

2000

The assumption V_ ¼ 0 may be wrong. In this case the observer no longer converges to the true values. Fig. 5 (bottom plot), e.g. displays the estimated roll angle compared to the angle produced by the MIDG2 in INS mode (in this mode the MIDG2 produces the ‘‘true’’ values, thanks to the GPS velocity information); during this motion JAV_ J sensibly differs from the 1000 mg value implied by the assumption (top plot).

θ MIDGII estimated θ

5 (°)

500

6.3. Acceleration disturbance: V_ a 0 (Fig. 5)

0 −5 0

(°)

150

500 ψ MIDGII estimated ψ

145

7. Implementation on an 8-bit microcontroller

140 0

500

1000 time (s)

1500

7.1. Preliminary implementation setup

2000

Fig. 4. Influence of correction terms.

Estimations of the roll angle with acceleration 2500 milli−g

2000

norm(yA)

1500 1000 500 0 180

185

190

195

200

(°)

φ MIDGII with GPS

40 20 0 −20 −40 −60 180

estimated φ

185

190 time (s)

195

200

Fig. 5. Estimated roll angle with V_ a 0.

To demonstrate its computational simplicity, the observer was implemented on an 8-bit microcontroller with very limited computing resources. An Atmel ATmega128, which is quite representative, was used: it costs a few euros, has only 4-Kbyte of RAM, a maximum clock speed of 16 MHz, and of course no floating-point unit. The experimental setup is a STK500/501 development kit fed (directly or indirectly) with the MIDG2 raw data and connected to a PC, see Fig. 6. The goal was to check the microcontroller implementation was possible with an estimation update rate of at least 50 Hz, and not too debased by the numerical integration scheme (a simple Euler explicit scheme at 50 Hz was used). In fact the observer is so simple that it was possible to do all the computations in C with the compiler standard floating-point emulation (thus avoiding assembly language and/or a fixed-point implementation). The experimental protocol was the following: 1. move the MIDG2 and record both its raw measurements and estimations at 50 Hz; 2. use MathWorks xPC Target to feed the ATmega with these data at 50 Hz via a serial port, while recording the estimations produced by the microcontroller-based observer; 3. compare offline the microcontroller-based estimations with those produced from the same raw data by the observer implemented in Matlab-Simulink (as in the previous section).

magnetic disturbance, Fig. 3; the MIDG2 exhibits a similar behavior. 6.2. Influence of the observer correction terms (Fig. 4) The observer correction terms are designed so that the magnetic measurements correct essentially the yaw angle and its corresponding bias, whereas the accelerometers measurements act on the other variables. This behavior is highlighted on the following experiment where the system is left at rest for 2300 s:

 for t o600 s the results are very similar for the observer and the MIDG2;

 at t= 600 s the ‘‘magnetic correction term’’ are switched off, i.e. the gains lc,ld,mc,md and o are set to 0. The yaw angle estimated by the observer diverges because the corresponding bias is no longer perfectly estimated. Indeed, these variables are not observable without the magnetic measurements. The other variables are not affected;

Fig. 6. Experimental protocol.

ARTICLE IN PRESS P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

721

Comparison ATmega/Matlab 200

φ ATmega φ Matlab

(°)

100 0 −100 −200 240

250

260

270

280

290

90

310

φ ATmega φ Matlab

85 (°)

300

80 75 70 253.25

253.3

253.35

253.4

time (s) Fig. 9. Algorithm CPU usage.

Fig. 7. Comparison ATmega/Simulink.

The two estimations are very similar, see Fig. 7; the 50 Hz discretization due to the crude Euler scheme is apparent on the bottom plot (a variable step Runge–Kutta scheme was used in Simulink). 7.2. Embedded prototype system To further test the observer potentialities, a complete embedded system not relying on the MIDG2 sensors was designed. The prototype comprises an ATmega128-based computer board, and a sensor board equipped with an Analog Devices ADIS16355 (digital triaxial gyro and accelerometer) and a PNI Micromag 3 (digital triaxial magnetometer), Fig. 8(a) and (b). The two boards can be stacked, Fig. 8(c), and fitted into a small UAV. The goal was to test the algorithm at a higher update pace, namely 100 Hz, with the microcontroller on the other hand servicing the many interrupts for the sensors and communications with a remote base. The same implementation as before was used (C language with standard floating-point emulation, Euler explicit scheme running at 100 Hz). The results are very satisfactory: the discretization effect is of course smaller; the algorithm itself uses only 28% of the 100 Hz cycle time, Fig. 9 (the low level on the scope signal corresponds to one step of the algorithm); about 30% of the remaining time, not shown in Fig. 9, is used for servicing the interrupts. It does not seem possible to run an EKF with a similar implementation.

8. Conclusion A nonlinear observer for attitude estimation from direct inertial and magnetic measurements has been proposed and completely implemented with low-cost components. It performs well, is easy to tune, computationally very thrifty, and with interesting convergence property. It can be seen as a credible but simpler alternative to the EKF.

Appendix A. Quaternions Fig. 8. The embedded prototype system. (a) Microcontroller board. (b) Sensor board. (c) Complete system.

A quaternion p can be thought of as a scalar p0 A R together p ÞT . The (non commutative) with a vector ~ p A R3 , i.e. p ¼ ðp0 ; ~

ARTICLE IN PRESS 722

P. Martin, E. Sala¨ un / Control Engineering Practice 18 (2010) 712–722

quaternion product  then reads ! p ~ q p0 q0 ~ p  q :¼ : q þ q0 ~ p0 ~ p þ~ p ~ q The cross product can be extended to quaternions by p ~ q. p  q :¼ 12 ðp  qq  pÞ ¼ ~ Quaternions provide a global parametrization of the orientation of a rigid body (whereas Euler angles necessarily have singularities). Indeed, to any quaternion q with unit norm is p  q ¼ Rq  ~ p for all associated a rotation matrix Rq A SOð3Þ by q1  ~ ~ 0ÞT , p A R3 . Any scalar p0 A R can be seen as the quaternion ðp0 ; ~ p ÞT . The quaternions and any vector ~ p A R3 as the quaternion ð0; ~ (with non zero norm) form a group with 1 as the identity element, and ðp  qÞ1 ¼ q1  p1 . If q depends on time, then q_ 1 ¼ q1  q_  q1 . Finally, consider the differential equation q_ ¼ q  u þ v  q with u; v A R3 ; then JqðtÞJ ¼ Jqð0ÞJ for all t. The tangent space at 1 of the unit norm quaternion space can be identified with R3 . Indeed, if q is a unit norm quaternion ‘‘close’’ to 1, its differential dq is dq1 e1 þ dq2 e2 þ dq3 e3 since implies q0 dq0 þq1 dq1 þ q2 dq2 þq3 dq3 ¼ 0, q20 þ q21 þq22 þq23 ¼ 1 hence dq0 ¼ 0. References Bonnabel, S., Martin, P., & Rouchon, P. (2008). Symmetry-preserving observers. IEEE Transactions on Automatic Control, 53(11), 2514–2526. Bonnabel, S., Martin, P., & Rouchon, P. (2009). Non-linear symmetry-preserving observers on lie groups. IEEE Transactions on Automatic Control, 54(7), 1709–1713. Crassidis, J. L., Markley, F. L., & Cheng, Y. (2007). Survey of nonlinear attitude estimation methods. Journal of Guidance Control and Dynamics, 30(1), 12–28.

Grewal, M., Weill, L., & Andrews, A. (2007). Global positioning systems, inertial navigation, and integration (2nd ed.). New York: Wiley. Kayton, M., & Fried, W. (1997). Avionics navigation systems (2nd ed.). New York: Wiley. Lefferts, E., Markley, F., & Shuster, M. (1982). Kalman filtering for spacecraft attitude. Journal of Guidance Control and Dynamics, 5(5), 417–429. Mahony, R., Hamel, T., & Pflimlin, J.-M. (2008). Nonlinear complementary filters on the special orthogonal group. IEEE Transactions on Automatic Control, 53(5), 1203–1218. Markley, F. (2003). Attitude error representations for kalman filtering. Journal of Guidance Control and Dynamics, 26(2), 311–317. ¨ Martin, P., & Salaun, E. (2007). Invariant observers for attitude and heading estimation from low-cost inertial and magnetic sensors. In IEEE conference on decision and control (pp. 1039–1045). ¨ E. (2008a). Design and implementation of a low-cost attitude Martin, P., & Salaun, and heading nonlinear estimator. In International conference on informatics in control, automation and robotics (pp. 53–61). ¨ Martin, P., & Salaun, E. (2008b). A general symmetry-preserving observer for attitude and heading reference systems. In IEEE conference on decision and control (pp. 2294–2301). ¨ Martin, P., & Salaun, E. (2008c). An invariant observer for Earth-velocity-aided attitude heading reference systems. In IFAC World Congress. Paper identifier 10.3182/20080706-5-KR-1001.3577. ¨ Martin, P., & Salaun, E. (2010). The true role of accelerometer feedback in quadrotor control. In IEEE international conference on robotics and automation. Preliminary version /http://hal.archives-ouvertes.fr/hal-00422423/en/S. Metni, N., Pfimlin, J.-M., Hamel, T., & Soue res, P. (2006). Attitude and gyro bias estimation for a flying UAV. Control Engineering Practice, 14, 1114–1120. Pflimlin, J.-M., Binetti, P., Soue res, P., Hamel, T., & Trouchet, D. (2010). Modeling and attitude control analysis of a ducted-fan micro aerial vehicle. Control Engineering Practice, in press, doi: 10.1016/j.conengprac.2009.09.009. Shuster, M., & Oh, S. (1981). Three-axis attitude determination from vector observations. Journal of Guidance Control and Dynamics, 4(1), 70–77. Stevens, B., & Lewis, F. (2003). Aircraft control and simulation (2nd ed.). New York: Wiley. Thienel, J., & Sanner, R. (2003). A coupled nonlinear spacecraft attitude controller and observer with an unknown constant gyro bias and gyro noise. IEEE Transactions on Automatic Control, 48(11), 2011–2015.