Nonlinear Filter Design for Pose and IMU Bias Estimation

Glauco Garcia Scandaroli, Pascal Morin.

INRIA Sophia Antipolis-Méditerranée2004 route des Lucioles, Sophia Antipolis 06902, France.

Glauco.Scandaroli@inria.fr, Pascal.Morin@inria.fr

Abstract— This paper concerns the problem of position andattitude estimation based on the fusion of complementarysensors. Nonlinear observers are proposed in order to obtainhigh-quality and high-rate estimates from raw position/attitudedata, and measurements obtained from an inertial measurementunit. Estimates are improved through the online identificationof diverse measurement biases, and a novel method for con-current position estimation and accelerometer bias estimationis presented. Almost global asymptotic convergence of thepose observer is proved based on a Lyapunov-like approach,and several implementation issues are addressed. Comparisonwith classical existing methods illustrate the relevance of theproposed approach.

Index Terms— Inertial Estimation, Nonlinear Observers,Lyapunov Function.

I. I NTRODUCTION

This work deals with pose (i.e. position and attitude)estimation via the fusion of sensory measurements. Givenpose and inertial measurements, the objective is to obtainestimates that best exploit the different measurements charac-teristics, while coping with measurements biases. This issueis fundamental in ground or aerial robotic applications, wherea precise knowledge of the robot location is often required.In general, high frequency measurements, typically from50 Hz up to 1 kHz, of rotational velocity and specific trans-lational acceleration are provided by an inertial measure-ment unit (IMU). As a result of micro-electro-mechanicalsensors (MEMS) manufacturing characteristics, IMU mea-surements are corrupted by additive noise and offset, alsoknown as measurement bias. Therefore, the solution of suchproblem using solely IMU data integration drifts after a fewseconds. The reduction of such drift can be achieved aftera good calibration of sensor’s bias, still, the time invariantmodel for bias is only an approximation, additionally it doesnot consider effects from temperature variation. Therefore,the consideration of an online method for bias estimationis indeed positive. Other sensors, that provide explicit orimplicit attitude and position measurements, must be em-ployed in order to cope with the calibration, and boundthe resulting drift from IMU data integration. Examples aregiven by the global positioning system (GPS) and/or visualsensors. These sensors can provide a fair measurement ofposition, however at low frequencies, typically from5 Hzup to 25 Hz. GPS provides position with respect to earth,or to a ground fixed station for differential GPS. However,

G. G. Scandaroli is funded by grants from theConseil Régional Provence-Alpes-Côte d’Azur, and theDGCISRapace project leaded by Geocean.

this system needs a direct sight to the satellites (or to thestations), therefore it is not suitable for several types ofenvironments. Visual systems are able to relate the rotationand translational displacement from two different views, thusproviding incremental measurements.

Since early works on attitude estimation, the extendedKalman filter (EKF) is the incipient solution to this esti-mation problem [1]. However, EKF provides no assuranceof convergence, which can be seen from its similarity to thefirst step of Gauss-Newton optimization method [2]. OtherKalman-based solutions have been proposed to overcome thenonlinear nature of the problem [3], [4]. Despite the factthat these methods improve significantly the propagation ofthe probability distribution, up to the authors knowledge,the achievement ofalmost global convergence using suchmethods remains to be achieved. To overcome such nonlinearpropagation of the distribution the use of uncertainty ellip-soids [5] have been proposed in the literature. Additionally,the problem can be rewritten as estimation of two non-collinear unconstrained vectors instead of the attitude matrixor some parametrization, hence becoming a linear time-varying system estimation problem [6].

In the past years, some effort has been put in the devel-opment of nonlinear observers for attitude, attitude-headingreference (AHR) and pose estimation. Nonlinear observersare application-specific estimators that take advantage ofstructural properties of the models. Although some technicaldifferences can be noticed between different nonlinear ob-servers, this class of estimators commonly share the benefitsof global, or at least semi-global, stability proofs. Some state-of-the-art results are Lyapunov-function based observers[7]–[9], complementary filters forSO(3) [10], and invariant-observers [11], [12]. Concerning online bias estimation, anAHR estimator that accomplishes gyroscope and (partial)accelerometer bias estimation is presented in [12].

In order to solve the full pose estimation using nonlinearobservers, the decoupling of the attitude and translationaldisplacement is often considered. An observer for attitudeand position using inertial data and visual line features, ispresented in [13], despite the fact that biases are consideredfor the presented simulation results, there is no procedurefor bias estimation in the observer. The estimation usingthe SE(3) representation by means of IMU and bearingmeasurements is addressed in [14], but online IMU biasestimation is not performed. A cascaded nonlinear observerfor attitude and position is presented in [15] exploiting

IMU measurements together with GPS measurements. Ex-ponential convergence for attitude and position is achieved,together with gyroscope bias estimation. Chevironet. al [16]presents a solution for full pose estimation, however due tomisconceived hypothesis on the accelerometer measurementdefinition the convergence proof for position estimation isonly valid for small angular velocities.

This paper presents the development of a nonlinear ob-server that employs the passive complementary filter forattitude estimation, and a novel method for concurrent posi-tion and accelerometer bias estimation. While the presentapproach bears resemblance with [16], in the sense thata position and accelerometer bias observer is presented,the observer form is different due to the definition of thesystem. This article also presents a global exponential sta-bility proof for the position and accelerometer bias observerindependently of the angular velocity, andalmost globalasymptotic stability for the nonlinear pose and IMU biasobserver. Recall that, due to a topological obstruction, globalstability on SO(3) cannot be achieved. A method for gaintuning based on settling time estimates is also presented.In addition to the guarantee ofalmost global convergence,the implementation of the proposed method is simpler thanKalman-based estimators. Simulation results illustrate theperformance of the method, and a comparison with theEKF is presented. While the simulations are developed usinginertial and visual data, the proposed method can be usedwith any measurement system that is able to recover attitudeand position.

II. BACKGROUND

A. Mathematical Notation and Identities

The special orthogonal group is denoted asSO(3). Itsassociated Lie algebra is the set of anti-symmetric matri-ces denoted asso(3). For the sake of design and anal-ysis, the following properties and definitions are recalledfrom [10]. The cross-product can be represented by theproductS(u)v = u× v, ∀ u, v ∈ R

3, whereS(·) ∈ so(3).The inverse of theS(·) operator is denotedvex(·), i.e.,vex(S(u)) = u, u ∈ R

3, S(vex(A)) = A, A ∈ so(3).With A ∈ R

3×3, the symmetric and anti-symmetric opera-tors are defined asPs(A) = A+AT

2, Pa(A) = A−AT

2. The

following properties hold:

S(Ru) = RS(u)RT , u ∈ R3, R ∈ SO(3),

R vex(Pa(R)

)= vex

(Pa(R)

), R ∈ SO(3) .

(1)

At a few places in this paper, parametrizations ofSO(3)will be used, and willing further information the interestedreader is conducted to [17]. Consider any parametrizationΘ such thatR ≈ I3 + S(Θ) at first order around the3 × 3identity matrixI3. Examples are given byΘ = [φ, θ, ψ]T , thevector of roll, pitch and yaw Euler angles, orΘ = 2qv withqv the vector part of the unitary quaternion representation.From the general rotation dynamicsd

dtR = RS(u), it follows

that aroundR = I3,

ddt

Θ ≈ u, vex(Pa(R)

)≈ Θ (2)

B. System description

Denote by{I} an inertial frame defined as appropriate.Coordinates of a vector expressed in this frame are writtenwith the subscriptI. Similarly,{B} denotes a frame attachedto a target rigid body, and coordinates of a vector expressedin this frame are written with the subscriptB. The attitude isdefined as the rotation matrixR ∈ SO(3) : {B} → {I}, andthe current positionpI

B∈ R

3 as the translational displace-ment of{B} in {I} coordinates. Throughout the text,pI

Bis

referred asp for only the displacement of{B} in inertialcoordinates is considered. The dynamics for the systemcomprising attitude and translational displacement writes

ddtR = RS(ωB), d

dtp = v, d

dtv = aI = RaB, (3)

wherev ∈ R3 is velocity of the body in inertial coordinates;

ωB ∈ R3, aB ∈ R

3 are the rotational velocity and transla-tional acceleration in body coordinates, andaI ∈ R

3 is thetranslational acceleration represented in inertial coordinates.

Assumption 1 There exist three positive constantsca, cv,and cω such that∀t ∈ [0,∞): |ωB(t)| ≤ cω,|aB(t)| ≤ca,|v(t)| ≤ cv.

This assumption indicates that the body rotational velocityis bounded, also the body acceleration and its first ordertime-derivative are bounded. This technical assumption isnotrestrictive for applications, all the more that the upper boundsca, andcv are not used in the observer’s design.

For the sake of rotational velocity and translational accel-eration measurement, an IMU consisting of rate gyroscopesand accelerometers is employed, and without loss of gener-ality it is assumed that the IMU reference frame and{B}coincide. Gyroscopes measure the angular velocityωB, andaccelerometers measure thespecific translational accelera-tion, which is the expression of the body’s acceleration minusthe gravity field in body coordinates. Therefore, effects arisenfrom the real body accelerationaB and earth’s gravitationalfield are measured as if they were the same force.

Due to manufacturing characteristics of MEMS, the mea-surement is modeled as being disturbed by an offset measure-ment, also called bias. The considered measurement modelis often employed

ω = ωB + ωb,ddtωb = 0, (4)

a = RT(

ddtv − gI

)+ ab,

ddtab = 0, (5)

where ω, a denote the rate gyroscope and accelerometermeasurements;ωb, ab denote gyroscope, accelerometer bias,respectively; andgI is the gravitational acceleration fieldrepresented in inertial coordinates. For the previous models,it is important to notice that other sensor characteristicsareneglected, such as limited bandwidth and bias variation withrespect to temperature and time. Moreover, MEMS measure-ments are corrupted by an additive measurement noise. Theseeffects are neglected for the observer design. However, thesenoises are indeed considered for the presented simulations.

The introduced solution assumes position and attitudemeasurements/estimates. Many methods can be used. For

example, position can be directly measured by a GPS.Attitude is usually more difficult to obtain and its estimationis still a research topic, especially for IMU-magnetometerbased solutions [9], [12]. An interesting alternative is vision,since efficient methods are now available to estimate relativedisplacements in both position and orientation from visualdata (see, e.g. [18]). In this work, a camera observing aknown target is used to obtain the complete pose.

III. O BSERVER DESIGN

In the previous section, the dynamics of the rigid body’spose and the available measurements were defined. In acomplete form, considering the actual IMU measurement andEqs. (3), (4), and (5), then the dynamics of attitude, position,and sensor bias is given by

{ddtR =RS(ω − ωb),

ddtωb= 0,

ddtp = v, d

dtv = gI +R(a− ab),

ddtab = 0.

(6)

The following observers are defined:

ddtR=RS(ω − ωb + αR), d

dtωb=αω, (7)

for the attitude, and

ddtp = v + αp,

ddtv=gI +R(a− ab) + αv,

ddtab = αa, (8)

for position, whereαR, αω, αp, αv, αa are innovation termsdefined further on. The estimation errors are defined

R , RRT , ωb , ωb − ωb, (9)

p , p− p, v , v − v, ab , ab − ab, (10)

then, the estimation error dynamics yields

ddtR =RS(−Rωb − RαR), d

dtωb= −αω, (11)

ddtp = v − αp,

ddtv = −Rab − αv,

ddtab = −αa. (12)

The objective of the nonlinear observer’s design is to defineαR, αω, αp, αv, andαa so that(R,ωb,p,v,ab)= (I3,0, 0, 0, 0)is an asymptotically stable equilibrium of this estimationerror dynamics.

Nonlinear observer solutions with semi-global stabilityhave already been proposed in the literature for attitude andonline gyroscope bias estimation. One of them is the passivecomplementary filter onSO(3) [10].

Lemma 1 (Passive complementary filter onSO(3)) Let

αR = k1RTvex

(Pa(R)

), αω= − k2R

Tvex

(Pa(R)

)(13)

with k1, k2 > 0. Then, for the attitude estimation errordynamics(11), the following statements hold:1) All solutions converge toEs ∪ Eu with Es = (I3, 0),

Eu ={

(R, ωb)∣∣ tr

(R

)= −1

}.

2) The equilibrium point(R, ωb) = (I3, 0) is locally expo-nentially stable.

Remark that observer (7), (13) is the same as the onein [10], despite the different definition ofR. This observeris endowed with additional stability properties (see [10] forthe complete statement).

The following result concerns the problem of position, andonline accelerometer bias estimation. This is the main resultof this section and complements Lemma 1.

Theorem 1 (Position and accelerometer bias observer)Let

αp= k3p, αv= k4p, αa= −k5(I3+1

k3

S(ωB))RT p (14)

with k3, k4, k5 > 0 such thatk5 < k3k4. Then,(p, v, ab) =(0, 0, 0) is a globally exponentially stable equilibrium pointof the position estimation error dynamics(12).

Proof. Appendix A.

This theorem provides a globally exponentially stableobserver for the position, velocity and accelerometer biasestimation problem, independently of the rotational dynam-ics. Note that the stability conditions on the gain parametersare necessary. For example, whenωB = 0, the dynamics ofthe estimation error is linear and autonomous. Therefore,the gain conditions in Theorem 1 correspond exactly to thestability conditions of this linear system.

It is implicitly assumed in (14) thatωB = ω−ωb is directlyavailable to measurements/estimation. In practice, this termshould be replaced by the estimateω − ωb, with ωb beingthe output of the attitude observer (7). The following result,derived from Theorem 1, shows that this can be done withoutconsequence on the convergence of the observer.

Corollary 1 Let

αp= k3p, αv= k4p, αa= −k5(I3+1

k3

S(ω−ωb))RT p (15)

with k3, k4, k5 > 0 such thatk5 < k3k4.If ωb converges asymptotically to zero, then(p, v, ab) con-verges asymptotically to zero along the solutions of theposition estimation error dynamics(12).

Proof. Appendix B.

Finally, the measured rotation matrixR in (12) and (14)can be replaced by its estimateR. However, global asymp-totic convergence cannot be achieved anymore, since theobserver onSO(3) is not globally asymptotically stable.

Corollary 2 (Nonlinear pose observer)Let{

ddtR = RS(ω − ωb + αR),

ddtωb = αω ,

(16)

ddtp = v + αp,

ddtv = gI + R(a− ab) + αv,

ddtab = αa,

(17)

with αR, αω defined by(13), and

αp = k3p, αv = k4p, αa = −k5(I3+1

k3

S(ω−ωb))RT p (18)

Assume thatk1, · · · , k5 > 0 and k5 < k3k4. Then,1) The origin (R, ωb, p, v, ab) = (I3, 0, 0, 0, 0) is a locallyexponentially stable equilibrium of the estimation error dy-namics.

2) If R converges asymptotically toI3, then (ωb, p, v, ab)converges asymptotically to zero.

Proof. Appendix C.

IV. I MPLEMENTATION ISSUES

While stability of the estimation error is a prerequisite, agood tuning of the innovation gains is also a relevant topicto ensure good response to estimation errors, and respectthe sensors characteristics. In IMU-based pose estimationofrobotic systems, two dynamics are distinguished: fast dynam-ics for pose variables and their derivatives, slow dynamicsof the gyroscope and accelerometer bias. In this section, wepropose a gain tuning strategy for the proposed observer thathas a direct interpretation in terms of time-response.

First, consider the attitude observer. Letτ1, τ2 denotetwo settling times withτ1 ≪ τ2. The parameterτ1 denotesa desired settling time for the dynamics of the attitudeestimation error dynamics. A typical value (depending ofcourse of the measurements characteristics) isτ1 ≤ 1. As forτ2, it denotes a desired settling time for the gyroscope biasestimation error. Since this bias varies slowly, a relativelylarge value ofτ2 can be considered (e.g.τ2 ≈ 10, or evenlarger). The gainsk1 and k2 for the attitude estimator aredefined as follows:

k1 = 3 τ1+τ2

τ1τ2

, k2 = 9 1

τ1τ2

. (19)

The rationale for this choice is the following. The errorsystem for the attitude estimator can also be written as:

{ddtR = − S(Rωb)R− k1RPa

(R

),

ddtωb = k2R

Tvex

(Pa

(R

)).

(20)

In order to simplify the analysis, the gyroscope bias error isexpressed in{I}, i.e. ωIb= Rωb, using (1) and (20) yields

ddtωIb = d

dt(R)ωb + k2vex

(Pa(R)

).

Consider a hover flight situation, i.e.ddtR ≈ 0, ωB ≈ 0.

DefiningΘ as any parametrizationR ≈I3+S(Θ) around theidentity matrix, from (2), (20), the linearized dynamics ofthe attitude estimation error dynamics around the equilibriumpoint Θ = 0 yields

d

dt

[ΘωIb

]=

[−k1I3 −I3k2I3 0

] [ΘωIb

].

This system can be decomposed into three independentsecond-order linear systems:

d

dt

[Θi

ωi

]=

[−k1 −1k2 0

] [Θi

ωi

], (i = 1, 2, 3), (21)

and the characteristic polynomial of these systems is

P (s) = s2 + k1s+ k2 = s2 + 3 τ1+τ2

τ1τ2s+ 9

τ1τ2

=(s+ 3

τ1

)(s+ 3

τ2

).

Hence, the gain choice (19) yields two real eigenval-ues λ1 = − 3

τ1

, λ2 = − 3

τ2

. Furthermore, using the variablechange (assumingτ1 6= τ2)

[x1(t)x2(t)

]=

[λ1 1λ2 1

] [Θi(t)ωi(t)

], (22)

then Eq. (21) writes

d

dt

[x1

x2

]=

[λ1 00 λ2

] [x1

x2

]. (23)

By using (22) and (23), it is not difficult to obtain thefollowing expression for the solutions of System (21):

Θi(t) = eλ1tΘi(0) +f12(t)

λ1 − λ2

ωi(0) +λ2f12(t)

λ1 − λ2

Θi(0)

ωi(t) = eλ2tωi(0) +λ1λ2f12(t)

λ1 − λ2

Θi(0) +λ2f12(t)

λ1 − λ2

ωi(0)

with f12(t) = eλ1t − eλ2t. Therefore the following (partial)dynamics decoupling is obtained:

• Fast exponential decrease ofΘi(t) to zero, corrupted byslowly decreasing terms with small amplitude:1

λ1−λ2

and λ2

λ1−λ2tend to zero asτ1 → 0 andτ2 → ∞.

• Slow exponential decrease ofωi(t) to zero corrupted byslowly decreasing terms with small amplitude.

The same rationale leads to the following definition of thepose estimation observer gains:

k3 = 3 τ3τ4+τ3τ5+τ4τ5

τ3τ4τ5

, k4 = 9 τ3+τ4+τ5

τ3τ4τ5

, k5 = 27

τ3τ4τ5

. (24)

with τ3, τ4, andτ5 different settling times. These gains satisfythe stability conditions of Theorem 1. Choosingτ3, τ4 ≪ τ5leads to the same (partial) decoupling of the dynamics ofp, v on one hand, andab on the other hand.

V. RESULTS

Two simulations are carried out to illustrate the observer’sperformance and robustness with respect to measurementnoise and large initial errors. The first one concerns the evalu-ation of the nonlinear observer and gain tuning method whenfacing large initial errors, and the second one consists of aflight simulation with visual feedback. Both simulations wereperformed using rotational velocity and specific accelerationsampled at a frequency of100 Hz. The measurement noisesconsidered for the inertial sensors are obtained from a xSensMTi-G unit, presenting variance of4 × 10−2 [rad]

2 and7 × 10−3 [m/s2]

2for rate gyroscopes and accelerometers

respectively. To present a measurement noise corrupted bynon-Gaussian source, attitude and position are reconstructedby a perspective camera sampling at10 Hz frequency. Thecamera is simulated with the following parameters

Kf =

1313 0 512

0 1313 3840 0 1

, RC

B =

1 0 00 −1 00 0 −1

, pCB =0,

with Kf being its calibration matrix,RC

Band pC

Bthe ro-

tation matrix and translational displacement{B} → {C},where{C} is the reference frame attached to the optical axisof the camera. The visual system is capable of tracking a

−80−60−40−20

0

φ θ ψ [ d e g ]

−1012

px py pz [m ]

−2024

vx vy vz [m / s ]

−0.15−0.1

−0.050

0.05

ωb x ωb y ωb z [ r a d / s ]

0 10 20 30 40 50 60 70−0.2

00.20.4

a b x a b y a b z [m / s 2]

t [ s ]

Fig. 1. Simulation for a body at rest. Rollφ, pitch θ and yawψ errors arepresented in blue, green, and red respectively. For the other variable p, v,ωb, and ab, the componentsx , y, z are presented in blue, red, and greenrespectively. Typical result from repeated simulations.

point, however its projection is corrupted by a Gaussian noisewith 1

9[pixel]2 variance for the width and height pixels.

To recover the rotation and the translational displacementbetween two views, thehomographybetween a referenceand the current image is recovered using thefour pointmethod. For further information on image formation and poserecovering from two different views the interested reader isconducted to [19]. The observer gains are computed fromthe aforementioned method given the following settling timesτ1=2 [s], τ2=15 [s], τ3=4 [s], τ4=4 [s], τ5=25 [s], yieldingthe gainsk1=1.7, k2=0.3, k3=1.62, k4=0.743, k5=0.068.The nonlinear observer presented in Corollary 2 is used.

A. Observer convergence from large initial errors

This simulation concerns the convergence of the pro-posed estimation for large initial errors. The body is atrest with R(t) = I3, p(t) = [0 0 1], and IMU biases arearound5 × 10−2 [rad] for gyroscopes and3 × 10−2 [m/s2]for accelerometers. The estimator is initialized with randomestimates obtained from zero-mean Gaussian distributions:R(0) = exp{0.999πS(u)}, whereu ∈ R

3 is a unitary-normrandom vector, andp(0), v(0), ωb(0), ab(0) are drawn fromzero-mean Gaussian distributions with3 [m]

2, 1 [m/s]2,

0.5 [rad]2 and 0.5 [m/s2]

2variance respectively. Fig. 1

depicts the evolution of estimation errors for a typical resultfrom repeated trials. From top to bottom, the results for theattitude error in Euler angles, position, velocity, gyroscopeand accelerometer bias errors are presented. Notice the con-vergence for all the estimates, and that defined settling timescorrespond to the dominant dynamics for each variable’sresponse. In order to better visualize the convergence of thebiases, Fig. 2 presents a closer view for the biases after thesettling time has passed. From top to bottom, the resultsfor gyroscope and accelerometer bias are presented. Thebiases are estimated with a good precision regarding themeasurement noises.

20 25 30 35 40 45 50

−4

−2

0

2x 10

−3 ωb x ωb y ωb z [ r a d / s ]

45 50 55 60 65 70 75 80 85 90 95 100

−5

0

5

x 10−3

a b x a b y a b z [m / s 2]

t [ s ]

Fig. 2. Closer view on extended time for the visualization ofbiases erroron steady state. Results for componentsx, y, z are presented in blue, red,and green respectively. Typical result from repeated simulations.

B. On–flight simulation

This simulation aims to evaluate the proposed algorithm ina situation that resembles a drone flight surveillance. Also,the results obtained using the proposed observer are com-pared to the response provided by the classical EKF writtenafter (6) with its parameters chosen as the noises of thesystem. The trajectory corresponds to a typical drone missionwith take-off, surveillance above a point of interest and land-ing. The camera observes two square targets, each consistingof four points, with sides40 [cm] and2.4 [m]. This way atleast one target can be distinguished during the take-off andlanding. The rotational movement is designed in order tokeep the image inside the camera’s sight. The homographymatrix is computed, and after its decomposition and a refer-ence frame change the current rotation and position are ob-tained. Time varying biases are used to simulate temperaturevariation, thus increasing the complexity of the simulation.From 0 [s] up to 50 [s], no bias is present. After50 [s] thebiases instantly change toωb = [0.035,−0.053, 0.018] [rad],and ab = [−0.3, 0.2,−0.1] [m/s2] and remain until100 [s]when they start to change with constant derivative reachingzero at 200 [s], and return with constant derivative until300 [s] when the values change instantly to their original zerovalue. For a better visualization of the simulation’s dynamics,the reader is directed to the attached video. The associ-ated simulation data are available athttp://www-sop.inria.fr/members/Glauco.Scandaroli.

A typical result obtained from repeated simulations ispresented in Fig. 3. The curves for measured, proposedalgorithm, and the EKF estimation errors are depicted ingray, solid blue, and dashed red, respectively. Estimationand measurement errors for the attitude are representedon the top left with Euler angles. Position estimation andmeasurement errors are depicted on the top right. Gyroscope,and accelerometer biases estimation errors are presented onthe bottom left and right. It is visually evident that theresulting noise for attitude and the position measurement arenot Gaussian. For the EKF, the attitude is estimated with agood precision until the first bias behavior switch at50 [s],then non-Gaussian noise and time-varying biases corruptthe estimates. Until the end of the simulation, the biasesare well estimated, however roll-pitch angles and estimatedposition start to present a biased behavior. Also, the yaw

−101

−505

−505

φ θ ψ [ d e g ]

0 50 100 150 200 250 300

00.020.040.06

t [ s ]

−0.08−0.04

0 0.04

−0.06−0.04−0.02

00.02

ωb x ωb y ωb z [ r a d / s ]−0.5

0

0.5

−1

0

1

−101

px py pz [m ]

0 50 100 150 200 250 300

−0.1

0

0.1

t [ s ]

−0.2−0.1

00.1

−0.50

0.51

a b x a b y a b z [m / s 2]

Fig. 3. Simulation for pose estimation during flight surveillance. Comparison of the visual measurement, nonlinear observer, and EKF. Results for visualmeasurements are presented in gray, nonlinear observer is depicted in solid blue, and EKF in dashed red. Typical result from repeated simulations.

angle does not correspond to the measurements. It could bestated that a2 [deg] estimation error is not relevant, how-ever considering that the measurements are given within a0.5 [deg] precision, this error is indeed relevant. Concerningthe proposed method, despite the presence of time-varyingbiases neglected in the observer’s design the resulting errorpresents a smaller variance than the measurement error. Alsoits precision is better than the EKF. Concerning the gaintuning of the EKF, one may argue thatinflatingmodel noisesmight eliminate its convergence problems. This argument isvalid, but it also supports the simplicity and effectiveness ofthe gain tuning method here proposed.

VI. CONCLUSION AND FUTURE WORK

This article presents a pose estimation method with onlinecalibration of IMU sensors. A nonlinear observer for concur-rent position estimation and accelerometer bias calibration isproposed, together with a global exponential stability proof.A procedure for gain tuning in terms of time-response isalso presented. Simulation results endorse the improvementdue to thealmostglobal stability properties of the nonlinearestimator against locally stable classical estimators. Futurework will consist on the evaluation of this approach usingreal visual-inertial data, and the conduction of a study to-wards a stochastic evaluation of the innovation gains relatingto the measurement noises.

ACKNOWLEDGMENTS

The authors would like to thank Rémi Desouche, EzioMalis, Gregory Chagnet and Daniele Pucci for the usefuldiscussions during the development of this project.

REFERENCES

[1] J. L. Crassidis, F. L. Markley, and Y. Cheng, “A survey of nonlinearattitude estimation methods,”AIAA Journal of Guidance, Control, andDynamics, vol. 30, pp. 12–28, 2007.

[2] B. Bell and F. Cathey, “The iterated Kalman filter update as a Gauss-Newton method,”IEEE Trans. on Automatic Control, vol. 38, pp. 294–297, 1993.

[3] J. Crassidis, “Sigma-point Kalman filtering for integrated GPS andinertial navigation,”IEEE Trans. on Aerospace and Electronic Systems,vol. 42, pp. 750–756, 2006.

[4] P. Vernaza and D. Lee, “Rao-Blackwellized particle filtering for 6DOFestimation of attitude and position via GPS and inertial sensors,” inIEEE Intl. Conf. on Robotics and Automation, 2006, pp. 1571–1578.

[5] A. K. Sanyal, T. Lee, M. Leok, and N. H. McClamroch, “Globaloptimal attitude estimation using uncertainty ellipsoids,” Systems &Control Letters, vol. 57, pp. 236–245, 2008.

[6] P. Batista, C. Silvestre, and P. Oliveira, “Sensor-based complementaryglobally asymptotically stable filters for attitude estimation,” in IEEEConf. on Decision and Control, 2009, pp. 7563–7568.

[7] B. Vik and T. Fossen, “A nonlinear observer for GPS and INSintegration,” inIEEE Conf. on Decision and Control, 2001, pp. 2956–2961.

[8] J. Thienel and R. Sanner, “A coupled nonlinear spacecraft attitudecontroller and observer with an unknown constant gyro bias and gyronoise,” IEEE Trans. on Automatic Control, vol. 48, pp. 2011–2015,2003.

[9] M.-D. Hua, “Attitude observers for accelerated rigid bodies based onGPS and INS measurements,” inIEEE Conf. on Decision and Control,2009, pp. 8071–8076.

[10] R. Mahony, T. Hamel, and J.-M. Pflimlin, “Nonlinear complementaryfilters on the special orthogonal group,”IEEE Trans. on AutomaticControl, vol. 53, pp. 1203–1218, 2008.

[11] S. Bonnabel, P. Martin, and P. Rouchon, “Symmetry-preserving ob-servers,”IEEE Trans. on Automatic Control, vol. 53, pp. 2514–2526,2008.

[12] P. Martin and E. Salaun, “An invariant observer for earth-velocity-aided attitude heading reference systems,” inIFAC World Conf., 2008,pp. 9857–9864.

[13] H. Rehbinder and B. Ghosh, “Pose estimation using line-based dy-namic vision and inertial sensors,”IEEE Trans. on Automatic Control,vol. 48, pp. 186–199, 2003.

[14] G. Baldwin, R. Mahony, and J. Trumpf, “A nonlinear observer for6 DOF pose estimation from inertial and bearing measurements,” inIEEE Intl. Conf. on Robotics and Automation, 2009, pp. 2237–2242.

[15] J. Vasconcelos, C. Silvestre, and P. Oliveira, “A nonlinear GPS/IMUbased observer for rigid body attitude and position estimation,” inIEEE Conf. on Decision and Control, 2008, pp. 1255–1260.

[16] T. Cheviron, T. Hamel, R. Mahony, and G. Baldwin, “Robust nonlinearfusion of inertial and visual data for position, velocity and attitudeestimation of UAV,” in IEEE Intl. Conf. on Robotics and Automation,2007, pp. 2010–2016.

[17] M. D. Shuster, “A survey of attitude representations,”The Journal ofthe Astronautical Sciences, vol. 41, pp. 439–517, 1993.

[18] S. Benhimane and E. Malis, “Homography-based 2D visualtrackingand servoing,”Intl. Journal of Robotics Research, vol. 26, pp. 661–676, 2007.

[19] Y. Ma, S. Soatto, J. Kosecka, and S. S. Sastry,An Invitation to 3-DVision: From Images to Geometric Models. SpringerVerlag, 2003.

APPENDIX

A. Proof of Theorem 1

From (12) and (14), pose estimation error dynamics yields

ddtp = v − k3p,

ddtv = −Rab − k4p,

ddtab = k5

(I3 + 1

k3

S(ωB))RT p.

(25)

Consider the following variable change

γ = Rab + k5

k3p, (26)

In the coordinates(p, v, γ), System (25) is thus given by

ddtp = v − k3p,

ddtv = − γ − (k4 − k5

k3)p,

ddtγ = S(RωB)γ + k5

k3

v.

(27)

Define the following candidate Lyapunov function:

Wt = 1

2

(k4 − k5

k3

)|p|2 + 1

2|v|2 + 1

2

k3

k5

|γ|2, (28)

and note thatWt is positive definite due to the assumptionk3, k4, k5> 0 andk5 <k3k4. Along the solutions of (27),

ddtWt =

(k4 − k5

k3

)pT (v − k3p) − vT

(γ +

(k4 − k5

k3

)p)

+ k3

k5γT

(S(RωB)γ + k5

k3v),

= − k3

(k4 − k5

k3

)|p|2 ≤ 0 (29)

By using Barbalat’s Lemma, and Assumption 1, it is notvery difficult from here to prove that(p, v, γ) = (0, 0, 0) isa globally asymptotically stable equilibrium point. However,in order to establishexponentialstability, it is proceededdifferently by modifyingWt so as to obtain a strict Lyapunovfunction. LetW = Wt − ǫvT (p − δγ) with δ ∈ (0, 1) andǫ > 0 some constants which will be specified further on. Dueto the conditionδ ∈ (0, 1) and the fact thatWt is positivedefinite, it is clear thatW is also positive definite forǫ > 0“small enough”. Letk4 =(k4−k5

k3) and recall thatk4 > 0. It

follows from (29) that along the solutions of System (27),ddtW = − k4(k3 − ǫ)|p|2

− ǫ(δ|γ|2 + (1 − δ k5

k3)|v|2 − δvTS(RωB)γ

)

+ ǫpT((1 − δk4)γ + k3v

)(30)

Consider the term

L2 = δ|γ|2 + (1 − δ k5

k3)|v|2 − δvTS(RωB)γ

in the second line of (30). From Assumption 1 it follows that

L2 ≥ δ|γ|2 + (1 − δ k5

k3

)|v|2 − δcω|v||γ|.

Therefore, withδ ∈ (0,min(1, k3

k5)) andδ2c2ω < 4δ(1−δ k5

k3),

L2 is a positive definite function of|γ|, and |v|. Clearly,there exist values ofδ which satisfy the above condition. Bychoosing such a valueδ, from (30), it can be deduced thatfor some constantδ′ > 0 independent ofǫ,

ddtW ≤ − k4(k3 − ǫ)|p|2 − ǫδ′

(|γ|2 + |v|2

)

+ ǫpT((1 − δk4)γ + k3v

)(31)

Using the fact that∣∣ǫpT

((1 − δk4)γ + k3v

)∣∣ =∣∣ǫ

1

4 pT ǫ3

4

((1 − δk4)γ + k3v

) ∣∣

≤ 1

2

(√ǫ|p|2 + ǫ

3

2 |(1 − δk4)γ + k3v|2),

it is straightforward to verify from (31) that there existsǫ0 > 0 such that, for anyǫ ∈ (0, ǫ0),

ddtW ≤ −η(ǫ)W , η(ǫ) > 0. (32)

SinceW is definite positive function forǫ > 0 small enough,this concludes the proof of global exponential stability oftheorigin of System (27). Considering the variable transforma-tion (26), it is concluded that the origin of System (25) isalso globally exponentially stable. This also follows fromthefact that the functionW defined byW(p, v, ab) = W(p, v, γ)satisfies inequality (32) along the solutions of System (25).

B. Proof of Corollary 1

The dynamics of the position estimation error (12) can bewritten as

Y = A(t)Y + g1(Y, ωb, t) (33)

with Y = (p, v, ab), A(t) the right-hand side of System (25),andg1(Y, ωb, t) a “perturbation term” such that

|g1(Y, ωb, t)| ≤ c|Y ||ωb| (34)

for some constantc. This readily implies that the solutions ofthe system are well defined for all time. From Section A ofthis appendix, there exists a quadratic Lyapunov functionWsuch that, along the solution ofY = A(t)Y ,

ddtW ≤ −ηW , η > 0 (35)

Furthermore, it is considered thatωb(t) converges asymp-totically to zero regardless of the initial conditions. Then,it is deduced from (34) and (35) that, along any solutionof System (33), there existsT ≥ 0 such that fort ≥ T ,ddtW ≤ − 1

2ηW . Convergence to zero ofY readily follows

from this inequality.

C. Proof of Corollary 2

The dynamics of the position estimation error (12) can bewritten as {

X = f0(X, t)

Y = A(t)Y + g2(X,Y, t)(36)

with X = (R, ωb), Y = (p, v, ab), f0(X, t), A(t) the right-hand side of System (25), andg2(X,Y, t) a “perturbationterm” such that

|g2(X,Y, t)| ≤ c|X |(1 + |X |)|Y | (37)

for some constantc. From [10], there exists a quadraticLyapunov functionU for the systemX = f0(X, t) such that,in a neighborhood ofX = (I3, 0), d

dtU ≤ −η′U with η′ > 0.

By settingV = U+W, it is verified from (36), and (37) thatin a neighborhood of(X,Y ) =

((I3, 0), 0

), d

dtV = −η′′V

with n′′ > 0. This shows Property 1) of Corollary 2. Theproof of Property 2) is similar to Corollary 1.