IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 1

Transcription

1 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 1 Non-linear complementary filters on the special orthogonal group Robert Mahony, Member, IEEE, Tarek Hamel, Member, IEEE, an Jean-Michel Pflimlin, Member, IEEE Abstract This paper consiers the problem of obtaining goo attitue estimates from measurements obtaine from typical low cost inertial measurement units. The outputs of such systems are characterise by high noise levels an time varying aitive biases. We formulate the filtering problem as eterministic observer kinematics pose irectly on the special orthogonal group SO3 riven by reconstructe attitue an angular velocity measurements. Lyapunov analysis results for the propose observers are erive that ensure almost global stability of the observer error. The approach taken leas to an observer that we term the irect complementary filter. By exploiting the geometry of the special orthogonal group a relate observer, terme the passive complementary filter, is erive that ecouples the gyro measurements from the reconstructe attitue in the observer inputs. Both the irect an passive filters can be extene to estimate gyro bias on-line. The passive filter is further evelope to provie a formulation in terms of the measurement error that avois any algebraic reconstruction of the attitue. This leas to an observer on SO3, terme the explicit complementary filter, that requires only accelerometer an gyro outputs; is suitable for implementation on embee harware; an provies goo attitue estimates as well as estimating the gyro biases on-line. The performance of the observers are emonstrate with a set of experiments performe on a robotic test-be an a raio controlle unmanne aerial vehicle. Inex Terms Complementary filter, nonlinear observer, attitue estimates, special orthogonal group. I. INTRODUCTION THE recent proliferation of Micro-Electro-Mechanical Systems MEMS components has lea to the evelopment of a range of low cost an light weight inertial measurement units. The low power, light weight an potential for low cost manufacture of these units opens up a wie range of applications in areas such as virtual reality an gaming systems, robotic toys, an low cost mini-aerialvehicles MAVs such as the Hovereye Fig. 1. The signal output of low cost IMU systems, however, is characterise by low-resolution signals subject to high noise levels as well as general time-varying bias terms. The raw signals must be processe to reconstruct smoothe attitue estimates an biascorrecte angular velocity measurements. For many of the low cost applications consiere the algorithms nee to run on embee processors with low memory an processing resources. R. Mahony is with Department of Engineering, Australian National University, ACT, 000, Australia. Robert.Mahony@anu.eu.au. T. Hamel is with I3S-CNRS, Nice-Sophia Antipolis, France. thamel@i3s.unice.fr. J.-M. Pflimlin is with Department of Navigation, Dassault Aviation, Saint Clou, Paris. France. Jean-Michel.Pflimlin@assault-aviation.com. Manuscript receive November 08, 006; revise August 03, 007. There is a consierable boy of work on attitue reconstruction for robotics an control applications for example [1] [4]. A stanar approach is to use extene stochastic linear estimation techniques [5], [6]. An alternative is to use eterministic complementary filter an non-linear observer esign techniques [7] [9]. Recent work has focuse on some of the issues encountere for low cost IMU systems [9] [1] as well as observer esign for partial attitue estimation [13] [15]. It is also worth mentioning the relate problem of fusing IMU an vision ata that is receiving recent attention [16] [19] an the problem of fusing IMU an GPS ata [9], [0]. Parallel to the work in robotics an control there is a significant literature on attitue heaing reference systems AHRS for aerospace applications [1]. An excellent review of attitue filters is given by Crassiis et al. []. The recent interest in small low-cost aerial robotic vehicles has lea to a renewe interest in lightweight embee IMU systems [8], [3] [5]. For the low-cost light-weight systems consiere, linear filtering techniques have prove extremely ifficult to apply robustly [6] an linear single-input single-output complementary filters are often use in practice [5], [7]. A key issue is on-line ientification of gyro bias terms. This problem is also important in IMU callibration for satellite systems [5], [1], [8] [31]. An important evelopment that came from early work on estimation an control of satellites was the use of the quaternion representation for the attitue kinematics [30], [3] [34]. The non-linear observer esigns that are base on this work have strong robustness properties an eal well with the bias estimation problem [9], [30]. However, apart from the earlier work of the authors [14], [35], [36] an some recent work on invariant observers [37], [38] there appears to be almost no work that consiers the formulation of non-linear attitue observers irectly on the matrix Lie-group representation of SO3. In this paper we stuy the esign of non-linear attitue observers on SO3 in a general setting. We term the propose observers complementary filters because of the similarity of the architecture to that of linear complementary filters cf. Appenix A, although, for the non-linear case we o not have a frequency omain interpretation. A general formulation of the error criterion an observer structure is propose base on the Lie-group structure of SO3. This formulation leas us to propose two non-linear observers on SO3, terme the irect complementary filter an passive complementary filter. The irect complementary filter is closely relate to recent work on invariant observers [37], [38] an correspons up to some minor technical ifferences to non-linear observers propose using the quaternion representation [9], [30], [3].

2 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR Fig. 1. The VTOL MAV HoverEye c of Bertin Technologies. We o not know of a prior reference for the passive complementary filter. The passive complementary filter has several practical avantages associate with implementation an lowsensitivity to noise. In particular, we show that the filter can be reformulate in terms of vectorial irection measurements such as those obtaine irectly from an IMU system; a formulation that we term the explicit complementary filter. The explicit complementary filter oes not require on-line algebraic reconstruction of attitue, an implicit weakness in prior work on non-linear attitue observers [] ue to the computational overhea of the calculation an poor error characterisation of the constructe attitue. As a result the observer is ieally suite for implementation on embee harware platforms. Furthermore, the relative contribution of ifferent ata can be preferentially weighte in the observer response, a property that allows the esigner to ajust for application specific noise characteristics. Finally, the explicit complementary filter remains well efine even if the ata provie is insufficient to algebraically reconstruct the attitue. This is the case, for example, for an IMU with only accelerometer an rate gyro sensors. A comprehensive stability analysis is provie for all three observers that proves local exponential an almost global stability of the observer error ynamics, that is, a stable linearisation for zero error along with global convergence of the observer error for all initial conitions an system trajectories other than on a set of measure zero. Although the principal results of the paper are presente in the matrix Lie group representation of SO3, the equivalent quaternion representation of the observers are presente in an appenix. The authors recommen that the quaternion representations are use for harware implementation. The boy of paper consists of five sections followe by a conclusion an two appenices. Section II provies a quick overview of the sensor moel, geometry of SO3 an introuces the notation use. Section III etails the erivation of the irect an passive complementary filters. The evelopment here is eliberately kept simple to be clear. Section IV integrates on-line bias estimation into the observer esign an provies a etaile stability analysis. Section V evelops the explicit complementary filter, a reformulation of the passive complementary filter irectly in terms of error measurements. A suite of experimental results, obtaine uring flight tests of the Hovereye Fig. 1, are provie in Section VI that emonstrate the performance of the propose observers. In aition to the conclusion VII there is a short appenix on linear complementary filter esign an a secon appenix that provies the equivalent quaternion formulation of the propose observers. II. PROBLEM FORMULATION AND NOTATION. A. Notation an mathematical ientities The special orthogonal group is enote SO3. The associate Lie-algebra is the set of anti-symmetric matrices so3 = {A R 3 3 A = A T } For any two matrices A, B R n n then the Lie-bracket or matrix commutator is [A, B] = AB BA. Let Ω R 3 then we efine Ω = 0 Ω 3 Ω Ω 3 0 Ω 1 Ω Ω 1 0. For any v R 3 then Ω v = Ω v is the vector cross prouct. The operator vex : so3 R 3 enotes the inverse of the Ω operator vex Ω = Ω, Ω R 3. vexa = A, A so3 For any two matrices A, B R n n the Eucliean matrix inner prouct an Frobenius norm are efine A, B = tra T B = n A ij B ij i,j=1 A = A, A = n i,j=1 A ij

3 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 3 The following ientities are use in the paper Rv = Rv R T, R SO3, v R 3 v w = [v, w ] v, w R 3 v T w = v, w = 1 v, w, v, w R 3 v T v = v = 1 v, v R 3 A, v = 0, A = A T R 3, v R 3 tr[a, B] = 0, A, B R 3 3 The following notation for frames of reference is use {A} enotes an inertial fixe frame of reference. {B} enotes a boy-fixe-frame of reference. {E} enotes the estimator frame of reference. Let P a, P s enote, respectively, the anti-symmetric an symmetric projection operators in square matrix space P a H = 1 H HT, P s H = 1 H + HT. Let θ, a a = 1 enote the angle-axis coorinates of R SO3. One has [39]: R = expθa, logr = θa cosθ = 1 trr 1, P ar = sinθa. For any R SO3 then 3 trr 1. If trr = 3 then θ = 0 in angle-axis coorinates an R = I. If trr = 1 then θ = ±π, R has real eigenvalues 1, 1, 1, an there exists an orthogonal iagonalising transformation U SO3 such that URU T = iag1, 1, 1. For any two signals xt : R M x, yt : R M y are terme asymptotically epenent if there exists a nonegenerate function f t : M x M y R an a time T such that for any t > T f t xt, yt = 0. By the term non-egenerate we mean that the Hessian of f t at any point x, y is full rank. The two signals are terme asymptotically inepenent if for any non-egenerate f t an any T there exists t 1 > T with f t xt 1, yt 1 0. B. Measurements The measurements available from a typical inertial measurement unit are 3-axis rate gyros, 3-axis accelerometers an 3-axis magnetometers. The reference frame of the strap own IMU is terme the boy-fixe-frame {B}. The inertial frame is enote {A}. The rotation R = A BR enotes the relative orientation of {B} with respect to {A}. Rate Gyros: The rate gyro measures angular velocity of {B} relative to {A} expresse in the boy-fixe-frame of reference {B}. The error moel use in this paper is Ω y = Ω + b + µ R 3 where Ω {B} enotes the true value, µ enotes aitive measurement noise an b enotes a constant or slowly time-varying gyro bias. Accelerometer: Denote the instantaneous linear acceleration of {B} relative to {A}, expresse in {A}, by v. An ieal accelerometer, strappe own to the boy-fixe-frame {B}, measures the instantaneous linear acceleration of {B} minus the conservative gravitational acceleration fiel g 0 where we consier g 0 expresse in the inertial frame {A}, an provies a measurement expresse in the boy-fixe-frame {B}. In practice, the output a from a MEMS component accelerometer has ae bias an noise, a = R T v g 0 + b a + µ a, where b a is a bias term an µ a enotes aitive measurement noise. Normally, the gravitational fiel g 0 = g 0 e 3 where g ominates the value of a for low frequency response. Thus, it is common to use v a = a a RT e 3 as a low-frequency estimate of the inertial z-axis expresse in the boy-fixe-frame. Magnetometer: The magnetometers provie measurements of the magnetic fiel m = R T A m + B m + µ b where A m is the Earths magnetic fiel expresse in the inertial frame, B m is a boy-fixe-frame expression for the local magnetic isturbance an µ b enotes measurement noise. The noise µ b is usually quite low for magnetometer reaings, however, the local magnetic isturbance can be very significant, especially if the IMU is strappe own to an MAV with electric motors. Only the irection of the magnetometer output is relevant for attitue estimation an we will use a vectorial measurement v m = m m in the following evelopment The measure vectors v a an v m can be use to construct an instantaneous algebraic measurement of the rotation A B R : {B} {A} R y = arg min λ1 e 3 Rv a + λ vm Rv m A BR R SO3 where vm is the inertial irection of the magnetic fiel in the locality where ata is acquire. The weights λ 1 an λ are chosen epening on the relative confience in the sensor outputs. Due to the computational complexity of solving an optimisation problem the reconstructe rotation is often obtaine in a suboptimal manner where the constraints are applie in sequence; that is, two egrees of freeom in the rotation matrix are resolve using the acceleration reaings an the final egree of freeom is resolve using the magnetometer. As a consequence, the error properties of the reconstructe attitue R y can be ifficult to characterise. Moreover, if either magnetometer or accelerometer reaings are unavailable ue to local magnetic isturbance or high acceleration manoeuvres then it is impossible to resolve the vectorial measurements into a unique instantaneous algebraic measurement of attitue.

4 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 4 C. Error criteria for estimation on SO3 Let ˆR enote an estimate of the boy-fixe rotation matrix R = A B R. The rotation ˆR can be consiere as coorinates for the estimator frame of reference {E}. It is also associate with the frame transformation ˆR = A E ˆR : {E} {A}. The goal of attitue estimate is to rive ˆR R. The estimation error use is the relative rotation from boy-fixeframe {B} to the estimator frame {E} R := ˆR T R, R = E B R : {B} {E}. 1 The propose observer esign is base on Lyapunov stability analysis. The Lyapunov functions use are inspire by the cost function E tr := 1 4 I 3 R = 1 I 4 tr 3 R T I 3 R One has that = 1 tri 3 R E tr = 1 tri R = 1 cosθ = sinθ/. 3 where θ is the angle associate with the rotation from {B} to frame {E}. Thus, riving Eq. to zero ensures that θ 0. III. COMPLEMENTARY FILTERS ON SO3 In this section, a general framework for non-linear complementary filtering on the special orthogonal group is introuce. The theory is first evelope for the iealise case where Rt an Ωt are assume to be known an use to rive the filter ynamics. Filter esign for real worl signals is consiere in later sections. The goal of attitue estimation is to provie a set of ynamics for an estimate ˆRt SO3 to rive the error rotation Eq. 1 Rt I 3. The kinematics of the true system are Ṙ = RΩ = RΩ R 4 where Ω {B}. The propose observer equation is pose irectly as a kinematic system for an attitue estimate ˆR on SO3. The observer kinematics inclue a preiction term base on the Ω measurement an an innovation or correction term ω := ω R erive from the error R. The general form propose for the observer is ˆR = RΩ + k P ˆRω ˆR, ˆR0 = ˆR0, 5 where k P > 0 is a positive gain. The term RΩ + k P ˆRω {A} is expresse in the inertial frame. The boy-fixe-frame angular velocity is mappe back into the inertial frame A Ω = RΩ. If no correction term is use k P ω 0 then the error rotation R is constant, R = ˆR T RΩ T R + ˆR T RΩ R = ˆR T RΩ + RΩ R = 0. 6 The correction term ω := ω R {E} is consiere to be in the estimator frame of reference. It can be thought of as a non-linear approximation of the error between R an ˆR as measure from the frame of reference associate with ˆR. In practice, it will be implemente as an error between a measure estimate R y of R an the estimate ˆR. The goal of the observer esign is to fin a simple expression for ω that leas to robust convergence of R I. In prior work [35], [36] the authors introuce the following correction term ω := vexp a R = vexp a ˆR T R y 7 This choice leas to an elegant Lyapunov analysis of the filter stability. Differentiating the storage function Eq. along trajectories of Eq. 5 yiels Ė tr = 1 tr R k P = ω tr T R = k [ P tr ω P T s R + P a R ] = k [ P tr ω P T a R ] = k P ω, P a R = k P ω 8 In Mahony et al. [35] a local stability analysis of the filter ynamics Eq. 5 is provie base on this erivation. In Section IV a global stability analysis for these ynamics is provie. We term the filter Eq. 5 a complementary filter on SO3 since it recaptures the block iagram structure of a classical complementary filter cf. Appenix A. In Figure : The ˆR T R R Fig.. SO3. Difference operation on SO3 ˆR T R R P a R ˆR T Inverse operation on SO3 Maps error R onto T I SO3. k Ω RΩ Maps angular velocity into correct frame of reference Maps angular velocity RΩ onto T I SO A ˆR = A ˆR System kinematics Block iagram of the general form of a complementary filter on operation is an inverse operation on SO3 an is equivalent to a operation for a linear complementary filter. The ˆR T R y operation is equivalent to generating the error term y ˆx. The two operations P a R an RΩ are maps from error space an velocity space into the tangent space of SO3; operations that are unnecessary on Eucliean space ue to the ientification T x R n R n. The kinematic moel is the Liegroup equivalent of a first orer integrator. To implement the complementary filter it is necessary to map the boy-fixe-frame velocity Ω into the inertial frame. In practice, the true rotation R is not available an an estimate of the rotation must be use. Two possibilities are consiere: irect complementary filter: The constructe attitue R y is use to map the velocity into the inertial frame ˆR = R y Ω y + k P ˆRω ˆR. A block iagram of this filter esign is shown in Figure 3. This approach can be linke to observers ocumente ˆR

5 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 5 in earlier work [30], [3] cf. Appenix B. The approach has the avantage that it oes not introuce an aitional feeback loop in the filter ynamics, however, high frequency noise in the reconstructe attitue R y will enter into the fee-forwar term of the filter. the architecture shown in Figure 5. The passive complementary filter avois coupling the reconstructe attitue noise into the preictive velocity term of the observer, has a strong Lyapunov stability analysis, an provies a simple an elegant realisation that will lea to the results in Section V. Ω y R yω y Ω y Ω R y R yω y R y ˆRT R P a R k ˆR = ˆRA ˆR ˆR T R y R P a R k + + ˆR = A ˆR A ˆR ˆR T Fig. 3. ˆR T Block iagram of the irect complementary filter on SO3. Fig. 5. filter. Block iagram of the simplifie form of the passive complementary passive complementary filter: The filtere attitue ˆR is use in the preictive velocity term Fig. 4. ˆR = ˆRΩ y + k P ˆRω ˆR. 9 A block iagram of this architecture is shown in Figure 4. The avantage lies in avoiing corrupting the preictive angular velocity term with the noise in the reconstructe pose. However, the approach introuces a seconary feeback loop in the filter an stability nees to be prove. Ω y R y ˆR T R y ˆR T R P a R ˆRΩ y ˆRΩ y + k + A ˆR = A ˆR Block iagram of the passive complementary filter on SO3. A key observation is that the Lyapunov stability analysis in Eq. 8 is still vali for Eq. 9, since E tr = 1 tr R 1 = tr Ω + k P ω R + RΩ = 1 tr[ R, Ω ] k P trωt R = k P ω, using the fact that the trace of a commutator is zero, tr[ R, Ω ] = 0. The filter is terme a passive complimentary filter since the cross coupling between Ω an R oes not contribute to the erivative of the Lyapunov function. A global stability analysis is provie in Section IV. There is no particular theoretical avantage to either the irect or the passive filter architecture in the case where exact measurements are assume. However, it is straightforwar to see that the passive filter Eq. 9 can be written ˆR = ˆRΩ + k P P a R. 10 This formulation suppresses entirely the requirement to represent Ω an ω = k P P a R in the inertial frame an leas to ˆR IV. STABILITY ANALYSIS In this section, the irect an passive complementary filters on SO3 are extene to provie on-line estimation of timevarying bias terms in the gyroscope measurements an global stability results are erive. Preliminary results were publishe in [35], [36]. For the following work it is assume that a reconstructe rotation R y an a biase measure of angular velocity Ω y are available R y R, vali for low frequencies, 11a Ω y Ω + b for constant bias b. 11b The approach taken is to a an integrator to the compensator term in the feeback equation of the complementary filter. Let k P, k I > 0 be positive gains an efine Direct complementary filter with bias correction: ˆR = R y Ω y ˆb + k P ˆRω ˆR, ˆR0 = ˆR0, 1a ˆb = k I ω, ˆb0 = ˆb0, ω = vexp a R, 1b R = ˆRT R y. 1c Passive complementary filter with bias correction: ˆR = ˆR Ω y ˆb + k P ω, ˆR0 = ˆR0, 13a ˆb = k I ω, ˆb0 = ˆb0, 13b ω = vexp a R, R = ˆRT R y. 13c The non-linear stability analysis is base on the iea of an aaptive estimate for the unknown bias value. Theorem 4.1: [Direct complementary filter with bias correction.] Consier the rotation kinematics Eq. 4 for a time-varying Rt SO3 an with measurements given by Eq. 11. Let ˆRt, ˆbt enote the solution of Eq. 1. Define error variables R = ˆR T R an b = b ˆb. Define U SO3 R 3 by Then: U = { R, b tr R = 1, P a b R = 0 }. 14

6 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 6 1 The set U is forwar invariant an unstable with respect to the ynamic system Eq. 1. The error Rt, bt is locally exponentially stable to I, 0. 3 For almost all initial conitions R 0, b 0 U the trajectory ˆRt, ˆbt converges to the trajectory Rt, b. Proof: Substituting for the error moel Eq. 11, Equation 1a becomes ˆR = RΩ + b + k P ˆRω ˆR. Differentiating R it is straightforwar to verify that R = k P ω R b R. 15 Define a caniate Lyapunov function by V = 1 tri 3 R + 1 k I b = E tr + 1 k I b 16 Differentiating V one obtains V = 1 tr R 1 bt ˆb k I = 1 k tr P ω R + b R 1 b, ˆb k I = k P ω, P a R + P s R 1 b, P a R + P s R 1 k I b, ˆb = k P ω, vexp a R b, vexp a R 1 k I b, ˆb Substituting for ˆb an ω Eqn s 1b an 1c one obtains V = k P ω = k P vexp a R 17 Lyapunov s irect metho ensures that ω converges asymptotically to zero [40]. Recalling that P a R = sinθ, where θ, a enotes the angle-axis coorinates of R. It follows that ω 0 implies either R = I, or log R = πa for a = 1. In the secon case one has the conition tr R = 1. Note that ω = 0 is also equivalent to requiring R = R T to be symmetric. It is easily verifie that I, 0 is an isolate equilibrium of the error ynamics Eq. 18. From the efinition of U one has that ω 0 on U. We will prove that U is forwar invariant uner the filter ynamics Eqn s 1. Setting ω = 0 in Eq. 15 an Eq. 1b yiels R = b R, ˆb = For initial conitions R 0, b 0 = R 0, b 0 U the solution of Eq. 18 is given by Rt = exp t b R 0, bt = b0, R 0, b 0 U. 19 We verify that Eq. 19 is also a general solution of Eqn s 15 an 1b. Differentiating tr R yiels t tr R = tr b exp t b R 0 = tr exp t b b R0 + R 0 b = tr exp t b P a b R0 = 0, where the secon line follows since b commutes with exp b an the final equality is ue to the fact that P a b R0 = 0, a consequence of the choice of initial conitions R 0, b 0 U. It follows that tr Rt = 1 on solution of Eq. 19 an hence ω 0. Classical uniqueness results verify that Eq. 19 is a solution of Eqn s 15 an 1b. It remains to show that such solutions remain in U for all time. The conition on R is prove above. To see that P a b R 0 we compute t P a b R = Pa b R = P a b R bt = 0 as R = R T. This proves that U is forwar invariant. Applying LaSalle s principle to the solutions of Eq. 1 it follows that either R, b I, 0 asymptotically or R, b R t, b 0 where R t, b 0 U is a solution of Eq. 18. To etermine the local stability properties of the invariant sets we compute the linearisation of the error ynamics. We will prove exponential stability of the isolate equilibrium point I, 0 first an then return to prove instability of the set U. Define x, y R 3 as the first orer approximations of R an b aroun I, 0 R I + x, x so3 0a b = y. 0b The sign change in Eq. 0b simplifies the analysis of the linearisation. Substituting into Eq. 15, computing b an iscaring all terms of quaratic or higher orer in x, y yiels t x y = kp I 3 I 3 k I I 3 0 x y 1 For positive gains k P, k I > 0 the linearise error system is strictly stable. This proves part ii of the theorem statement. To prove that U is unstable, we use the quaternion formulation see Appenix B. Using Eq. 49, the error ynamics of the quaternion q = s, ṽ associate to the rotation R is given by s = 1 k P s ṽ + ṽ T b, a b = k I sṽ, b ṽ = 1 s b + k P sṽ + b ṽ, c It is straightforwar to verify that the invariant set associate to the error ynamics is characterise by { U = s, ṽ, b s = 0, ṽ = 1, b } T ṽ = 0

7 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 7 Define y = b T ṽ, then an equivalent characterisation of U is given by s, y = 0, 0. We stuy the stability properties of the equilibrium 0, 0 of s, y evolving uner the filter ynamics Eq. 1. Combining Eq. c an b, one obtains the following ynamics for ẏ ẏ = ṽ T b + bt ṽ = k I s ṽ 1 s b 1 k P s y Linearising aroun small values of s, y one obtains s 1 = k 1 P s ẏ k I 1 b 0 0 y Since K P an K I are positive gains it follows that the linearisation is unstable aroun the point 0, 0 an this completes the proof of part i. The linearisation of the ynamics aroun the unstable set is either strongly unstable for large values of b 0 or hyperbolic both positive an negative eigenvalues. Since b 0 epens on the initial conition then there there will be trajectories that converge to U along the stable centre manifol [40] associate with the stable irection of the linearisation. From classical centre manifol theory it is known that such trajectories are measure zero in the overall space. Observing in aition that U is measure zero in SO3 R 3 proves part iii an the full proof is complete. The irect complimentary filter is closely relate to quaternion base attitue filters publishe over the last fifteen years [9], [30], [3]. Details of the similarities an ifferences is given in Appenix B where we present quaternion versions of the filters we propose in this paper. Apart from the formulation irectly on SO3, the present paper extens earlier work by proposing globally efine observer ynamics an a full global analysis. To the authors best unerstaning, all prior publishe algorithms epen on a sgnθ term that is iscontinuous on U Eq. 14. Given that the observers are not well efine on the set U the analysis for prior work is necessarily non-global. However, having note this, the recent work of Thienel et al. [30] provies an elegant powerful analysis that transforms the observer error ynamics into a linear timevarying system the transformation is only vali on a omain on SO3 R 3 U for which global asymptotic stability is prove. This analysis provies a global exponential stability uner the assumption that the observer error trajectory oes not intersect U. In all practical situations the two approaches are equivalent. The remainer of the section is evote to proving an analogous result to Theorem 4.1 for the passive complementary filter ynamics. In this case, it is necessary to eal with non-autonomous terms in the error ynamics ue to passive coupling of the riving term Ω into the filter error ynamics. Interestingly, the non-autonomous term acts in our favour to isturb the forwar invariance properties of the set U Eq. 14 an reuce the size of the unstable invariant set. Theorem 4.: [Passive complementary filter with bias correction.] Consier the rotation kinematics Eq. 4 for a time-varying Rt SO3 an with measurements given by Eq. 11. Let ˆRt, ˆbt enote the solution of Eq. 13. Define error variables R = ˆR T R an b = b ˆb. Assume that Ωt is a boune, absolutely continuous signal an that the pair of signals Ωt, R are asymptotically inepenent see II-A. Define U 0 SO3 R 3 by { U 0 = R, b tr R = 1, b } = 0. 3 Then: 1 The set U 0 is forwar invariant an unstable with respect to the ynamic system 13. The error Rt, bt is locally exponentially stable to I, 0. 3 For almost all initial conitions R 0, b 0 U 0 the trajectory ˆRt, ˆbt converges to the trajectory Rt, b. Proof: Substituting for the error moel Eq. 11 in Eqn s 13 an ifferentiating R, it is straightforwar to verify that R = [ R, Ω ] k P ω R b R, b = k I ω 4a 4b The proof procees by ifferentiating the Lyapunov-like function Eq. 16 for solutions of Eq. 13. Following an analogous erivation to that in Theorem 4.1, but aitionally exploiting the cancellation tr[ R, Ω ] = 0, it may be verifie that V = k P ω = k P vexp a R where V is given by Eq. 16. This bouns V t V 0, an it follows b is boune. LaSalle s principle cannot be applie irectly since the ynamics Eq. 4a are not autonomous. The function V is uniformly continuous since the erivative V = k P P a R T P a [ R, Ω ] P a k P ω b R is uniformly boune. Applying Barbalat s lemma proves asymptotic convergence of ω = vexp a R to zero. Direct substitution shows that R, b = I, 0 is an equilibrium point of Eq. 4. Note that U 0 U Eq. 14 an hence ω 0 on U Th For R, b U 0 the error ynamics Eq. 4 become R = [ R, Ω ], b = 0. The solution of this orinary ifferential equation is given by Rt = exp At R 0 expat, At = t 0 Ω τ. Since At is anti-symmetric for all time then exp At is orthogonal an since exp At = expat T it follows R is symmetric for all time. It follows that U 0 is forwar invariant uner the filter ynamics Eq. 13. We prove by contraiction that U 0 U is the largest forwar invariant set of the closeloop ynamics Eq. 13 such that ω 0. Assume that there exits R 0, b 0 U U 0 such that Rt, bt remains in U for all time. One has that P a b R = 0 on this trajectory. Consequently, t P a b R = Pa b [ R, T Ω ] P b Rb = P a b [ R, Ω ] = 1 b Ω R + R b Ω = 0, 5

8 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 8 where we have use P a b R = b R + R b = 0, 6 several times in simplifying expressions. Since Ωt, Rt are asymptotically inepenent then the relationship Eq. 5 must be egenerate. This implies that there exists a time T such that for all t > T then bt 0 an contraicts the assumption. It follows that either R, b I, 0 asymptotically or R, b R t, 0 U 0. Analogously to Theorem 4.1 the linearisation of the error ynamics Eq. 4 at I, 0 is compute. Let R I + x an b y for x, y R 3. The linearise ynamics are the time-varying linear system t x y = kp I 3 Ωt I 3 k I I 3 0 x Let Ω max enote the magnitue boun on Ω an choose α > 0, α 1 > α Ω max + k I, k P α 1 + k P α < α 3 < α 1 + k P α + Ω max α k I k I k I Set P, Q to be matrices α1 I 3 α I 3 kp α 1 α k I α Ω max P =, Q = α I 3 α 3 I 3 α Ω max α 7 It is straightforwar to verify that P an Q are positive efinite matrices given the constraints on {α 1, α, α 3 }. Consier the cost function W = 1 ξt P ξ, with ξ = x, y T. Differentiating W yiels Ẇ = k P α 1 α k I x α y + y T xα 1 + k P α α 3 k I + α y T Ω x 8 It is straightforwar to verify that ξ T P ξ x x, y Q. t y This proves exponential stability of the linearise system at I, 0. The linearisation of the error ynamics on a trajectory in U 0 are also time varying an it is not possible to use the argument from Theorem 4.1 to prove instability. However, note that V R, b = for all R, b U 0. Moreover, any neighbourhoo of a point R, b U 0 within the set SO3 R 3 contains points R, b such the V R, b <. Trajectories with these initial conitions cannot converge to U 0 ue to the ecrease conition erive earlier, an it follows that U 0 is unstable. Analogous to Theorem 4.1 it is still possible that a set of measure zero initial conitions, along with very specific trajectories Ωt, such that the resulting trajectories converge to to U 0. This proves part iii an completes the proof. Apart from the expecte conitions inherite from Theorem 4.1 the key assumption in Theorem 4. is the inepenence of Ωt from the error signal R. The perturbation of the y passive ynamics by the inepenent riving term Ω provies a isturbance that ensures that the aaptive bias estimate converges to the true gyroscopes bias, a particularly useful property in practical applications. V. EXPLICIT ERROR FORMULATION OF THE PASSIVE COMPLEMENTARY FILTER A weakness of the formulation of both the irect an passive an complementary filters is the requirement to reconstruct an estimate of the attitue, R y, to use as the riving term for the error ynamics. The reconstruction cannot be avoie in the irect filter implementation because the reconstructe attitue is also use to map the velocity into the inertial frame. In this section, we show how the passive complementary filter may be reformulate in terms of irect measurements from the inertial unit. Let v 0i {A}, i = 1,..., n, enote a set of n known inertial irections. The measurements consiere are boyfixe-frame observations of the fixe inertial irections v i = R T v 0i + µ i, v i {B} 9 where µ i is a noise process. Since only the irection of the measurement is relevant to the observer we assume that v 0i = 1 an normalise all measurements to ensure v i = 1. Let ˆR be an estimate of R. Define ˆv i = ˆR T v 0i to be the associate estimate of v i. For a single irection v i, the error consiere is which yiels E i = 1 cos v i, ˆv i = 1 v i, ˆv i E i = 1 tr ˆR T v 0i v T 0iR = 1 tr RR T v 0i v T 0iR For multiple measures v i the following cost function is consiere n n E mes = k i E i = k i tr RM, k i > 0, 30 where i=1 i=1 M = R T M 0 R with M 0 = n k i v 0i v0i T 31 Assume linearly inepenent inertial irection {v 0i } then the matrix M is positive efinite M > 0 if n 3. For n = then M is positive semi-efinite with one eigenvalue zero. The weights k i > 0 are chosen epening on the relative confience in the measurements v i. For technical reasons in the proof of Theorem 5.1 we assume aitionally that the weights k i are chosen such that M 0 has three istinct eigenvalues λ 1 > λ > λ 3. Theorem 5.1: [Explicit complementary filter with bias correction.] Consier the rotation kinematics Eq. 4 for a timevarying Rt SO3 an with measurements given by Eqn s 9 an 11b. Assume that there are two or more, n vectorial measurements v i available. Choose k i > 0 such i=1

9 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 9 that M 0 efine by Eq. 31 has three istinct eigenvalues. Consier the filter kinematics given by ˆR = ˆR Ω y ˆb + k P ω mes, ˆR0 = ˆR0 3a ˆb = k I ω mes 3b n ω mes := k i v i ˆv i, k i > 0. 3c i=1 an let ˆRt, ˆbt enote the solution of Eqn s 3. Assume that Ωt is a boune, absolutely continuous signal an that the pair of signals Ωt, R T are asymptotically inepenent see II-A. Then: 1 There are three unstable equilibria of the filter characterise by ˆR i, ˆb i = U 0 D i U T 0 R, b, i = 1,, 3, where D 1 = iag1, 1, 1, D = iag 1, 1, 1 an D 3 = iag 1, 1, 1 are iagonal matrices with entries as shown an U 0 SO3 such that M 0 = U 0 ΛU0 T where Λ = iagλ 1, λ, λ 3 is a iagonal matrix. The error Rt, bt is locally exponentially stable to I, 0. 3 For almost all initial conitions R 0, b 0 ˆR i T R, b, i = 1,..., 3, the trajectory ˆRt, ˆbt converges to the trajectory Rt, b. Proof: Define a caniate Lyapunov-like function by n V = k i tr RM + 1 k b = E mes + 1 b I k I i=1 The erivative of V is given by V = tr RM + RṀ bt ˆb k I = tr [ RM, Ω ] b + k P ω mes RM bt ˆb k I Recalling that the trace of a commutator is zero, the erivative of the caniate Lyapunov function can be simplifie to obtain V = k P tr ω mes P a RM +tr b P a RM 1kI ˆb 33 Recalling the ientities in Section II-A one may write ω mes as n k i ω mes = ˆv ivi T v i ˆv T i = P a RM 34 i=1 Introucing the expressions of ω mes into the time erivative of the Lyapunov-like function V, Eq. 33, one obtains V = k P P a RM. The Lyapunov-like function erivative is negative semiefinite ensuring that b is boune. Analogous to the proof of Theorem 4., Barbalat s lemma is invoke to show that P a RM tens to zero asymptotically. Thus, for V = 0 one has RM = M R T. 35 We prove next Eq. 35 implies either R = I or tr R = 1. Since R is a real matrix, the eigenvalues an eigenvectors of R verify R T x k = λ k x k an x H k R = λ H k x H k 36 where λ H k for k = represents the complex conjugate of the eigenvalue λ k an x H k represents the Hermitian transpose of the eigenvector x k associate to λ k. Combining Eq. 35 an Eq. 36, one obtains x H k RMx k = λ H k x H k Mx k x H k M R T x k = λ k x H k Mx k = λ H k x H k Mx k Note that for n 3, M > 0 is positive efinite an x H k Mx k > 0, k = {1,, 3}. One has λ k = λ H k for all k. In the case when n =, it is simple to verify that two of the three eigenvalues are real. It follows that all three eigenvalues of R are real since complex eigenvalues must come in complex conjugate pairs. The eigenvalues of an orthogonal matrix are of the form eig R = 1, cosθ + i sinθ, cosθ i sinθ, where θ is the angle from the angle-axis representation. Given that all the eigenvalues are real it follows that θ = 0 or θ = ±π. The first possibility is the esire case R, b = I, 0. The secon possibility is the case where tr R = 1. When ω mes 0 then Eqn s 3 an Eqn s 13 lea to ientical error ynamics. Thus, we use the same argument as in Theorem 4. to prove that b = 0 on the invariant set. To see that the only forwar invariant subsets are the unstable equilibria as characterise in part i of the theorem statement we introuce R = R ˆR T. Observe that RM = M R T RM 0 = M 0 RT Analogous to Eq. 35, this implies R = I 3 or tr R = 1 on the set ω mes 0 an R = R T. Set R = U0 T RU 0. Then R Λ Λ R = 0 i, j λ i λ j R ij = 0 As M 0 has three istinct eigenvalues, it follows that R ij = 0 for all i j an thus R is iagonal. Therefore, there are four isolate equilibrium points R 0 = U 0 D i U T 0, i = 1,..., 3 where D i are specifie in part i of the theorem statement an R = I that satisfy the conition ω mes 0. The case R 0 = I = U 0 D 4 U T 0 where D 4 = I correspons to the equilibrium R, b = I, 0 while we will show that the other three equilibria are unstable. We procee by computing the ynamics of the filter in the new R variable an using these ynamics to prove the stability properties of the equilibria. The ynamics associate to R are R = Ṙ ˆR T + R ˆRT = RΩ ˆRT RΩ + b ˆRT T k P RP a RM ˆR = R b ˆRT k P R RM M R T ˆR T = R b R T R ˆR T k P R ˆR T M 0 R R T M 0 ˆR ˆRT = R b R k P RM 0 R M0

10 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 10 Setting b = R b, one obtains R = b R k P RM 0 R M0. 37 The ynamics of the new estimation error on the bias b are b = Ṙ b R T + R b Ṙ T + k I RP a RMR T = [RΩ, b ] + k I R ˆR T M 0 R R T M 0 ˆRR T = [RΩ, b ] + k I RM 0 M 0 RT 38 The ynamics of R, b Eqn s 37 an 38 are an alternative formulation of the error ynamics to R, b. Consier a first orer approximation of R, b Eqn s 37 an 38 aroun an equilibrium point R 0, 0 R = R 0 I 3 + x, The linearisation of Eq. 37 is given by an thus an finally b = y. R 0 ẋ = y R0 k P R 0 x M 0 R0 + R 0 M 0 R0 x, ẋ = R T 0 y R0 k P x M 0 R0 + M 0 R0 x, U T 0 ẋ U 0 = D i U T 0 y D i k P U T 0 x ΛD i +ΛD i U T 0 x for i = 1,..., 4 an where Λ is specifie in part i of the theorem statement. Define A 1 = 0.5iagλ + λ 3, λ 1 + λ 3, λ 1 + λ A = 0.5iagλ λ 3, λ 1 λ 3, λ 1 + λ A 3 = 0.5iag λ + λ 3, λ 1 + λ 3, +λ 1 λ A 4 = 0.5iag λ λ 3, λ 1 λ 3, λ 1 λ Setting y = U T 0 y an x = U T 0 x one may write the linearisation Eq. 37 as ẋ = k P A i x + D i y, i = 1,..., 4. We continue by computing the linearisation of b. Equation 38 may be approximate to a first orer by an thus ẏ = [RΩ, y ] + k I R 0 x M 0 + M 0 x R0 U T 0 ẏ U 0 = [U T 0 RΩ, y ] + k I D ix Λ + Λx D i. Finally, for i = 1,..., 4 U T 0 ẏ U 0 = k I D ix D i Λ + ΛD i D i x + [Ω, y ]. Rewriting in terms of the variables x, y an setting Ω = U T 0 RΩ one obtains ẏ = k I A i D i x + Ω y, for i = 1,..., 4. The combine error ynamic linearisation in the prime coorinates is ẋ kp A i D i x ẏ = k I A i D i Ω t y, i = 1,..., To complete the proof of part i of the theorem statement we will prove that the three equilibria associate with R i, b i for i = 1,, 3 are unstable. The emonstration is analogous to the proof of the Chetaev s Theorem see [40, pp ]. Consier the following cost function: S = 1 k Ix T A i x 1 y It is straightforwar to verify that its time erivative is always positive Ṡ = k P k I A i x. Note that for i = 1,..., 3 then A i has at least one element of the iagonal positive. For each i = 1,..., 3 an r > 0, efine U r = {ξ = x, y T : Sξ > 0, ξ < r} an note that U r is non-null for all r > 0. Let ξ 0 U r such that Sξ 0 > 0. A trajectory ξ t initialize at ξ 0 = ξ 0 will iverge from the compact set U r since Ṡξ > 0 on U r. However, the trajectory cannot exit U r through the surface Sξ = 0 since Sξ t Sξ 0 along the trajectory. Restricting r such that the linearisation is vali, then the trajectory must exit U r through the sphere ξ = r. Consequently, trajectories initially arbitrarily close to 0, 0 will iverge. This proves that the point 0, 0 is locally unstable. To prove local exponential stability of R, b = I, 0 we consier the linearisation Eq. 39 for i = 4. Note that D 4 = I an A 4 < 0. Set K P = k P A 4 an K I = k I A 4. Then K P, K I > 0 are positive efinite an Eq. 39 may be written as t x y = KP I 3 K I Ω t x Consier a cost function V = ξ T P ξ with P given by Eq. 7. Analogous to Eq. 8, the time erivative of V is given by V = K P α 1 α K I x α y + y T x α 1 + K P α α 3 K I α x T Ω y. Once again, it is straightforwar to verify that x V x, y Q y where Q is efine in Eq. 7 an this proves local exponential stability of R, b = I, 0. The final statement of the theorem follows irectly from the above results along with classical ynamical systems theory an the proof is complete. Remark: If n = 3, the weights k i = 1, an the measure irections are orthogonal v T i v j = 0, i j then M = I 3. The cost function E mes becomes E mes = 3 tr RM = tri 3 R = E tr. y

11 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 11 In this case, the explicit complementary filter Eqn s 3 an the passive complementary filter Eqn s 13 are ientical. Remark: It is possible to weaken the assumptions in Theorem 5.1 to allow any choice of gains k i an any structure of the matrix M 0 an obtain analogous results. The case where all three eigenvalues of M 0 are equal is equivalent to the passive complementary filter scale by a constant. The only other case where n > has M 0 = U 0 iagλ 1, λ 1, λ U T 0 for λ 1 > λ 0. Note that the situation where n = 1 is consiere in Corollary 5.. It can be shown that any symmetry R = expπa with a span{v 01, v 0 } satisfies ω mes 0 an it is relatively straightforwar to verify that this set is forwar invariant uner the close-loop filter ynamics. This invaliates part i of Theorem 5.1 as state, however, it can be shown that the new forwar invariant points are unstable as expecte. To see this, note that any R, b in this set correspons to the minimal cost of E mes on U 0. Consequently, any neighbourhoo of R, b contains points R, b such that V R, b < V R, b an the Lyapunov ecrease conition ensures instability. There is still a separate isolate unstable equilibrium in U 0, an the stable equilibrium, that must be treate in the same manner as unertaken in the formal proof of Theorem 5.1. Following through the proof yiels analogous results to Theorem 5.1 for arbitrary choice of gains {k i }. The two typical measurements obtaine from an IMU unit are estimates of the gravitational, a, an magnetic, m, vector fiels v a = R T a 0 a 0, v m = R T m 0 m 0. In this case, the cost function E mes becomes E mes = k 1 1 ˆv a, v a + k 1 ˆv m, v m The weights k 1 an k are introuce to weight the confience in each measure. In situations where the IMU is subject to high magnitue accelerations such as uring takeoff or laning manoeuvres it may be wise to reuce the relative weighting of the accelerometer ata k 1 << k compare to the magnetometer ata. Conversely, in many applications the IMU is mounte in the proximity to powerful electric motors an their power supply busses leaing to low confience in the magnetometer reaings choose k 1 >> k. This is a very common situation in the case of mini aerial vehicles with electric motors. In extreme cases the magnetometer ata is unusable an provies motivation for a filter base solely on accelerometer ata. A. Estimation from the measurements of a single irection Let v a be a measure boy fixe frame irection associate with a single inertial irection v 0a, v a = R T v 0a. Let ˆv a be an estimate ˆv a = ˆR T v 0a. The error consiere is E mes = 1 tr RM; M = R T v 0a v T 0aR Corollary 5.: Consier the rotation kinematics Eq. 4 for a time-varying Rt SO3 an with measurements given by Eqn s 9 for a single measurement v 1 = v a an Eq. 11b. Let ˆRt, ˆbt enote the solution of Eqn s 3. Assume that Ωt is a boune, absolutely continuous signal an Ωt, v a t are asymptotically inepenent see II-A. Define U 1 = { R, b : v T 0a Rv 0a = 1, b = 0}. Then: 1 The set U 1 is forwar invariant an unstable uner the close-loop filter ynamics. The estimate ˆv a, ˆb is locally exponentially stable to v a, b. 3 For almost all initial conitions R 0, b 0 U 1 then ˆv a, ˆb converges to the trajectory v a t, b. Proof: The ynamics of ˆv a are given by ˆv a = Ω + b + k P v a ˆv a ˆv a 40 Define the following storage function V = E mes + 1 k I b. The erivative of V is given by V = k P v a ˆR T v 0a = k P v a ˆv a The Lyapunov-like function V erivative is negative semiefinite ensuring that b is boune an v a ˆv a 0. The set v a ˆv a = 0 is characterise by v a = ±ˆv a an thus ˆv T av a = ±1 = v T 0a ˆR T Rv 0a = v T 0a R v 0a. Consier a trajectory ˆv a t, b t that satisfies the filter ynamics an for which ˆv a = ±v a for all time. One has t v a ˆv a = 0 = Ω v a ˆv a v a Ω ˆv a v a b ˆv a k P v a v a ˆv a ˆv a = ±v a b v a = 0. Differentiating this expression again one obtains Ω v a b v a + v a b Ω v a = 0 Since the signals Ω an v a are asymptotically inepenent it follows that the functional expression on the left han sie is egenerate. This can only hol if b 0. For ˆv a = v a, this set of trajectories is characterise by the efinition of U 1. It is straightforwar to aapt the arguments in Theorems 4.1 an 4. to see that this set is forwar invariant. Note that for b = 0 then V = E mes. It is irect to see that ˆv a t, b t lies on a local maximum of E mes an that any neighbourhoo contains points such that the full Lyapunov function V is strictly less than its value on the set U 1. This proves instability of U 1 an completes part i of the corollary. The proof of part ii an part iii is analogous to the proof of Theorem 5.1 see also [15]. An important aspect of Corollary 5. is the convergence of the bias terms in all egrees of freeom. This ensures that, for a real worl system, the rift in the attitue estimate aroun the unmeasure axis v 0a will be riven asymptotically by a zero mean noise process rather than a constant bias term. This makes the propose filter a practical algorithm for a wie range of MAV applications.

12 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 1 VI. EXPERIMENTAL RESULTS In this section, we present experimental results to emonstrate the performance of the propose observers. Experiments were unertaken on two real platforms to emonstrate the convergence of the attitue an gyro bias estimates. 1 The first experiment was unertaken on a robotic manipulator with an IMU mounte on the en effector an supplie with synthetic estimates of the magnetic fiel measurement. The robotic manipulator was programme to simulate the movement of a flying vehicle in hovering flight regime. The filter estimates are compare to orientation measurements compute from the forwar kinematics of the manipulator. Only the passive an irect complimentary filters were run on this test be. The secon experiment was unertaken on the VTOL MAV HoverEye c evelope by Bertin Technologies Figure 1. The VTOL belongs to the class of sit on tail ucte fan VTOL MAV, like the istar9 an Kestrel evelope respectively by Allie Aerospace [41] an Honeywell [4]. It was equippe with a low-cost IMU that consists of 3-axis accelerometers an 3-axis gyroscopes. Magnetometers were not integrate in the MAV ue to perturbations cause by electrical motors. The explicit complementary filter was use in this experiment. For both experiments the gains of the propose filters were chosen to be: k P = 1ra.s 1 an k I = 0.3ra.s 1. The inertial ata was acquire at rates of 5Hz for the first experiment an 50Hz for the secon experiment. The quaternion version of the filters Appenix B were implemente with first orer Euler numerical integration followe by rescaling to preserve the unit norm conition. Experimental results for the irect an passive versions of the filter are shown in Figures 6 an 7. In Figure 6 the only significant ifference between the two responses lies in the initial transient responses. This is to be expecte, since both filters will have the same theoretical asymptotic performance. In practice, however, the increase sensitivity of the irect filter to noise introuce in the computation of the measure rotation R y is expecte to contribute to slightly higher noise in this filter compare to the passive. The response of the bias estimates is shown in Figure 7. Once again the asymptotic performance of the filters is similar after an initial transient. From this figure it is clear that the passive filter isplays slightly less noise in the bias estimates than for the irect filter note the ifferent scales in the y-axis. Figures 8 an 9 relate to the secon experiment. The experimental flight of the MAV was unertaken uner remote control by an operator. The experimental flight plan use was: First, the vehicle was locate on the groun, initially heae towar ψ0 = 0. After take off, the vehicle was stabilize in hovering conition, aroun a fixe heaing which remains close the initial heaing of the vehicle on the groun. Then, the operator engages a 90 o -left turn manoeuvre, returns to the initial heaing, an follows with a 90 o -right turn Fig. 6. Fig. 7. roll φ pitch θ yaw ψ b est irect /s 50 0 φ measure φ passive φ irect θ measure θ passive θ irect ψ measure ψ passive ψ irect time s Euler angles from irect an passive complementary filters b est passive /s b time s 10 b 1 5 b b time s Bias estimation from irect an passive complementary filters manoeuvre, before returning to the initial heaing an laning the vehicle. After laning, the vehicle is place by han at its initial pose such that final an initial attitues are the ientical. Figure 8 plots the pitch an roll angles φ, θ estimate irectly from the accelerometer measurements against the estimate values from the explicit complementary filter. Note the large amounts of high frequency noise in the raw attitue estimates. The plots emonstrate that the filter is highly successful in reconstructing the pitch an roll estimates. Figure 9 presents the gyros bias estimation verses the preicte yaw angle φ base on open loop integration of the gyroscopes. Note that the explicit complementary filter here is base solely on estimation of the gravitational irection. Consequently, the yaw angle is the ineterminate angle that is not irectly stabilise in Corollary 5.. Figure 9 emonstrates that the propose filter has successfully ientifie the bias of the yaw axis gyro. The final error in yaw orientation of the microrone after laning is less than 5 egrees over a two minute flight. Much of this error woul be ue to the initial transient when the bias estimate was converging. Note that the b 1 b

13 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 13 secon part of the figure inicates that the bias estimates are not constant. Although some of this effect may be numerical, it is also to be expecte that the gyro bias on low cost IMU systems are highly susceptible to vibration effects an changes in temperature. Uner flight conitions changing engine spees an aeroynamic conitions can cause quite fast changes in gyro bias. into the estimator frame of reference. The resulting observer kinematics are consierably simplifie an avoi coupling of constructe attitue error into the preictive velocity upate. Explicit complementary filter: A reformulation of the passive complementary filter in terms of irect vectorial measurements, such as gravitational or magnetic fiel irections obtaine for an IMU. This observer oes not require online algebraic reconstruction of attitue an is ieally suite for implementation on embee harware platforms. Moreover, the filter remains well conitione in the case where only a single vector irection is measure. The performance of the observers was emonstrate in a suite of experiments. The explicit complementary filter is now implemente as the primary attitue estimation system on several MAV vehicles worl wie. APPENDIX A A REVIEW OF COMPLEMENTARY FILTERING Fig. 8. φ eg b r/s Fig Estimation results of the Pitch an roll angles. φ yaw angle from gyros φ from the estimator time s time s Gyros bias estimation an influence of the observer on yaw angle. VII. CONCLUSION This paper presents a general analysis of attitue observer esign pose irectly on the special orthogonal group. Three non-linear observers, ensuring almost global stability of the observer error, are propose: Direct complementary filter: A non-linear observer pose on SO3 that is relate to previously publishe non-linear observers erive using the quaternion representation of SO3. Passive complementary filter: A non-linear filter equation that takes avantage of the symmetry of SO3 to avoi transformation of the preictive angular velocity term b x b y b z Complementary filters provie a means to fuse multiple inepenent noisy measurements of the same signal that have complementary spectral characteristics [11]. For example, consier two measurements y 1 = x+µ 1 an y = x+µ of a signal x where µ 1 is preominantly high frequency noise an µ is a preominantly low frequency isturbance. Choosing a pair of complementary transfer functions F 1 s + F s = 1 with F 1 s low pass an F s high pass, the filtere estimate is given by ˆXs = F 1 sy 1 +F sy = Xs+F 1 sµ 1 s+f sµ s. The signal Xs is all pass in the filter output while noise components are high an low pass filtere as esire. This type of filter is also known as istorsionless filtering since the signal xt is not istorte by the filter [43]. Complementary filters are particularly well suite to fusing low banwith position measurements with high ban with rate measurements for first orer kinematic systems. Consier the linear kinematics with typical measurement characteristics ẋ = u. 41 y x = Lsx + µ x, y u = u + µ u + bt 4 where Ls is low pass filter associate with sensor characteristics, µ represents noise in both measurements an bt is a eterministic perturbation that is ominate by low-frequency content. Normally the low pass filter Ls 1 over the frequency range on which the measurement y x is of interest. The rate measurement is integrate y u s to obtain an estimate of the state an the noise an bias characteristics of the integrate signal are ominantly low frequency effects. Choosing F 1 s = Cs Cs + s F s = 1 F 1 s = s Cs + s

14 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 14 with Cs all pass such that LsF 1 s 1 over the banwith of Ls. Then ˆXs Xs + F 1 sµ x s + µ us + bs Cs + s Note that even though F s is high pass the noise µ u s+bs is low pass filtere. In practice, the filter structure is implemente by exploiting the complementary sensitivity structure of a linear feeback system subject to loa isturbance. Consier the block iagram in Figure 10. The output ˆx can Fig. 10. y x + Cs - + y u + R ˆx Block iagram of a classical complementary filter. be written ˆxs = Cs s + Cs y s y u s xs + Cs + s s = T sy x s + Ss y us s where Ss is the sensitivity function of the close-loop system an T s is the complementary sensitivity. This architecture is easy to implement efficiently an allows one to use classical control esign techniques for Cs in the filter esign. The simplest choice is a proportional feeback Cs = k P. In this case the close-loop ynamics of the filter are given by ˆx = y u + k P y x ˆx. 43 The frequency omain complementary filters associate with this choice are F 1 s = k P s+k P an F s = s s+k P. Note that the crossover frequency for the filter is at k P ra.s 1. The gain k P is typically chosen base on the low pass characteristics of y x an the low frequency noise characteristics of y u to choose the best crossover frequency to traeoff between the two measurements. If the rate measurement bias, bt = b 0, is a constant then it is natural to a an integrator to the compensator to make the system type I Cs = k P + k I s. 44 A type I system will reject the constant loa isturbance b 0 from the output. Gain esign for k P an k I is typically base on classical frequency esign methos. The non-linear evelopment in the boy of the paper requires a Lyapunov analysis of close-loop system Eq. 43. Applying the PI compensator, Eq. 44, one obtains state space filter with ynamics ˆx = y u ˆb + ky x ˆx, ˆb = k I y x ˆx The negative sign in the integrator state is introuce to inicate that the state ˆb will cancel the bias in y u. Consier the Lyapunov function L = 1 x ˆx + 1 k I b 0 ˆb Abusing notation for the noise processes, an using x = x ˆx, an b = b 0 ˆb, one has t L = k P x µ u x + µ x b k x In the absence of noise one may apply Lyapunov s irect metho to prove convergence of the state estimate. LaSalle s principal of invariance may be use to show that ˆb b 0. When the unerlying system is linear, then the linear form of the feeback an aaptation law ensure that the close-loop system is linear an stability implies exponential stability. APPENDIX B QUATERNION REPRESENTATIONS OF OBSERVERS The unit quaternion representation of rotations is commonly use for the realisation of algorithms on SO3 since it offers consierable efficiency in coe implementation. The set of quaternions is enote Q = {q = s, v R R 3 : q = 1}. The set Q is a group uner the operation [ ] s 1 s v1 T v q 1 q = s 1 v + s v 1 + v 1 v with ientity element 1 = 1, 0, 0, 0. The group of quaternions are homomorphic to SO3 via the map F : Q SO3, F q := I 3 + sv + v This map is a two to one mapping of Q onto SO3 with kernel {1, 0, 0, 0, 1, 0, 0, 0}, thus, Q is locally isomorphic to SO3 via F. Given R SO3 such that R = expθa then F 1 R = {±cos θ, sin θ a} Let Ω {A} enote a boy-fixe frame velocity, then the pure quaternion pω = 0, Ω is associate with a quaternion velocity. Consier the rotation kinematics on SO3 Eq. 4, then the associate quaternion kinematics are given by q = 1 q pω 45 Let q y q be a low frequency measure of q, an Ω y Ω + b for constant bias b be the angular velocity measure. Let ˆq enote the observer estimate an quaternion error q [ ] s q = ˆq 1 q = ṽ Note that sṽ = cosθ/ sinθ/a = 1 sin θa = vexp a R where θ, a is the angle axis representation of R = F q. The quaternion representations of the observers propose in this paper are: Direct complementary filter Eq. 1: ˆq = 1 ˆq p RΩ y ˆb + k P sṽ ˆb = k I sṽ 46a 46b

15 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 15 Passive complementary filter Eq. 13: ˆq = 1 ˆq pω y ˆb + k P sṽ ˆb = k I sṽ Explicit complementary filter Eq. 3: n k i ω mes = vex v i ˆv T i ˆv i vi T i=1 ˆq = 1 ˆq pω y ˆb + k P ω mes ˆb = k I ω mes 47a 47b 48a 48b 48c The error ynamics associate with the irect filter expresse in the quaternion formulation are q = 1 p b + k P sṽ q. 49 The error ynamics associate with the passive filter are q = 1 q pω pω q p b + k P sṽ q. 50 There is a fifteen year history of using the quaternion representation an Lyapunov esign methoology for filtering on SO3 for example cf. [9], [30], [3]. To the authors knowlege the Lyapunov analysis in all cases has been base aroun the cost function Φ q = s 1 + ṽ. Due to the unit norm conition it is straightforwar to show that Φ q = 1 s = 1 cosθ/ The cost function propose in this paper is E tr = 1 cosθ Eq. 3. It is straightforwar to see that the quaratic approximation of both cost functions aroun the point θ = 0 is the quaratic θ /. The quaternion cost function Φ, however, is non-ifferentiable at the point θ = ±π while the cost tri R has a smooth local maxima at this point. To the authors unerstaning, all quaternion filters in the publishe literature have a similar flavour that ates back to the seminal work of Salcuean [3]. The closest publishe work to that unertaken in the present paper was publishe by Thienel in her octoral issertation [44] an transactions paper [30]. The filter consiere by Thienel et al. is given by ˆq = 1 ˆq p RΩ y ˆb + k P sgn sṽ ˆb = k I sgn sṽ 51a 51b The sgn s term enters naturally in the filter esign from the ifferential, t s = sgn s t s, of the absolute value term in the cost function Φ, uring the Lyapunov esign process. Consier the observer obtaine by replacing sgn s in Eqn s 51 by s. Note that with this substitution, Eq. 51b is transforme into Eq. 46b. To show that Eq. 51a transforms to Eq. 46a it is sufficient to show that Rṽ = ṽ. This is straightforwar from s Rṽ = R sṽ = RvexP a R = vex RP a R R T = vexp a R = sṽ This emonstrates that the quaternion filter Eqn s 51 is obtaine from the stanar form of the complimentary filter propose Eq. 1 with the correction term Eq. 1c replace by ω q = sgn sṽ, q F 1 ˆR T R. Note that the correction term efine in Eq. 1c can be written ω = sṽ. It follows that ω q = sgn s ω s The correction term for the two filters varies only by the positive scaling factor sgn s/ s. The quaternion correction term ω q is not well efine for s = 0 where θ = ±π an these points are not well efine in the filter ynamics Eq. 51. It shoul be note, however, that ω q is boune at s = 0 an, apart from possible switching behaviour, the filter can still be implemente on the remainer of SO3 R 3. An argument for the use of the correction term ω q is that the resulting error ynamics strongly force the estimate away from the unstable set U cf. Eq. 14. An argument against its use is that, in practice, such situations will only occur ue to extreme transients that woul overwhelm the boune correction term ω q in any case, an cause the numerical implementation of the filter to eal with a iscontinuous argument. In practice, it is an issue of little significance since the filter will general work sufficiently well to avoi any issues with the unstable set U. For s 1, corresponing to θ = 0, the correction term ω q scales to a factor of 1/ the correction term ω. A simple scaling factor like this is compensate for the in choice of filter gains k P an k I an makes no ifference to the performance of the filter. REFERENCES [1] J. Vaganay, M. Alon, an A. Fournier, Mobile robot attitue estimation by fusion of inertial ata, in Proceeigns of the IEEE Internation Conference on Robotics an Automation ICRA, vol. 1, 1993, pp [] E. Foxlin, M. Harrington, an Y. Altshuler, Miniature 6-DOF inertial system for tracking HMD, in Proceeings of the SPIE, vol. 336, Orlano, Floria, 1998, pp [3] J. Balaram, Kinematic observers for articulate robers, in Proceings of the IEEE International Conference on Robotics an Automation, 000, pp [4] J. L. Marins, X. Yun, E. R. Backmann, R. B. McGhee, an M. Zya, An extene kalman filter for quaternion-base orientation estimation using marg sensors, in IEEE/RSJ International Conference on Intelligent Robots an Systems, 001, pp [5] E. Lefferts, F. Markley, an M. Shuster, Kalman filtering for spacecraft attitue estimation, AIAA Journal of Guiance, Control an Navigation, vol. 5, no. 5, pp , September 198. [6] B. Barshan an H. Durrant-Whyte, Inertial navigation systems for mobile robots, IEEE Transactions on Robotics an Automation, vol. 44, no. 4, pp , [7] M. Zimmerman an W. Sulzer, High banwith orientation measurement an control ase on complementary filtering, in Proceeings of Symposium on Roboitcs an Control, SYROCO, Vienna, Austria, [8] A.-J. Baervelt an R. Klang, A low-cost an low-weight attitue estimation system for an autonomous helicopter, Intelligent Engineering Systems, [9] B. Vik an T. Fossen, A nonlinear observer for gps an ins integration, in Proceeings of teh IEEE Conference on Decisioin an Control, Orlano, Floria, USA, December 001, pp [10] H. Rehbiner an X. Hu, Nonlinear state estimation for rigi boy motion with low-pass sensors, Systems an Control Letters, vol. 40, no. 3, pp , 000.

16 IEEE TRANSACTIONS ON AUTOMATIC CONTROL, VOL. XX, NO. XX, MONTH YEAR 16 [11] E. R. Bachmann, J. L. Marins, M. J. Zya, R. B. Mcghee, an X. Yun, An extene kalman filter for quaternion-base orientation estimation using MARG sensors, 001. [1] J.-M. Pflimlin, T. Hamel, P. Soueeres, an N. Metni, Nonlinear attitue an gyroscoples bias estimation for a VTOL UAV, in Proceeings of the IFAC worl congress, 005. [13] H. Rehbiner an X. Hu, Drift-free attitue estimation for accelerate rigi boies, Automatica, 004. [14] N. Metni, J.-M. Pflimlin, T. Hamel, an P. Soueeres, Attitue an gyro bias estimation for a flying UAV, in IEEE/RSJ International Conference on Intelligent Robots an Systems, August 005, pp [15], Attitue an gyro bias estimation for a VTOL UAV, in Control Engineering Practice, 006, p. to appear. [16] J. Lobo an J. Dias, Vision an inertial sensor cooperation using gravity as a vertical reference, IEEE Transactions on Pattern Analysis an Machine Intelligence, vol. 5, no. 1, pp , Dec [17] H. Rehbiner an B. Ghosh, Pose estimation using line-base ynamic vision an inertial sensors, IEEE Transactions on Automatic Control, vol. 48, no., pp , Feb [18] J.-H. Kim an S. Sukkarieh, Airborne simultaneous localisation an map builing, in Proceeings of the IEEE International Conference on Robotics an Automation, Taipei, Taiwan, September 003, pp [19] P. Corke, J. Dias, M. Vincze, an J. Lobo, Integration of vision an inertial sensors, in Proceeings of the IEEE International Conference on Robotics an Automation, ICRA 04., ser. W-M04, Barcellona, Spain, April 004, full ay Workshop. [0] R. Phillips an G. Schmit, System Implications an Innovative Applications of Satellite Navigation, ser. AGARD Lecture Series 07. help@sti.nasa.gov: NASA Center for Aerospace Information, 1996, vol. 07, ch. GPS/INS Integration, pp [1] D. Gebre-Egziabher, R. Haywar, an J. Powell, Design of multi-sensor attitue etermination systems, IEEE Transactions on Aerospace an Electronic Systems, vol. 40, no., pp , April 004. [] J. L. Crassiis, F. L. Markley, an Y. Cheng, Nonlinear attitue filtering methos, Journal of Guiance, Control,an Dynamics, vol. 30, no. 1, pp. 1 8, January 007. [3] M. Jun, S. Roumeliotis, an G. Sukhatme, State estimation of an autonomous helicopter using Kalman filtering, in Proc IEEE/RSJ International Conference on Robots an Systems IROS 99, [4] G. S. Sukhatme an S. I. Roumeliotis, State estimation via sensor moeling for helicopter control using an inirect kalman filter, [5] G. S. Sukhatme, G. Buskey, J. M. Roberts, P. I. Corke, an S. Saripalli, A tale of two helicopters, in IEEE/RSJ, International Robots an Systems,, Los Vegas, USA, Oct. 003, pp , srik/papers/iros003.pf. [6] J. M. Roberts, P. I. Corke, an G. Buskey, Low-cost flight control system for small autonomous helicopter, in Australian Conference on Robotics an Automation, Aucklan, 7-9 Novembre, 00, pp [7] P. Corke, An inertial an visual sensing system for a small autonomous helicopter, J. Robotic Systems, vol. 1, no., pp , February 004. [8] G. Creamer, Spacecraft attitue etermination using gyros an quaternion measurements, The Journal of Astronautical Sciences, vol. 44, no. 3, pp , July [9] D. Bayar, Fast observers for spacecraft pointing control, in Proceeings of the IEEE Conference on Decision an Control, Tampa, Floria, USA, 1998, pp [30] J. Thienel an R. M. Sanner, A couple nonlinear spacecraft attitue controller an observer with an unknow constant gyro bias an gyro noise, IEEE Transactions on Automatic Control, vol. 48, no. 11, pp , Nov [31] G.-F. Ma an X.-Y. Jiang, Spacecraft attitue estimation from vector measurements using particle filter, in Proceeings of the fourth Internation conference on Machine Learning an Cybernetics, Guangzhou, China, August 005, pp [3] S. Salcuean, A globally convergent angular velocity observer for rigi boy motion, IEEE Trans. Auto. Cont., vol. 36, no. 1, pp , Dec [33] O. Eglan an J. Gohaven, Passivity-base aaptive attitue control of a rigi spacecraft, IEEE Transactions on Automatic Control, vol. 39, pp , April [34] A. Tayebi an S. McGilvray, Attitue stabilization of a four-rotor aerial robot: Theory an experiments, To appear in IEEE Transactions on Control Systems Technology, 006. [35] R. Mahony, T. Hamel, an J.-M. Pflimlin, Complimentary filter esign on the special orthogonal group SO3, in Proceeings of the IEEE Conference on Decision an Control, CDC05. Seville, Spain: Institue of Electrical an Electronic Engineers, December 005. [36] T. Hamel an R. Mahony, Attitue estimation on SO3 base on irect inertial measurements, in International Conference on Robotics an Automation, ICRA006, 006. [37] S. Bonnabel an P. Rouchon, Control an Observer Design for Nonlinear Finite an Infinite Dimensional Systems, ser. Lecture Notes in Control an Information Sciences. Springer-Verlag, 005, vol. 3, ch. On Invariant Observers, pp [38] S. Bonnabel, P. Martin, an P. Rouchon, A non-linear symmetrypreserving observer for velocity-aie inertial navigation, in American Control Conference, Proceeings of the, June 006, pp [39] R. Murray, Z. Li, an S. Sastry, A mathematical introuction to robotic manipulation. CRC Press, [40] H. Khalil, Nonlinear Systems n eition. New Jersey: Prentice Hall, [41] L. Lipera, J. Colbourne, M. Tischler, M. Mansur, M. Rotkowitz, an P. Patangui, The micro craft istar micro-air vehicle: Control system esign an testing, in Proc. of the 57th Annual Forum of the American Helicopter Society, Washington DC, USA, May 001, pp [4] J. Fleming, T. Jones, P. Gelhausen, an D. Enns, Improving control system effectiveness for ucte fan vtol uavs operating in crosswins, in Proc. of the n Unmanne Unlimite System. San Diego, USA: AIAA, September 003. [43] R. G. Brown an P. Y. C. Hwang, Introuction to Ranom Signals an Applie Kalman Filtering, n e. New York, NY: John Wiley an Sons, 199. [44] J. Thienel, Nonlinear observer/controller signs for spacecraft attitue control systems with uncalibrate gyros, PhD, Faculty of the Grauate School of the University of Marylan, Dep. Aerospace Engineering,, 004. Robert Mahony is currently a reaer in the Department of Engineering at the Australian National University. He receive a PhD in 1995 systems engineering an a BSc in 1989 applie mathematics an geology both from the Australian National University. He worke as a marine seismic geophysicist an an inustrial research scientist before completing a two year postoctoral fellowship in France an a two year Logan Fellowship at Monash University in Australia. He has hel his post at ANU since 001. His research interests are in non-linear control theory with applications in robotics, geometric optimisation techniques an learning theory. Tarek Hamel receive his Bachelor of Engineering from the University of Annaba, Algeria, in He receive his PhD in Robotics in 1995 from the University of technology Compiègne UTC, France. After two years as a research assistant at the UTC, he joine the Centre Etues e Mècanique Iles e France in 1997 as an associate professor. Since 003, he has been a Professor at the I3S UNSA- CNRS laboratory of the University of Nice-Sophia Antipolis, France. His research interests inclue nonlinear control theory, estimation an vision-base control with applications to Unmanne Aerial Vehicles an Mobile Robots. Jean-Michel Pflimlin grauate from Supaero, the French Engineering school in Aeronautics an Space in 003. He conucte his Ph.D. research at the Laboratory for Analysis an Architecture of Systems, LAAS-CNRS, in Toulouse, France an receive the PhD egree in automatic control from Supaero in 006. He is currently working at Navigation epartment of Dassault Aviation in Saint Clou, Paris. His research interests inclue nonlinear control an filtering, avance navigation systems an their applications to Unmanne Aerial Vehicles.