Attitude and Gyro Bias Estimation for a Flying UAV

Najib Metni
Laboratoire Centrale des Ponts et Chausses Paris, France [email protected]

Jean-Michel Pimlin
LAAS - CNRS Toulouse, France [email protected]

Tarek Hamel

Philippe Sou` res e

I3S-CNRS LAAS - CNRS Nice-Sophia Antipolis, France Toulouse, France [email protected] [email protected]

Abstract In this paper, a nonlinear complimentary lter (x-estimator) is presented to estimate the attitude of a UAV (Unmanned Aerial Vehicle). The measurements are taken from a low-cost IMU (Inertial Measurement Unit) which consists of 3-axis accelerometers and 3-axis gyroscopes. The gyro bias are estimated online. A second nonlinear complimentary lter (z-estimator) is also designed, it combines 3-axis gyroscope readings with 3-axis magnetometer measurements. From the proposed estimators, the full rotation matrix R will be retrieved. Both estimators use the fact that the orientation matrix, evolving on SO(3), is not locally parameterized and thus could be used to describe any kind of 3-D motion. Convergence of the two observers is theoretically proved and simulations as well as experiments are conducted on a real platform in hovering ight conditions. Index Terms Sensor Fusion, Mobile Robot Navigation, Aerial Robots, Inspection Robots.

Chauss s de Paris), we have recently started a project e called PMI (Instrumentation Plate-Form) which aims at developing a UAV capable of quasi-stationary ights in order to perform missions of inspection of bridges to localize defects and cracks. All bridges need to be inspected in details every 4 to 5 years. Using ying vehicles, the inspection will be more secure and less expensive by reducing the number of workers. It will allow to avoid the use of footbridges (gure I) and circulation trafc obstruction.

I. I NTRODUCTION A crucial problem in localizing and controlling an unmanned aerial vehicle (UAV) is the state estimation: position and orientation. A single sensor is not sufcient to measure the orientation and the position of the vehicle with respect to a xed inertial frame. The attitude is determined by the roll and pitch angles. It constitutes a part of the 3D orientation which is known also by the gravity direct cosine. Attitude estimation is done through the use of inertial sensors (accelerometers or inclinometers, gyroscopes and magnetometers). When using low-cost inertial sensors, the error growth is linear in orientation and all gravity direction sensors are sensitive to translational accelerations, making impossible the distinction between gravity and inertial forces. For most robotic systems, accelerometers could be used as attitude sensors only during phases of low accelerations. For autonomous hovering systems, capable of vertical take-off, landing and quasi-stationary (hover and near hover) ight conditions, such as helicopters, high and low accelerations readings could be used for attitude estimation. This is due to the under-actuated nature of the system and the high coupling between translational and rotational dynamics of UAV systems capable of quasistationary ight [11]. In contrast to the situation encountered in fully-actuated systems, high acceleration readings imply high gyros readings except for vertical manoeuvres for which the gyroscopes will give low values. A more detailed discussion on this topic will be given in section III-C. In LCPC-Paris (Laboratoire Centrale des Ponts et
Fig. 1. Footbridge for cracks inspection

Attitude estimation is a crucial step for defects localization. Due to the gyros drift it is not possible to rely totally on gyroscopes to estimate the vehicles attitude. The problem of attitude estimation from IMU measurements was studied from many perspectives such as navigation, aircraft technology, image stabilization, biomechanics, virtual reality and tracking of human limb systems [9]. For walking robots applications, Borenstein and Feng [3] developed a technique using odometers as well as gyros when the information from these 2 sensors differ. Many papers used also the EKF (Extended Kalman Filter) to fuse measurements taken from different inertial sensors to estimate position and attitude [6], [17]. For ying vehicles applications, Earl and DAndrea [5] studied a fusion algorithm between gyros sensors and an off-board vision system. This algorithm based on a decomposition approach uses a kalman lter to have a real-time estimate of the orientation of the vehicle. Many authors presented lter designs to estimate the orientation and position of UAVs, Kingston and Beard used a cascaded lter approach based on an Extended Kalman lter [7]. The use of inertial sensors in aerospace applications has been thoroughly studied [4]. Baerveldt and Klang [1] have analyzed the problem of fusing inclinometers and gyros using linear complimentary lter based frequency domain

analysis, but they only considered planar motions. Such a lter has been used by a few people in the area of helicopter based IMUs recently [15] and so only linear equations are applied. Rehbinder and Hu [14] addressed the case of a 3DOF motion and present a theoretical proof of the attitude estimation using inclinometers and gyroscopes, but an exponential high-gain observer of Luenberger-type was used. In [13] the authors took into account the evolution of the attitude on the SO(3) manifold but this idea was not exploited in an adequate observer. The vehicles attitude estimate was normalized to insure its evolution on the invariant manifold. Moreover, the gyroscope bias were not estimated. In this paper, two nonlinear complimentary lters are designed. The rst one (x-estimator) gives stable estimations of the mobile attitude (roll and pitch angles) and gyro bias by fusing measurements given by a 3-axis accelerometer and a 3-axis gyroscope. The second lter (z-estimator) uses accelerometer as well as magnetometer measurements. Based on both estimators and after some mathematical manipulations, the full orientation matrix R of the UAV will be retrieved. It will be shown that all estimates converge to the true states. There are 2 major contributions in this paper: rst, the orientation matrix observer exploits the structure of the Special Orthogonal Group SO(3) and its invariant manifold. Secondly, the gyro bias, assumed to be slowly varying, are estimated online. The orientation matrix extraction and gyro bias estimation are used for controlling the aerial vehicles. Experiments are done on a real UAV capable of vertical take-off, landing and quasi-stationary (hover and near hover) ight conditions. The x-estimator was successfully used on two ying vehicles: the X4-yer ` platform developed by CEA (Commissariat a lEnergie Atomique) and the VTOL (Vertical Take-Off and Landing) developed by the BERTIN-Technologies. Due to difculties with magnetometer readings, the second estimator (zestimator) has not been integrated in experimental UAVs. The outline of this paper is as follows: in Section II, the problem is formulated mathematically, models of the inertial sensors as well as a general model of a UAV are given. In section III, we describe the two observers for the estimation of the attitude, the full rotation matrix and the gyro bias and we prove their convergence using Lyapunovs methods. In section IV, we present the results of simulations and experiments. The work is concluded in section V with some discussions on future work and experiments. II. P ROBLEM F ORMULATION As each UAV has its own characteristis, it is clearly not possible to determine a general model of this kind of aerial vehicles. However, to describe their behaviour in hover or quasi-stationary ight the dynamic model of a 3D-rigid body can be used. In this section we will derive a mathematical formulation of this model to describe a ying UAV equipped with a IMU (Inertial Measurement

Unit) which is composed of a 3-axis accelerometer and a 3-axis gyroscope. A. Rigid body dynamical model Let I = {e1 , e2 , e3 } denote the direct orthogonal inertial frame such that e3 denotes the downwards vertical direction, and e1 points to the magnetic north. Let = (x, y, z) denote the position of the center of mass of the object expressed in frame I. Let B = {eb , eb , eb } be a body xed 1 2 3 frame whose center coincides with the center of mass of the mobile. The orientation of the airframe is given by a rotation R : B I, where R SO(3) is an orthogonal rotation matrix. The classical representation of R is the Euler parametrization with the 3 angles: roll(), pitch(), and yaw() 1 : c c s s c c s c s c + s s R = c s s s s + c c c s s s c . (1) s s c c c Let v I denote the linear velocity and = (p, q, r) denote the angular velocity of the airframe with respect to B. Newtons equations of motion yield the following dynamic model for the motion of a rigid body: =v v = Ry + ge3 (2) (3) (4)

RT = sk()RT ,

In equation (3), g is the acceleration of gravity, and y = 2 RT ( d 2 ge3 ) is the linear acceleration readings. For an dt idealized UAV capable of vertical take-off, landing and quasi-stationary (hover and near hover) ight conditions, the vector y = |y|e3 represents the acceleration provided by the collective thrust. The notation sk() denotes the skew-symmetric matrix such that sk()v = v for any vector v 3 and is given by: 0 r q 0 p sk() = r (5) q p 0 B. Inertial measurements and magnetometers modelling The vehicle is equipped with a strap-down IMU composed of a 3-axis rate gyroscope and a 3-axis accelerometer. The gyros rate measures the rotational velocities, the angle of rotation could be obtained through integration of this sensor signal. However, as an integration is required, even the smallest constant bias can make the error grow to innity. This phenomenon is known as drift. The value of the biased measurement of the rotational velocity will be assumed to be known. In the following analysis we denote by the biased gyro measurements: bx (6) =+b and b = by bz
1 The

following shorthand notation is used: c := cos(), s := sin(), t := tan().

where b is the bias vector of the 3-axis gyroscope. This vector is assumed to be slowly varying (its dynamics will be neglected with respect to the dynamics of the gyros mesurements) and will be estimated online through the observer presented in section III. The second part of the IMU is the accelerometer which measures the sum of all inertial forces and gravity. These measurements are expressed in the frame B. The output of the accelerometers is given by the following equation: y = RT (v ge3 ) (7)

The available inertial measurements are: noisy accelerometers and biased gyro readings. From equation (8), assuming low accelerations for the rigid body, accelerometers working as inclinometers will give us noisy readings: y = gx + . We propose a nonlinear complimentary lter to get estimations of the attitude x and the gyroscope bias b. This observer has to inherit ego-motion from the attitude dynamics of the rigid body frame. In other words, the observer has to guarantee the unit length of x and obey the following kinematics: x x = sk( + x ) (11)

where v is given by equation (3). The accelerometers will be used as attitude sensors allowing to measure the direction of gravity. To this end, v will be considered as disturbances on the accelerometer readings. Formally, we rewrite equation (7) as follows: y = gRT e3 + (8)

where = RT v are considered as measurement per turbations. This assumption is acceptable when studying the attitude of the mobile in hover or quasi-stationary conditions (see section III-C). For further information on inertial measurements, the reader is referred to any standard text on INS Unit (see for example [12]). To achieve complete attitude estimation, we add to inertial measurements a 3-axis magnetometer from which it is possible to derive the yaw angle by measuring the earth magnetic eld in the body xed frame. Let m be the direct cosine vector of the earth magnetic eld expressed in the inertial frame. Assuming without loss of generality that m span{e1 , e3 }, and denoting by z its expression in the body xed frame B, we have the following equation: z = RT m (9)

where x represents the innovation vector. Lemma 3.1: : Consider the nonlinear complimentary lter (11). Choose the following expressions for the innovation x and for the dynamics of the gyroscope bias estimate: b x x = kx ( x) = kb ( x) x b (12) (13)

where kx and kb are positive constants. Then for any positive ( << 1) such that |(0)| = 1 x xT (0)x(0) > 1 + (14) 2 b(0) kb > T
2(1+ (0)x(0)) x

In the following section, we will present an observer which allows to reconstruct the complete attitude matrix, associated to an adaptive estimation of gyros bias b. III. O BSERVER D ESIGN In this section, we rst design an observer which estimates the gravity direct cosine in the body xed frame and adapts gyros bias estimation to ensure the convergence of the observer. In the case where the vehicle is in hover condition, we will show that such an estimator does not allow to ensure the convergence of gyros bias error. Then, we prove that adding magnetometers information allows to estimate the full attitude matrix and ensures the convergence of gyros bias error to zero. A. Gravity direction cosine x-estimator In this section, we present a nonlinear complimentary lter for the direction of gravity in the body frame given by x = RT e3 , where e3 = (0, 0, 1) is the unit vector in the I-frame. As the vector e3 is constant in the inertial frame and using equation (4), the kinematic equation of x (for some (t)) is given by: x = RT e3 = (sk()RT )e3 = sk()x (10)

the estimates x(t) converges to the true value of x(t) and the gyros bias estimation error = b converges to b b a constant. Moreover, if x(t) is sufciently rich (x(t) is not a constant vector), the and gyros bias estimation error = b converges to zero b b Proof: We must prove that x converges to the true value of x and that the gyro bias vector estimate b converges to a constant vector. Let us consider rst the following storage function: S = 1 xT x Using (6), (10), (11) and (12), the time derivative of S1 can be written under the following form: b x S = T sk( + b kx ( x))x + xT sk()x x At this stage, one must keep in mind two main properties of skew-matrix (cf. [10]): u, v
3

sk(u)T = sk(u) sk(u)v = sk(v)u

The gyroscope bias error estimation is expressed by: = b b So we have: b. S = xT sk(x) kx xT sk(x)( x) b x Knowing that xT sk(x) = ( x)T , the expression of the x storage function derivative can be rewritten as follows: b S = ( x)T kx x x x
2

At this step, we are able to determine a Lyapunov function S2 for the x-estimator: 1 2 S2 = S + b 2kb Assuming that the gyros bias vector is slowly varying (b 0), the Lyapunov time derivative is then expressed as: 1 T b b (15) kb From equation (15), it is clear how to choose the dynamics for to cancel the contribution of the gyroscope bias error b estimation to the dynamics of S2 . Using expression (13) b for the dynamics of the derivative of the positive denite b, storage function S2 becomes: S2 = ( x)T kx x x x b
2

S2 = kx x x

(16)

From the last equation where the storage function derivative is negative semi-denite, standard Lyapunov argument concludes that the cross product ( x) tends to zero and x therefore the estimate of the attitude x converges to the true value x (the solution x = x, is excluded due to the constraint (14)). Indeed, if the adaptive control gain kb satises the constraint (14), one can insure that the Lyapunov function associated with the x-estimator veries: S2 < 2 and then xT (t)x(t) > 1 + , t 0. To address the problem of gyroscopes bias error estimation we appeal to LaSalle principle. The invariant set is contained in the set dened by the condition x x. = 0 on the invariant set Recalling (13), it follows that b and therefore the gyros bias estimation error converges to a constant value . Taking x = x x, we have b x x x = sk()x + sk( + kx ( x)) b (17) Note that from equation (16), x x then x 0 and x 0. Thus veries : b sk( )x = 0 b In the case where the vector x is sufciently rich (x is not a constant direction), one can insure that the constant vector converges to zero and therefore the bias estimate b converge to the true vector b. b Note that, in the case where x is a constant vector, one can insure that is stable and converges to a constant vector b collinear to the direction of x. In order to extract the full orientation of the body and estimation of the gyros bias without disturbing the xestimator response, a secondary observer is proposed. The approach taken here requires additional information. B. Complete orientation matrix estimation using magnetometers 1) z-estimator and complete gyros bias adaptive lter: In the previous section, we proved that the gyro bias estimation error tends to zero if and only if x(t) is not reduced to a constant vector. Otherwise, it is a vector collinear

to x. To achieve the characterization of the orientation matrix and guarantee the gyros bias error convergence to zero for all x, we have to build an additional estimator to determine another inertial vector (assumed to be constant in the inertial frame). Following a reasoning similar to the one used in section III, we will prove that the gyro bias estimation error is forced to converge to zero, whatever the value of x is. In this way, another orientation direction of the full attitude will be extracted. In this section we will consider the earth magnetic eld as the second inertial vector (constant in the I-frame) and we will add to the previous observer an estimator of the earth magnetic eld direct cosine in the frame B. We will show that adding such an estimator allows to restitute the full orientation matrix R and ensures the convergence of gyros bias estimation error to zero. b From equation (9), the evolution of the vector z is given by the following equation: z = RT m = sk()RT m = sk()z (18)

Note that m is a constant vector in I. By analogy with the previous nonlinear complimentary lter, we will propose the following z-estimator: Theorem 3.2: Consider the x-estimator dened by equations (11) and (12) and combine it with the following observer of the earth magnetic eld direction cosine z: z = sk( + z ) z kz ( z) z = b z (19) (20)

with kz > 0. Moreover, consider, instead of relation (13), the gyros bias adaptive lter given by: = kb [( x) + ( z)] x z b kb > 0 (21)

Then, for any positive ( << 1) such that: |(0)| = 1 z z (0)T z(0) > 1 + 2 b(0) kb > T T

(22)

2(2+ (0)x(0)+ (0)z(0)) x z

the estimates ((t) , z (t) , x b(t)) converge asymptotically to the true values (x(t) , z(t) , b) from any initial x(0) and z (0) satisfying constraints (14) and (22). Proof: : For the proof of this proposition, we have to introduce a Lyapunov function VT for the global system (x-estimator and z-estimator). A simple idea consists in augmenting the previous storage function S2 as follows: V T = S2 + 1 z T z Using (19), its time derivative is written under the following form: VT = S2 z T sk( + b kz ( z))z + z T sk()z b z Introducing the gyros bias error estimation = b and b b replacing S2 by its expression (equation(15)), it yields: 1 VT = (x)T x xx 2 T z z)T z z z x bk b b+( bk kb
2

 Replacing by its expression given in equation (21), one b can cancel the inuence of in the expression of VT and b T : ensure the non positivity of V VT = kx x x
2

kz z z

(23)

The last equation ensures the convergence of x(t) to the true gravity direction x(t) and the convergence of z (t) to the true earth magnetic eld cosine direction z(t) in the body frame. Similarly to previous proposition, the constraint on the adaptive gain (22), insures that the vector x (resp. z ) does not converge to x (resp. z). Using a reasoning similar to the one described by equation (17), the gyros bias estimation error converges b to a constant verifying: b sk( )x = 0 b sk( )z = 0 b (24) (25)

From the above analysis, the error will converge to a b vector collinear to both x and z. Knowing that x and z are two independent vectors, this ensures that = 0 and b consequently, b. b 2) Complete rotation matrix restitution: In this section a simple method allowing to build the complete attitude matrix estimation R from both vectors x and z is proposed. The main idea is to use the Gram-Schmidt orthonormaliza tion method to construct RT e3 and RT e1 starting from x and z and to complete the base by the cross product of both vectors. Knowing that the vector z = RT m belongs T e1 and RT e3 to the plane spanned by the two vectors R (m span{e1 , e3 }) and using the convention that the unit vector e1 points to the north (eT m > 0), one can verify 1 that the rst and the third rows of RT may be dened as follows: RT e3 = x xT x T e1 = z(T z) R z ( z ) x x As the second row of RT is dened by the cross product between RT e3 and RT e1 , it yields: RT e2 = (RT e3 ) (RT e1 ) = z x z (T z ) x x

by rotating the airframe in order to provide the necessary acceleration for the linear dynamics [11]. Moreover, the high accelerations will happen only in transient modes and for small time intervals. Note however that it is possible that accelerations readings, even in stationary ight, be contaminated during the ight by very strong vibrations which are generally of high frequency. By choosing appropriate gains for the proposed lter one can insure that the x-estimator (or both proposed lters) works as a complimentary lter insuring that the estimate x is the result of fusing low bandwidth accelerometer readings with high bandwidth gyros measurements. Since the proposed estimator has a nonlinear expression, we do not have a frequency domain interpretation. However, by linearizing the dynamics of the x-estimator ( = RT e3 ), it yields: x x = , x = x + kx (x x) sk( x b) 1 where x , represents the estimated attitude using orientation velocity. In other words, the relation x(0) = x(0) implies that x represent the actual attitude. Taking the second order derivative of the linearized estimation of x near hover condition and considering that the gyros bias vector b is slowly varying, it yields: x = x + kx (x x) + kb (x x) Using the Laplace transform, it follows: X(s) = s2 s2 kx s + kb X (s) + 2 X(s) + kx s + kb s + kx s + kb

Which represents the equation of a classical complimentary lter. This is due to the fact that the sum of the two transfer functions given in the above expression is equal to 1. The rst transfer function works in high bandwidth frequencies and the second one in low bandwidth frequencies. Note that, the denominator of both transfer functions has the following form:
2 s2 + 20 s + 0

Finally, the complete expression of the orientation matrix estimation could be written as follows: R=
z (T z ) x x z (T z ) x x x z z (T z ) x x

If we choose = 22 , then the cutoff frequency for the lter is at kb rad/s. By identication, it follows: kx = 2kb .

C. Discussions on accelerometer readings and on the proposed nonlinear complimentary lter implementation The attitude is calculated by integrating angular rates measurements given by gyroscopes. Accelerometers will work as inclinometers to correct the predicted attitude and gyros bias. Taking the dynamic equations of the system (equations (2) to (4)) and considering the case of the under-actuated VTOL UAV, accelerometers could be used as inclinometers to estimate the gravity direction. This is natural for the considered under-actuated dynamics since any motion, except vertical manoeuvres for which the translational and rotational motion are decoupled, is obtained

The design of the gain kb is based on the analysis of characteristics and frequencies of the vibration which contaminate the measurements and has to verify the constraint (14). Finally, the use of the proposed estimator is very appropriate in the special case of autonomous hovering systems, capable of vertical take-off, landing and quasi-stationary (hover and near hover) ight conditions. In fact, the xestimator was successfully implemented on two different experimental VTOL vehicles developed by two laboratories (Bertin-Technologie and the CEA research center).

IV. S IMULATIONS & E XPERIMENTAL R ESULTS In this section, we present simulations and experimental results to show the efciency of the proposed lters. The x-estimator has been successfully integrated in the embedded software of the VTOL UAV HoverEye developed by Bertin Technologies (see [11]). The VTOL was equipped with an IMU. Magnetometers have not been integrated in the UAV due to perturbations caused by electrical engines. The frequency of inertial sensors data acquisition is 60 Hz. At a given temperature, gyro bias are assumed to be constant (or slowly varying). This assumption holds to almost all kind of inertial gyroscopes [2]. Due to the discrete data acquisition and in order to pre serve the evolution of the estimated matrix R on the SO(3) manifold, the proposed estimators have been implemented in simulations and experiments in discrete time using exact integration of equations (11) and (19) and Euler integration of equation (21) corresponding to the gyro bias estimation dynamics. More precisely, if we denote the sample time by T and assume that (t) = k for t [kT, (k + 1)T ], then we have the discrete time model of the observer: xk+1 = zk+1 = k+1 = bk + T kb [( x) + ( z)] x z b Ax xk k Az zk k
k (.) where Ak = esk(k +(.) )T has the closed form solution given by Rodrigues formula (see equation [16]):

30 measure est
gyro

20

10 pitch ()

10

20

30

10

15

20 time (s)

25

30

35

40

Fig. 2. Experimental result of the use of the x-estimator ltering for the HoverEye VTOL UAV. The dotted line corresponds to the pitch angle measurement extracted from accelerometers readings, the dashed line is the result of pure gyros integration and the solid line is the pitch angle estimation using the x-estimator.

The initial condition of the gyro bias estimation vector is chosen to be: = (0.2, 0.2, 0.2) b This corresponds to a large error between the real and estimated gyro bias vectors. Moreover, in order to show the efciency of the proposed lters the following Gaussian noise: = N (0, 6 degrees) has been added to the actual orientation (see gure IV).
40

30

Ak = I
(.)

(.)

(26) and (resp. is the innovation where u = given by equation (12) (resp. (20)). The discrete form will (.) also preserve the SO(3) manifold evolution because Ak could be viewed as a rotation matrix thus preserving the vector unit length. The rst experiment concerns the estimation result provided by the x-estimator of the VTOL HoverEye near hover ight conditions along with the discrete form of equation (13) instead of equation (21) for the estimate The results b. are shown on gure IV. Due the lack of the true values of the vehicle orientation and in order to compare actual and estimated orientations, simulation results have been added. The experiments simulates the VTOL UAV in stationary ight where the orientation matrix R is equal to identity (the Euler angles for the actual orientation are set to zero). Both estimators have been simulated together to extract the full orientation. The initial condition of the estimated matrix R is given in terms of Euler angles ( = 60o , = o o 20 , = 5 ). Instead of considering the gyro bias vector b as constant, the following choice is made:
k k + (.) k x k z )

Measured Pitch ()

(.) (.) sin(uk T ) sk(uk ) (.) uk

+ sk2 (uk )

(.)

(.) cos(uk T ) (.) uk 2

20

10

10

20

30

40

100

200

300

400

500 Time

600

700

800

900

1000

Fig. 3.

Prole of the considered orientation measurements

b = (bx , by , bz ) = (0.1rd/s, 0.021trd/s, 0.05 sin(0.1t))

Different values of the gain kb have been used that satisfy the constraint (22) and correspond to different cutoff frequencies. The gains kx and kz have been chosen such that kx = kz = 2kb to insure a good behavior of the lters and corresponds to the previous discussion on the complimentary lter presented in section III-C. Simulation results are presented in gure IV. A good behavior of the estimator can be observed despite the large errors between initial conditions of actual and estimated Euler angles, the important varying gyros drift and the important additional Gaussian noise. Comparing

200 Roll () 100 0

100 200 100 Pitch () 50 0 kb=14 kb=4 kb=1 0 20 40 60 80 100 Time 120 140 160 180 200

50 200 Yaw () 100 0 100

20

40

60

80

100 Time

120

140

160

180

200

kb=14 kb=4 k =1
b

20

40

60

80

100 Time

120

140

160

180

200

UAV. The algorithm could be used for all 6-DOF motion in many applications. Unlike the Euler angles parametrization, the global representation of the orientation matrix R has no singularities. The nonlinear aspect of the motion have been also preserved in the lter design in contrast with the linearisation needed for the Kalman and the Extended Kalman lters. To estimate the full matrix R, another sensor must be added to IMU. In this paper, we used a 3-axis magnetometer, but any other inertial vector could be used instead. Our present work investigates fusing information from a INS (Inertial Navigation Sensor) and a camera which will be useful in estimating the full orientation as well as correcting the inertial sensors drift. Vision would be an adequate candidate for replacing magnetometers [8], which are not sensitive to magnetic perturbations caused by electrical engines. R EFERENCES
[1] A.J. Baerveldt and R. Klang. A low-cost and low-weight attitude estimation system for an autonomous helicopter. pages 391395, 1997. [2] J. Borenstein, H.R. Everett, and L. Feng. Where am I?. 1996. [3] J. Borenstein and L. Feng. Gyrodometry: A new method for combining data from gyros and odometry in mobile robots. In IEEE International Conference on Robot and Automation, pages 423428, 1996. [4] R.P.G. Collinson. Introducton to avionics. Chapman and Hall, 1996. [5] M.G. Earl and R. DAndrea. Real-time estimation techniques applied to a four-rotor helicopter. In 43rd IEEE COnference on Decision and Control, pages 39563961, December 2004. [6] Y. Fuke and E. krotkov. Dead reckoning for a lunar rover on uneven terrain. In IEEE International Conference on Robot and Automation, pages 411416, 1996. [7] D. Kingston and R. Beard. Real-time attitude and position estimation for small uavs using low-cost sensors. In AIAA 3rd Unmanned Unlimited Systems Conference and Workshop, September 2004. [8] J. Lobo and J. Dias. Vision and inertial sensor cooperation using gravity as a vertical reference. In IEEE Transactions on Pattern Analysis and Machine Intelligence, volume 25, pages 15971608, 2003. [9] R. McGhee, E. Bachmann, X. Yun, and M. Zyda. Real-time tracking and display of human limb segment motions using sourceless sensors and a quaternion-based ltering algorithm. Technical report, Naval Postgraduate School, November 2000. [10] R.M. Murray, Z. Li, and S. Sastry. A mathematical introduction to robotic manipulation. CRC Press, 1994. [11] J.M. Pimlin, P. Soures, and T. Hamel. Hovering ight stabilization in wind gusts for ducted fan uav. In 44th Conference on Decision and Control CDC04, Bahamas, 2004. [12] J.C. Radix. Systmes inertiels composants lis strap - down. Cepadues Edition, 1980. [13] H. Rehbinder and X. Hu. Drift-free attitude estimation for accelerated rigid bodies. Automatica, April. [14] H. Rehbinder and X. Hu. Nonlinear state estimation for rigid-body motion with low-pass sensors. Systems and Control Letters, 40. [15] S. Saripalli, J. Roberts, P. Corke, and G. Buskey. A tale of two helicopters. In IEEE/RSJ, International Robots and Systems, pp. 805-810, Oct 2003, Las Vegas, USA. [16] S. Sastry. Nonlinear Systems: Analysis, stability and control. Springer, 1999. [17] J. Vaganay and M.J. Aldon. Attitude estimation for a vehicle using inertial sensors. Control Engineering Practice, 2.

Fig. 4. Results of proposed lters using different values of the gain kb . kb = 1, 4, 14.

different plots corresponding to different choices of kb , it may be seen that, for small values of the gain kb , the added disturbance do not qualitatively degrade the performance of the proposed nonlinear complimentary lters. This conrms that the proposed estimators work as complimentary lters. Indeed, from the discussion presented in section IIIC and by considering the linear dynamics of the lters, the cutoff frequency of the proposed lters will correspond to kb .
0.6 Biasx (rad) 0.4 0.2 0 0 20 40 60 80 100 Time (s) 120 140 160 180 200

0.2 4 Biasy (rad) 2 0

2 0.6 Biasz (rad) 0.4 0.2 0

20

40

60

80

100 Time (s)

120

140

160

180

200

0.2

20

40

60

80

100 Time (s)

120

140

160

180

200

Fig. 5.

Evolution of the estimation of the gyro bias vector components

Finally by observing the evolution of the gyro bias estimation vector (gure IV) it is shown that effective tracking is maintained at all times after a short transient due to the large initial error on b. V. C ONCLUDING REMARKS By taking advantage of the evolution of the attitude on the SO(3) manifold, we have designed in this paper a nonlinear complimentary lter to estimate the attitude (roll and pitch angles) as well as gyroscopes bias for a