J Intell Robot SystDOI 10.1007/s10846-014-0092-z

Quaternion-based Orientation Estimation Fusing a Cameraand Inertial Sensors for a Hovering UAV

Gaston Araguas · Claudio Paz · David Gaydou ·Gonzalo Perez Paina

Received: 24 January 2014 / Accepted: 8 August 2014© Springer Science+Business Media Dordrecht 2014

Abstract Orientation estimation in quadrotors isessential for low-level stability control and for high-level navigation and motion planning. This is usuallycarried out by fusing measurements from differentsensors including inertial sensor, magnetic compass,sonar, GPS, camera, etc. In indoor environments, theGPS signal is not available and the Earth’s magneticfield is highly disturbed. In this work we present a newapproach for visual estimation of the yaw angle basedon spectral features, and a fusion algorithm using unitquaternions, both applied to a hovering quadrotor. Theapproach is based on an Inertial Measurement Unitand a downward-looking camera, rigidly attached tothe quadrotor. The fusion is performed by means of anExtended Kalman Filter with a double measurementupdate stage. The inertial sensors provide informationfor orientation estimation, mainly for roll and pitchangles, whereas the camera is used for measuring the

G. Araguas (�) · C. Paz · D. Gaydou · G. Perez PainaCentro de Investigacion en Informatica para la Ingenierıa(CIII), Universidad Tecnologica Nacional, FacultadRegional Cordoba, Maestro Lopez S/N, Cordoba,Argentinae-mail: garaguas@scdt.frc.utn.edu.ar

C. Paze-mail: cpaz@scdt.frc.utn.edu.ar

D. Gaydoue-mail: dgaydou@scdt.frc.utn.edu.ar

G. Perez Painae-mail: gperez@scdt.frc.utn.edu.ar

yaw angle. A new method to integrate the yaw angle inthe measurement update of the filter is also proposed,using an augmented measurement vector in order toavoid discontinuities in the filter innovation vector.The proposed approach is evaluated with real data andcompared with ground truth given by a Vicon system.Experimental results are presented for both the visualyaw angle estimation and the fusion with the iner-tial sensors, showing an improvement in orientationestimation.

Keywords Orientation estimation · Quadrotor · IMUcamera fusion · Unit quaternions

1 Introduction

Quadrotors have gained popularity in entertainment,aero-shooting and many other civilian or militaryapplications in which these Unmanned Aerial Vehicles(UAV) are teleoperated to perform particular tasks.Disciplines related to mobile robotics are also inter-ested in quadrotors. For these disciplines autonomyhas become one of the most important challenges [1].Driven by the goal of autonomous operation, manyareas of robotics, like control, computer vision, sen-soristics and others, are developing new techniquesand applications to allow these flying robots to per-form some tasks by themselves. Navigation is oneof the fundamental capabilities that an autonomousmobile robot must perform and one important task

J Intell Robot Syst

involved in it is the attitude determination [2]. Itis well known that the attitude can be determinedby means of absolute measurements or by integrat-ing the velocities or accelerations perceived by therobotic platform (dead-reckoning). The characteristicsof quadrotors make them a good choice for opera-tion at low altitude, in cluttered scenarios or evenindoor applications. Such environments limit the useof GPS or compass measurements which are indeedexcelent options for attitude determination in wideopen outdoor areas [3, 4]. Operation spaces wherethe earth’s magnetic field is altered by metallic struc-tures or power lines, and where GPS signals areintermitent or denied reduce the options for attitudecalculation. Two main approaches arise at this point,both offering advantages as well as drawbacks. Onthe one hand, the implementation of beacons or visualmarkers in the operation area, or the use of exter-nal fixed sensors, yields absolute position information,but such external resources limit the operation areato labs arenas. On the other hand, integral methodsor dead-reckoning have practically no working areasconditionings, but suffer the unbounded drift nature ofcummulative techniques. In this context, we presenta new approach to accurately estimate the orientationof a quadrotor in indoor environments for stationaryflights, using a downward-looking camera for headingcalculation as part of the attitude estimation. Station-ary flights or hovering is a particular flight modewhich consists in achieving that the six degrees offreedom of the robot remain fixed around a state asstable as possible. This is an advantageous condi-tion to point a camera downwards and to computecorrespondences between images using homography.Since the focal axis is aproximately orthogonal to thefloor most of the time, transformations between suc-cessive images, represented by a homography, canbe approximated by a pure rotation around the focalaxis. An important part of the standard avionics ofquadrotors is the Inertial Measurements Unit (IMU).It plays a fundamental rol in fast angular-stabilizationcontrol loops. The measurements taken by an IMUhave high precision and high output rates; however,they suffer from the already mentioned problem ofunbounded drift. Usually, other sensors come intoplay to complement this drawback of the measure-ments taken by an IMU. One of the most preferredcomplementary sensor applied to quadrotors are cam-eras, because their outputs are rich in information

of the environment and they are lightweight; andit is worth mentioning that payload is a weakpoint in quadrotors platforms. AR.Drone fromParrot company presents an example of such acombination [5].

In order to exploit the complementary properties ofIMUs (high output rate, high drift) and cameras (lowoutput rate, low drift), their measurements are usu-ally fused using Bayesian filtering approaches, likethe Kalman Filter (KF) and its derived versions; i.e.Extended Kalman Filter (EKF), Unscented KalmanFilter (UKF), Extended Information Filter (EIF),etc. [6]. Alternative formulations of these filters, aretheir corresponding sequential measurement updateversions (SEKF, SUKF, SEIF), which have lowercomputational cost compared to the non-sequentialversions, and allow to integrate measurements fromsensors with different output rates. In sequential fil-ters the measurement vector and the correspondingcovariance matrix are divided into smaller parts, andthey are used sequentially in the measurement updatefilter stage. It is important to note that it is only appli-cable if the covariance matrix of the measurementnoise is block diagonal. Because of this, sequentialfilters result with direct application in fusing the dif-ferent sources of information, as presented in [7].In this work, the authors propose a multi-sensor fusionscheme with a sequential UKF (SMU-UKF, Sequen-tial Measurement Update UKF). This sensor fusionscheme is used as the core of an inertial naviga-tion system, which uses accelerometers and gyro-scopes readings in each measurement update stage,and has the possibility of adding other sensors (likeGPS, magnetometers, altimeters, cameras, etc.) asaid sensors for the estimation. Accordingly, the fil-ter works with a measurement vector of varyingsize, composed of sensor measurements of differ-ent output rates depending on the number of sensorsused.

A simplified version of the aforementioned fusionscheme is proposed in [8], using a double measure-ment update EKF. This work presents the imple-mentation of an application-specific instruction-setprocessor (ASIP) for the fusion of a 9D inertialsensor. The proposed sensor fusion scheme allowsto decouple the measurement updates of the rolland pitch estimated angles, from the yaw estimatedangle. In the scheme of [8], the roll and pitch anglesare updated using the accelerometer measurements,

J Intell Robot Syst

and the yaw angle update is based on magnetome-ter measurements. Authors argue that using theirdecoupling scheme, the perturbations of the magne-tometer measurements do not affect the estimationof the roll and pitch angles. Taking advantage of thedecoupling characteristic of the double measurementupdate EKF, it is possible to replace the magnetome-ter used in the second stage by another sensor. Weadopt this approach in our implementation of thefusion algorithm, and replace the magnetometer head-ing reference by a yaw angle estimated using visualinformation.

A number of spatial and frequency domainapproaches have been proposed to estimate the image-to-image transformation between two views of a pla-nar scene. Most of them are limited to similaritytransformations. Spatial domain methods need cor-responding points, lines, conics, etc. [9–11], whoseidentification in many practical situations is nontrivial,thereby limiting their applicability. Many approachesto this problem are based on the color informationpresent in the scene [12, 13]. They compute fea-tures using the intensity values of the image. Scale,rotation, and translation invariant features have beenpopular, facilitating recognition under these transfor-mations. Geometry of multiple views of the samescene has been a subject of extensive research overthe past decade. Important results relating correspond-ing entities such as points and lines can be foundin [9, 10]. Recent work has also focused on morecomplex entities such as conics and higher-orderalgebraic curves [11]. Invariants have been formu-lated for contours in multiple views [14]. However,these approaches depend upon extracting correspond-ing entities such as points, lines or contours and do notutilize the abundant information present in the form ofthe intensity values in the multiple views of the scene.Frequency domain methods are in general superior tomethods based on spatial features [10] because theentire image information is used for matching. Theyalso avoid the crucial issues regarding the selection ofthe best features.

Our work proposes the utilization of a fixed num-ber of patches distributed on each image of thesequence to determine the yaw angle of the cam-era. The visual yaw angle measurement is estimatedusing the homography induced by the (assumed flat)floor, and the corresponding points are obtained from

the frequency domain. This spectral information cor-responds to an image patch which we call spec-tral feature. These kind of features perform betterthan the interest points based on the image inten-sity when observing a floor with homogeneous tex-ture. Moreover, because their position in the imageplane is previously selected, they are always welldistributed.

Another contribution presented in this work is anovel approach in the fusion scheme that exploitsthe complementary properties of IMU measurementsand the visual yaw angle estimation. The fusionscheme is based on an augmented measurementvector integrated by an Extended Kalman Filter(EKF) with a double update stage, using quater-nions for orientation representation. Results are val-idated using an appropriate measurement for localerror as well as the cosine distance to the groundtruth proposed in the benchmark of the used dataset.These evaluations are performed for both, the yawangle given by the visual algorithm and the finalorientation estimation. Our results show that theestimated yaw angle has low absolute error, andthe orientation estimation shows a significant driftreduction.

The paper is organized as follows: Section 2 givesan overview of the proposed approach for orienta-tion estimation as well as the visual algorithm foryaw angle estimation. Section 3 presents the basisfor camera orientation estimation, particularly for thecase of yaw angle estimation, and a brief explana-tion of the spectral features. The description of themodels used for orientation estimation fusing inertialmeasurements and the camera information is given inSection 4. Results are presented in Section 5 for both,camera yaw angle estimation and the fusion algo-rithm, using a double measurement update stage EKF.Finally, Section 6 remarks the conclusions and futurework.

2 General Description

The proposed approach for orientation estimation isbased on the fusion of the measurements of two sen-sors, the inertial measurements, and the visual infor-mation. The block diagram is represented in Fig. 1.The developed algorithm is applied to an UAV which

J Intell Robot Syst

has an IMU and a monocular downward-looking cam-era, both rigidly attached. The fusion is based on adouble measurement update stage EKF, using quater-nions for orientation representation. As shown inFig. 1, the prediction stage as well as the first mea-surements update of the EKF use the informationgiven by the IMU, whereas the second measurementsupdate uses the relative angular motion obtained bythe camera.

The IMU is composed of a three-axis accelerom-eter and a three-axis gyroscope. The latter measuresthe body angular rate whilst the first one measuresthe body accelerations, together with the gravity force.The proposed fusion scheme uses the gyroscope mea-surement in the prediction stage of the filter, work-ing as an integration process of the angular rate todetermine changes in the orientation. Accelerome-ter readings are used in the second measurementupdate stage of the filter, correcting mainly the esti-mation of the body roll and pitch angles. This isbecause considering a UAV in an nearly hover opera-tion, as assumed in the present work, the accelerom-eter measurements are mainly due to the gravityeffect.

On the other hand, and to complete the full threedegree-of-freedom orientation estimation, the camerais used to estimate the body yaw angle. The visualyaw angle estimation is based on the principle thattwo consecutive images of a planar scene are related

Fig. 1 Block diagram of the implemented orientation estima-tion approach

by a homography. The planar scene corresponds tothe floor surface, which is assumed to be relativelyflat, observed by the downward-looking camera on theUAV. The spatial transformation of the camera, andtherefore of the UAV, is encoded in this homography.Considering some constraints imposed by a smoothflight, the yaw angle between two consecutive viewscan be derived. In order to determine the homographyinduced by the planar surface, a set of correspondingpoints on two consecutive frames must be obtained.This correspondence process is performed selecting aset of features in the first image and finding the corre-sponding set of features in the second one. Then, theimage coordinates of each feature in both images con-form the set of corresponding image points needed tocalculate the homography.

The image features used in this approach are the so-called spectral features. A spectral feature is a Fourierdomain representation of an image patch. Selecting aset of spectral features in both images (with the samenumber, size, and position), the displacement of eachfeature can be obtained using the Fourier shift the-orem. This displacement, in addition to the featurecenter, determines the correspondence between fea-tures in both images. In Fig. 2 a block diagram of theyaw angle estimation process is shown, in which ninespectral features in both images are used.

3 Visual Yaw Angle Estimation

The transformation between two images taken fromdifferent views (with a moving camera) contains infor-mation about the spatial transformation of the views,or the camera movement. Considering a downward-looking camera, and assuming that the floor is a planarsurface, all the space points imaged by the cameraare coplanar and there is a homography between theworld and the image planes. Under this constraint,if the camera center moves, the images taken fromdifferent points of view are also related by a homogra-phy. The spatial transformation that relates both viewscan be completely determined from this homographybetween images.

3.1 Homography Estimation

Using the pin-hole camera model, a given 3D scenepoint M = [X, Y, Z, 1]T ∈ P

3 is projected onto

J Intell Robot Syst

Fig. 2 Block diagram of the implemented visual yaw angle estimation approach

the point m = [u, v, 1]T ∈ P2 on the image plane.

This projection is represented by the camera matrix P

accordingly to

sm = PM = K[R|t]M (1)

where R ∈ SO(3) is a rotation matrix, t ∈ R3 a trans-

lation vector, s an arbitrary scale factor, and K theintrinsic parameters matrix of the camera. The rigidtransformation (R, t), also known as extrinsic param-eters, relates the camera coordinate system (CCS) tothe world coordinate system (WCS) in which the pointM is expressed.

Assuming that the camera is pointing to the ground(downward-looking camera) and that the scene struc-ture is approximately a planar surface, the projectionof 3D points can be expressed by an affine transforma-tion [15]. Without loss of generality, it can be assumedthat the plane is in Z = 0, then

sm = K[R|t]

⎡⎢⎢⎣X

Y

01

⎤⎥⎥⎦ = K [r1 r2 t]

⎡⎣X

Y

1

⎤⎦ (2)

where r1 and r2 are the first and second column of R.From (2), the point M is projected onto a point on theimage plane m by a homography [10]

sm = HM (3)

with

H = K [r1 r2 t] (4)

If we consider a moving camera, the correspondingcoordinate system moves together with the camera.Calling A the CCS at time tA we have

sAmA = HWAM (5)

where HWA is the homography that relates the WCSto the camera coordinate system at time tA.

J Intell Robot Syst

Under the same assumption, the scene point M inthe next camera frame is projected onto mB

sBmB = HWBM (6)

where HWB relates the WCS to the CCS at time tB .If the camera motion between two consecutive framesis smooth, the unknown scale factor sB can be con-sidered the same as sA in Eq. 5. Therefore, from (5)and (6)

mA ≈ HBAmB (7)

with HBA = HWA(HWB)−1 the homography that

relates corresponding points mA ↔ mB . This homog-raphy represents the CCS transformation betweeninstant tA and tB : hence, it contains the informationof the camera rotation and translation. Although thiscamera can generally rotate in any direction or aroundany axis, in hovering operation or in a smooth flight,roll and pitch rotations (around axes nearly parallel tothe ground plane) can be considered small comparedto the yaw angle rotation, as is shown in Fig. 3. Thus,neglecting roll and pitch rotations, the homographyrelating consecutive images can be approximated by

mA = HBAmB ≈[Rz t0 1

]mB (8)

where Rz ∈ SO(2) is the rotation matrix around thefocal axis and t ∈ R

2 is the translation vector, repre-senting the rotation and translation of the CCS from

instant tA to tB . Equation 8 shows that a rotation inhovering mode for an UAV can be approximated by anEuclidean transformation.

3.2 Spectral Features Correspondence

In order to estimate the homography given by twoconsecutive images from a moving camera, a setof corresponding points must be determined. Clas-sically, this set of points is obtained by detectingfeatures, such as lines and corners in both images,and determining correspondences. The feature detec-tors are typically based on image gradient methods.The most common approach consists on a Harriscorner detector [16] followed by a Lucas-Kanade fea-ture tracker [17]. An alternative to this approach isto use frequency-based features, or spectral features,and to determine correspondences in the frequencydomain.

The so-called spectral feature refers to the Fourierdomain representation of an image patch of 2n × 2n,where n ∈ N

+ is set accordingly to the allowed imagedisplacement [18]. The power of 2 of this patch sizeis selected based on the efficiency of the Fast FourierTransform (FFT) algorithm. The number and posi-tion of spectral features in the image are set before-hand. Even though a minimum of four points areneeded to estimate the homography, a higher num-ber of features are used to increase the accuracyand the RANSAC algorithm [19] is used for outlierselimination.

Consider two consecutive frames, where spectralfeatures on each image were computed. To determinethe correspondence between features is equivalentto determine the displacement between them. Thisdisplacement can be obtained using the spectral

Fig. 3 Estimation of therotation and translationbetween two consecutiveimages based on spectralfeatures

J Intell Robot Syst

information by means of the Phase CorrelationMethod (PCM) [20]. This method is based on theFourier shift theorem, which states that the Fouriertransforms of two identical but displaced images differonly in a phase shift.

Given two images iA and iB of size N×M differingonly in a displacement (u, v), such as

iA(x, y) = iB(x−u, y−v) ; u ≤ x < N−u, v ≤ y < M−v

(9)

their Fourier transforms are related by

IA(ωx, ωy) = e−j (uωx+vωy)IB(ωx, ωy) (10)

where IA and IB are the Fourier transforms ofimages iA and iB , respectively; u and v are thedisplacements for each axis. From (10), the ampli-tudes of both transformations are the same andonly differ in phase which is directly related to theimage displacement (u, v). Therefore, this displace-ment can be obtained from (10) using the crosspower spectrum of the given transformations IA andIB .

The cross power spectrum (CPS) of two complexfunctions is defined as

C(F,G) = F(ωx, ωy)G∗(ωx, ωy)

|F(ωx, ωy)||G∗(ωx, ωy)| (11)

where G∗ is the complex conjugate of G, and the selfCPS of a given function is C(F, F ) = 1. Using (10) inEq. 11 over the transformed images IA and IB , gives

C(IA, IB) = IA(ωx, ωy)I∗B(ωx, ωy)

|IA(ωx, ωy)||I∗B(ωx, ωy)| = e−j (uωx+vωy) (12)

= Q(ωx, ωy)

where Q(ωx, ωy) is called the correlation phasematrix. The inverse transform of Q(ωx, ωy) is animpulse located exactly in (u, v) which represents thedisplacement between the two images

F−1[Q(ωx, ωy)] = q(x, y) = δ(x − u, y − v). (13)

Using the discrete FFT (Fast Fourier Transform) algo-rithm instead of the continuous version, the result willbe a pulse signal centered in (u, v) [21].

3.3 Yaw Angle Estimation

The previous subsection describes how to determinethe displacement between two images using PCM.Applying this method to each image patch pair, thedisplacement between spectral features is determined.The set of corresponding points required to estimatethe homography can be constructed with the patchcenters of the first image and the displaced patchcenters of the second one, that is

{mAi ↔ mAi +�di = mBi } (14)

where �di represents the displacement between thei-th spectral feature, and mAi the center of the i-thspectral feature in the CCS A. This is schematicallyshown in the zoomed area of Fig. 3.

As shown in Section 3.1, this set of correspond-ing points is related by a homography from which,using linear methods plus nonlinear optimization, theassociated homography matrix can be computed [10].Then from (8), the rotation matrix containing the rota-tion angle of the camera around the focal axis isobtained

Rz =[

cosψ − sinψsinψ cosψ

](15)

As the camera is rigidly attached to the UAV, the angleψ in Eq. 15 represents the change of the UAV head-ing within a sampling period. The UAV yaw angle canbe computed by accumulating this change over time.Consequently the yaw angle obtained in this way willdrift, because of accumulating errors. Nevertheless,the accumulating errors are much less than those asso-ciated to the IMU bias, and therefore the estimatedyaw angle given by the fusion will be improved.

It is important to note that the number, size, andposition of spectral features are set beforehand: there-fore, neither a search nor a correspondence processneeds to be performed. Moreover, due to the fact thatthe spectral features use the frequency spectrum of theimage intensity as feature descriptor, they result to bemore robust than the gradient-based features, which ingeneral work only in presence of corners in the image.

Summarizing, Algorithm 1 shows the proposedsteps to achieve the visual yaw angle estimation.

J Intell Robot Syst

Algorithm 1 Visual Yaw Estimation Function

function YAW ESTIMATION(It , It−1) � Current and previous images are passed as argumentsObtain patches pi t and pi t−1 from imagesfor all {pi t , pi t−1} do

Pi t ← FFT (pi t )

Pi t−1 ← FFT (pi t−1) � Previous patch FFT can be saved for faster processingR ← CPS(Pi t , Pi t−1)

r ← IFFT (R) � Inverse FFT�di ← MAX(r)

end formi t ← mi t−1 +�di

yaw ← Rz ← H ← homography(mi t ,mi t−1)

return yaw

end function

4 Orientation Estimation

This section describes the proposed approach for ori-entation estimation applied to a hovering UAV. Theorientation is estimated by the fusion of inertial sensormeasurements provided by an IMU and the visual yawangle estimation presented in the previous section.The IMU is composed of accelerometers and gyro-scopes, both with three axes, which deliver high-rateraw data. The fusion is performed using a double mea-surement update stage EKF and unit quaternions fororientation representation.

Measurements of accelerometers provide accurateinformation about the tilt of the body, given that theymeasure not only body accelerations, but also theEarth’s gravity vector. The accelerometer readings areused in the first measurement update stage of the EKFin which the orientation is corrected mainly at the rolland pitch angles. On the other hand, and to completethe three degrees of freedom of the orientation estima-tion, the yaw angle from the visual algorithm is usedin the second measurement update stage of the EKF.

4.1 Quaternion for Orientation Representation

The orientation of a rigid body can be represented bynumerous methods. Euler angles and Direct CosineMatrix (DCM) are more intuitive representationmethods than quaternions; nevertheless, the quater-nion representation is computationally more con-venient. Quaternions are an extension of complexnumbers and consist of a scalar and a complexvector. Both can be represented by a four-compo-nent vector of the form q = [q0, q1, q2, q3]T . Unit

quaternion has the additional constraint of ||q|| =1. This constraint must be satisfied to represent therotations.

Given the angular velocity vector of a rigid bodyω = [ωx, ωy, ωz]T in the local reference frame,changes in the orientation of the body in the globalreference frame can be expressed by

q = 1

2q ⊗

[0ω

](16)

where ⊗ stands for quaternion multiplication orHamilton product. Equation 16 represents the timederivative of the orientation quaternion [22] and it canalso be written as

q = 1

2�(ω)q (17)

where �(ω) is a skew-symmetric matrix associated tothe vector ω

�(ω) =[

0 −ωT

ωT ω×�T]=

⎡⎢⎣

0 −ωx −ωy −ωz

ωx 0 ωz −ωy

ωy −ωz 0 ωx

ωz ωy −ωx 0

⎤⎥⎦ (18)

Following [23], the discrete solution of Eq. 17 is givenby

qk = e12�(ω)�tqk−1; q0 = q(0) (19)

where �t = tk − tk−1. Assuming that ω(t) = ωk

between tk and tk−1, (19) can be expanded to

qk=[cos

(|ωk|2

�t

)I4×4+�(ωk)

|ωk| sin

(|ωk|2

�t

)]qk−1

(20)

J Intell Robot Syst

Furthermore, considering that |ωk|�t � 1, whichis a valid assumption in hovering flight, (20) can beapproximated to

qk ≈(

1

2�(ωk)�t + I

)qk−1 (21)

This relation is numerically more stable than (20)(see [23] for details) and will be used to represent thesystem rotations in the next section.

4.2 IMU and Yaw Angle Fusion

In order to apply an EKF, the system has to bedescribed by a stochastic state space dynamic model,i.e.

xk = fk(xk−1,wk−1) (22)

zk = hk(xk, vk) (23)

where fk(·) and hk(·) are known as the process andthe observation functions, respectively: wk and vkare the uncorrelated zero mean Gaussian noises. Thestate vector is defined by the four elements of thequaternion at time k, xk = [q0, q1, q2, q3]T . As it iswell known, every implementation of a Kalman fil-ter consists of two stages: prediction and measurementupdate [24].

In the prediction stage, the current state is com-puted based on the state of the previous step usingEq. 22, in which the process function fk(·) is obtainedfrom (21) which results to be linear. Therefore, it isrepresented by the so-called transition matrix F k

F k = 1

2�(ωk)�t + I (24)

where angular velocity ωk is provided by the gyro-scope measurements, I is a 4 × 4 identity matrix,and �t is the time elapsed between the gyroscopereadings.

As usually done, the measurement update stage iscomputed based on the so-called innovation, which isthe difference between the current and the predictedmeasurements (zk − zk). Given that the sampling rateof the inertial sensor is higher than the camera framerate, the measurement update stage is divided into twoparts [25]. Each of these parts requires its own obser-vation function given by Eq. 23, where the first oneis stated as za = ha(·) for accelerometer measure-ments and the second one zh = hh(·) for headingmeasurements.

In the first part, the gravity vector measurement andthe gravity vector prediction are used to calculate theinnovation. The gravity vector measurement is givenby the accelerometer readings za = [ax, ay, az]T , andthe predicted gravity za is obtained by rotating thegravity vector to the predicted quaternion as in [22],that is

za=⎡⎣q2

0 + q21 − q2

2 − q23 2(q1q2 + q3q0) 2(q1q3 − q2q0)

2(q1q2 − q3q0) q20 − q2

1 + q22 − q2

3 2(q2q3 + q1q0)

2(q1q3 + q2q0) 2(q2q3 − q1q0) q20 − q2

1 − q22 + q2

3

⎤⎦ g

(25)

given that g = [0, 0, g]T , the observation functionha(·) in time step k is defined by

zak = hak(x−k ) = g

⎡⎣

2q1q3 − 2q2q0

2q2q3 + 2q1q0

q20 − q2

1 − q22 + q2

3

⎤⎦ (26)

which results to be a nonlinear function.The second part of the measurement update stage

is executed when visual information is available, andis used to correct the yaw angle estimation. As in thefirst part, the yaw angle innovation also needs to becomputed. This is given by the difference betweenthe angle provided by the visual algorithm and thepredicted one using the current estimated quaternion.This predicted angle is calculated with the equivalencebetween yaw angle and quaternions [26] as follow

ψ = arctan (γ ) (27)

where

γ = 2(q0q3 + q1q2)

q20 + q2

1 − q22 − q2

3

(28)

An important issue in computing the innovationusing (27) is that this function presents discontinuities,which can produce erroneous and huge innovation val-ues. Similarly, the same problem can occur if the yawangle estimated by the visual algorithm is boundedbetween ±π instead of being an absolute angle. Toovercome this problem, a two-dimensional measure-ment vector defined by zh = [cos ψ, sin ψ]T isproposed to be used. Thus, the predicted measurementfor computing the innovation can be simplified usingthe follow trigonometric identities

cos (arctan (a)) = 1√a2+1

(29)

sin (arctan (a)) = a√a2+1

(30)

J Intell Robot Syst

Hence, the observation function hh(·) in time step k isdefined by

zhk = hhk(x−k ) =

⎡⎣

1√γ 2+1γ√γ 2+1

⎤⎦ (31)

which like (26) is also a nonlinear function.Once the complete system has been defined, and

given that the process function is linear, the predic-tion of the state x−k and its covariance matrix P−

k arecomputed as in the standard version of the Kalmanfilter [6]

x−k = F k xk−1 (32)

P−k = F kP k−1F

Tk +Qk (33)

where F k is given by Eq. 24 and Qk is the covariancematrix of the process noise wk in Eq. 22. More-over, given that both measurement functions ((26) and

(31)) are nonlinear, the measurement update stages arecalculated by

Kk = P−k H

Tk (H kP

−k H

Tk + Rk)

−1 (34)

xk = x−k +Kk(zk − hk(x−k )) (35)

P k = (I −KkH k)P−k (36)

where Rk is the covariance matrix of the observationnoise vk in Eq. 23, Kk is the so-called Kalman gainmatrix, and H k is the Jacobian matrix of Eq. 23. Par-ticularly, H k = H ak when IMU measurements areused and H k = Hhk when camera yaw angle estima-tion is available. The Jacobian matrices H ak and H hk

are computed from the linearization of Eq. 26 and (31)shown in Appendix A and evaluated in the predictedstate x−k .

To summarize, the full estimation procedure isshown in Algorithm 2.

-3

-2.5

-2

-1.5

-1

0 20 40 60 80 100

yaw angle [rad]

time [s]

CameraGround truth

α = 0.99558

0

0.02

0.04

0.06

0.08

0.1

0.12

0.14

0.16

20 40 60 80 100

yaw angle error [rad]

time [s]

Fig. 4 Visual yaw angle estimation (a) Ground truth yaw angle (thin line) and estimated angle (thick line) (b) Yaw angle absoluteestimation error

J Intell Robot Syst

Algorithm 2 Full Algorithm for Orientation Estimation

x0,P 0 ← EKF INIT()for ever do

x−k ,P−k ← PREDICT(ω) � Gyroscope measurement is passed as argument

za ← GRAVITY ROTATION(x−k ) � Gravity prediction andxk,P k ← UPDATE(za, za) � Accelerometer measurement are passed as argumentsif new frame is available then

zh ← [cosψ, sinψ]T ← YAW ESTIMATION(It , It−1)

zh ← [cos ψ, sin ψ]T ← QUATERNION2YAW(x−k ) � Yaw angle prediction andxk,P k ← UPDATE(zh, zh) � Yaw angle estimation are passed as arguments

end ifend for

5 Results

Experimental results were obtained using the datasetsdescribed in [27], which are available in thesFly project website1. Every dataset taken by anUAV consists of image sequences from both aforward-looking and a downward-looking camera,along with measurements from an IMU and theground truth information given by a Vicon system.In [27] the authors also described a benchmarkingprocedure based on the cosine distance, which isused to measure the similarity between the estimatedangles by means of three different visual algorithmsand the ground truth. Explicitly, given two vec-tors E (estimation) and G (ground truth), the cosinesimilarity is α = (EG)/(||E|| ||G||). This simi-larity factor measures the coincidence of the esti-mation with respect to the ground truth onlyin a global sense. Moreover, according to itsdefinition if the estimation has a bias (i.e. ascale in E), it is not reflected in the similarityvalue.

The used dataset is named hoveringDown, andit consists of 2041 image frames taken at approxi-mately 20fps. The IMU and the Vicon system sam-ple rate are 200Hz, resulting in a total of 21388samples. These correspond to a flight period ofapproximately 106 seconds, where the UAV takesoff, performs a hovering flight, and ends landingnear the takeoff place. The total amount of yawangle change during the flight is approximately1.4rad.

1http://www.sfly.org

5.1 Visual Yaw Angle Estimation

Figure 4 shows the yaw angle estimation results.In Fig. 4a the yaw angle value given by the ground

truth (in thin line) together with the estimated by thevisual algorithm previously presented (in thick line)are observed. The cosine similarity factor given bythe standard benchmark procedure for the dataset isα = 0.99558, showing that the estimated yaw anglestrongly matches the ground truth. Figure 4b showsthe estimation error, defined as the absolute differ-ence between the ground truth and the estimated yawangles (ψe = |ψ − ψ |). As it can be observed, themaximum accumulated error is about 0.12rad (atapproximately 65s) which is then compensated by alater rotation in the opposite direction. However, aglobal evaluation of a local estimation method usingthe absolute error or the cosine similarity does notdescribe completely the accuracy of the results, main-ly because the estimation error can be compensated bybackward motions [28]. A better approach to evaluatethe accuracy of a local method is the mean accumula-ted error [29]. The mean accumulated error εkN for thek-th sample of N consecutive images is given by

εkN = 1

N

k+N∑i=k

∣∣∣ψie − ψk

e

∣∣∣ , k = 1, . . . ,M −N (37)

where M is the total number of samples. Figure 5shows the mean accumulated errors ε10 and ε100 cor-responding to 10 and 100 consecutive images (equiv-alent to 0.5s and 5s of flight). As can be seen, the ε10

is lower than 0.008rad most of the time, and presents apeak of approximately 0.025rad near 80s correspond-ing to a sudden maneuver of the UAV. In the case ofε100 the accumulated error grows to a maximum of0.05rad.

J Intell Robot Syst

0

0.01

0.02

0.03

0.04

0.05

0.06

20 40 60 80 100

yaw angle error [rad]

time [s]

ε10ε100

Fig. 5 Visual yaw angle mean accumulated error for 10 (ε10) and 100 frames (ε100)

5.2 IMU/Camera Orientation Estimation

The orientation estimation is performed using adouble measurement update stage EKF, fusing theIMU and the visual yaw angle estimation. Another

filter with a single update stage using only the IMUmeasurements is also implemented for comparisonpurposes. The prediction stage of both filters is thesame, given by Eq. 32 and (33). The matrixF k in theseequations was assembled based on Eq. 20 and (21),

-0.2

0

0.2

0.4

0.6

0.8

1

1.2

0 20 40 60 80 100

q0

Ground truthIMU EKF

IMU/camera EKF

-0.06-0.04-0.02

00.020.040.060.080.1

0.12

0 20 40 60 80 100

q1

Ground truthIMU EKF

IMU/camera EKF

-0.06

-0.04

-0.02

0

0.02

0.04

0.06

0.08

0.1

0 20 40 60 80 100

q2

Ground truthIMU EKF

IMU/camera EKF

-1.05-1

-0.95-0.9-0.85-0.8-0.75-0.7-0.65-0.6

0 20 40 60 80 100

q3

time [s]

Ground truthIMU EKF

IMU/camera EKF

Fig. 6 Estimated quaternion components using IMU measurements (dashed line), IMU/camera (thick solid line) and ground truthdata (thin solid line)

J Intell Robot Syst

obtaining in both cases identical results for the useddataset. Given these results, the simplest expresion ofF k was chosen to be used because of its numericalstability.

Figure 6 shows the estimated quaternion compo-nents for both implemented filters, together with theground truth orientation reference (thin solid line).

In both cases, the estimated quaternion presents adrift in the first and last components. These compo-nents contain the yaw angle information, and the driftappears because of the IMU gyroscope measurementsintegration. Using the camera yaw angle measure-ments, this drift is reduced (thick solid line) and the

mentioned components (q0 and q3) present a lowererror, given that the visual yaw angle estimation con-tains only zero mean random error. Conversely, theother two components (q1 and q2), which encode onlythe roll and pitch angles, are drift free due to the factthat the gravity vector acts as an external reference.Figure 7 shows the same results but expressed in Eulerangles (roll, pitch and yaw). In this case, it can beseen a lower estimation error in the yaw angle for thecase of the IMU/camera fusion filter (thick solid line).The cosine similarity α applied to each of the Eulerangles does not show significant changes, comparingthe IMU and the IMU/camera fusion results. For the

-0.1

-0.05

0

0.05

0.1

0.15

0 20 40 60 80 100

roll [rad]

Ground truthIMU EKF

IMU/camera EKFα = 0.88464α = 0.88393

-0.15

-0.1

-0.05

0

0.05

0.1

0.15

0 20 40 60 80 100

pitch [rad]

Ground truthIMU EKF

IMU/camera EKFα = 0.83222α = 0.83057

-3.5

-3

-2.5

-2

-1.5

-1

0 20 40 60 80 100

yaw [rad]

time [s]

Ground truthIMU EKF

IMU/camera EKF

α = 0.99967α = 0.99984

Fig. 7 Estimated Euler angles using IMU measurements (dashed line), IMU/camera (thick solid line), and ground truth data (thinsolid line)

J Intell Robot Syst

case of the roll and pitch angles, the fusion with thecamera measurements does not improve the estima-tion, given that the camera provides only yaw angleinformation. The yaw angle in the camera-aided ver-sion has a lower bias with respect to the IMU onlycase; however, the cosine similarity does not reflectthis improvement.

Figure 8 shows the absolute error and the meanaccumulated error of the yaw angle given by the twofilters. As Fig. 8a shows, the absolute error given bythe camera-aided version is similar to the one obtainedby the visual algorithm shown in Fig. 4b. In con-trast, the absolute error of the IMU filter grows mostof the time, because of the gyroscope measurementsbias error integration. This behavior can be clearlyobserved in the first 40s of the sequence, when theyaw angle of the UAV remains relatively constant.

Figure 8b shows the mean accumulated error for100 samples (using N = 100 in Eq. 37) equivalent

to the error of 10 image frames (ε10). It can be seenthat for a short time both filters present similar errors.However, the magnitude of this error, most of the timebelow 0.004rad, is lower compared with the accumu-lated error of the visual yaw angle estimation (ε10)presented in Fig. 5. These results certainly prove thecomplementary nature of both the IMU and the cam-era sensors, because fusing their measurements theestimation takes the better of each one, i.e. lowerbias (given by the camera) and lower short-time error(given by the IMU).

Finally, in Fig. 9 the estimation error for both fil-ters together with the ±3σ bounds are shown. In thiscase, it can be observed that the uncertainties in theestimation of the IMU/camera filter (Fig. 9b) resultto be lower compared to the estimation using onlythe IMU measurements (Fig. 9a). These lower uncer-tainties also appear in the quaternion componentscorresponding to the yaw angle.

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0 20 40 60 80 100

yaw absolute error [rad]

time [s]

IMU EKFIMU/camera EKF

0

0.005

0.01

0.015

0.02

0.025

0 20 40 60 80 100

mean accumulated error [rad]

time [s]

IMU EKFIMU/camera EKF

Fig. 8 Yaw angle estimation errors of IMU (dashed line) and IMU/camera (thick solid line) filters: (a) Absolute error, (b) Meanaccumulated error

J Intell Robot Syst

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

0 20 40 60 80 100

q0

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q1

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q2

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q3

time [s]

-0.8

-0.6

-0.4

-0.2

0

0.2

0.4

0.6

0.8

0 20 40 60 80 100

q0

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q1

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q2

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

0.3

0.4

0 20 40 60 80 100

q3

time [s]

Fig. 9 Estimation error for both, (a) only IMU and (b) IMU/camera filters together with ±3σ bounds

6 Conclusions

In this work a new approach for quadrotor orienta-tion estimation fusing inertial measurements with adownward-looking camera was presented. The pro-posed fusion algorithm is performed by means of thewidely used Extended Kalman Filter, where the mea-surement update stage is composed of one part whenonly the IMU measurement is available, and of twoparts when the IMU and the camera measurements areavailable. The IMU measurements are used mainly for

roll and pitch angles estimation, exploiting the knowngravity vector as a reference. The downward-lookingcamera is used for heading or yaw angle estimationbased on spectral features. The main advantage ofusing spectral features as in this implementation, isthat the typical correspondence process does not needto be performed.

Experimental results have been obtained using apublic dataset of a hovering UAV. The proposedapproach was evaluated separately for the visualyaw angle estimation algorithm and the IMU/camera

J Intell Robot Syst

fusion. The evaluation of the visual algorithm hasshown that the yaw angle is estimated without signif-icant absolute error, despite the typical accumulatederror of the integration process. On the other hand,the fusion of the IMU and the camera measurementshas shown an important reduction of the absolute andshort-term accumulated errors. In other words, theIMU/camera fusion takes the best of each sensor, thatis a lower bias of the visual estimation, and a highrate together with a lower short-term error of the IMUmeasurements.

Currently, as a direct application of this work,a close-loop control algorithm for the UAV orien-tation is being tested. Moreover, as an extensionof the present work, the next step is to estimatealso the position in order to have a six degree-of-freedom estimation. In order to compute theposition using visual information, the image scalealso needs to be estimated. Based on the resultsobtained in this work, significant reduction of IMUerrors are expected by fusing the visual positionestimation.

Acknowledgments This work was partially funded by theArgentinian National Agency for the Advance in Science andTechnology through the project “Autonomous Vehicle GuidanceFusing Low-cost GPS and other Sensors”, PICT-PRH-2009-0136 and the Universidad Tecnologica Nacional through theproject “Autonomous Quadcopter of Open Architecture”, UTN-PID-1484. Both currently under development at the Center forIT Research (CIII), Cordoba, Argentina.

Appendix A Jacobian matrices Ha and Hh

The Jacobian of Eq.26 is

H a = ∂ha

∂q=

[ −2q2 2q3 −2q0 2q12q1 2q0 2q3 2q22q0 −2q1 −2q2 2q3

](38)

The Jacobian of Eq.31 is

Hh = ∂hh

∂q=

[h11 h12 h13 h14

h21 h22 h23 h24

](39)

where

h11 = −8q3(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 −

16q0(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)3

2

(4(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)2 +1

) 32

(40)

h12 = −8q2(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 −

16q1(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)3

2

(4(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)2 +1

) 32

(41)

h13 = −8q1(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 +

16q2(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)3

2

(4(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)2 +1

) 32

(42)

h14 = −8q0(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 +

16q3(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)3

2

(4(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)2 +1

) 32

(43)

and

h21 = 2q3

d1− 4q0 (q0q3 + q1q2)

d2(45)

−(q0q3 + q1q2)

(8q3(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 − 16q0(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)3

)

d3

h22 = 2q2

d1− 4q1 (q1q2)

d2(46)

−(q0q3 + q1q2)

(8q2(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 − 16q1(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)3

)

d3−

h23 = 2q1

d1+ 4q2 (q0q3 + q1q2)

d2(47)

−(q0q3 + q1q2)

(8q1(q0q3+q1q2)

(−q32−q2

2+q12+q0

2)2 + 16q2(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)3

)

d3

h24 = 2q0

d1+ 4q3 (q0q3 + q1q2)

d2(48)

−(q0q3 + q1q2)

(8q0(q0q3+)

(−q32−q2

2+q12+q0

2)2 + 16q3(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)3

)

d3

with

d1 = (−q32 − q2

2 + q12 + q0

2)√

4(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)2 + 1 (49)

d2 = (−q32 − q2

2 + q12 + q0

2)2

√4(q0q3+q1q2)

2

(−q32−q2

2+q12+q0

2)2 + 1 (50)

d3 = (−q32 − q2

2 + q12 + q0

2)(

4(q0q3+q1q2)2

(−q32−q2

2+q12+q0

2)2 + 1

) 32

(51)

J Intell Robot Syst

References

1. Gupte, S., Mohandas, P.I.T., Conrad, J.M.: A survey ofquadrotor unmanned aerial vehicles. In: Southeastcon, 2012Proceedings of IEEE, pp. 1–6 (2012)

2. Brockers, R., Hummenberger, M., Weiss, S., Matthies, L.:Towards autonomous navigation of miniature uav. In: TheIEEE Conference on Computer Vision and Pattern Recog-nition (CVPR) Workshops (2014)

3. Li, B., Gallagher, T., Dempster, A.G., Rizos, C.: How feasi-ble is the use of magnetic field alone for indoor positioning?In: International Conference on Indoor Positioning andIndoor Navigation (IPIN), pp. 1–9 (2012)

4. Angermann, M., Frassl, M., Doniec, M., Julian, B.J.,Robertson, P.: Characterization of the indoor magnetic fieldfor applications in localization and mapping. In: Inter-national Conference on Indoor Positioning and IndoorNavigation (IPIN), pp. 1–9 (2012)

5. Bristeau, P.J., Callou, F., Vissiere, D., Petit, N.: The nav-igation and control technology inside the ar.drone microuav. In: 18th IFAC World Congress, pp. 1477–1484, Milano(2011)

6. Simon, D.: Optimal State Estimation: Kalman, H Infinity,and Nonlinear Approaches. Wiley-Interscience (2006)

7. Seung-Min, O.: Multisensor fusion for autonomous uavnavigation based on the unscented kalman filter withsequential measurement updates. In: IEEE Conference onMultisensor Fusion and Integration for Intelligent Systems(MFI), pp. 217–222 (2010)

8. Sabatelli, S., Galgani, M., Fanucci, L., Rocchi, A.: Adouble-stage kalman filter for orientation tracking withan integrated processor in 9-d imu. IEEE Trans. Instrum.Meas. 62(3), 590–598 (March 2013)

9. Faugeras, O., Luong, Q.-T.: The geometry of multipleimages: the laws that govern the formation of multipleimages of a scene and some of their applications. MITpress, Cambridge (2004)

10. Hartley, R., Zisserman, A.: Multiple View Geometry inComputer Vision. Cambridge University Press, Cambridge(2003)

11. Kaminski, J.Y., Shashua, A.: Multiple view geometry ofgeneral algebraic curves. Int. J. Comput. Vis. 56(3), 195–219 (2004)

12. Chantler, M.J., Jiahua, W.: Rotation invariant classificationof 3d surface textures using photometric stereo and surfacemagnitude spectra. In: BMVC, pp. 1–10 (2000)

13. Varma, M., Zisserman, A.: Classifying images of materi-als: Achieving viewpoint and illumination independence.In: Computer Vision. ECCV, pp. 255–271. Springer, BerlinHeidelberg New York (2002)

14. Mundy, J.L., Zisserman, A., et al.: Geometric invariance incomputer vision, vol. 92. MIT press, Cambridge (1992)

15. Forsyth, D.A., Ponce, J.: Computer vision: a modernapproach. Prentice Hall Professional Technical Reference(2002)

16. Harris, C., Stephens, M.: A combined corner and edgedetector. Alvey Vision Conf. 15, 50 (1988)

17. Baker, S., Matthews, I.: Lucas-kanade 20 years on: A uni-fying framework: Part 1: The quantity approximated, thewarp update rule, and the gradient descent approximation.Int. J. Comput. Vis. 56(3), 221–255 (2004)

18. Araguas, G., Sanchez, J., Canali, L.: Monocular visualodometry using features in the fourier domain. InVI Jornadas Argentinas de Robotica, Instituto Tec-nologico de Buenos Aires, Buenos Aires, Argentina(2010)

19. Fischler, M.A., Bolles, R.C.: Random sample consensus:a paradigm for model fitting with applications to imageanalysis and automated cartography. Commun. ACM 24(6),381–395 (1981)

20. Kuglin, C.D., Hines, D.C.: The phase correlation imagealignment method. Proc. Int. Conf. on Cybernetics andSociety 4, 163–165 (1975)

21. Zitova, B., Flusser, J.: Image registration methods: a survey.Image Vis. Comput. 21(11), 977–1000 (2003)

22. Phillips, W.F., Hailey, C.E.: Review of attitude representa-tions used for aircraft kinematics. J. Aircr. 38(4), 718–737(2001)

23. Nikolas, T., Roumeliotis, S.I.: Indirect Kalmanfilter for 3D attitude estimation. Technical Report 2005-002, University of Minnesota, Dept. of Comp. Sci. & Eng.,(2005)

24. Arulampalam, S., Maskell, S., Gordon, N., Clapp, T.: Atutorial on particle filters for online nonlinear/non-gaussianbayesian tracking. IEEE Trans. on Signal Process. 50(2),174–188 (2002)

25. Sabatelli, S., Galgani, M., Fanucci, L., Rocchi, A.: A dou-ble stage kalman filter for sensor fusion and orientationtracking in 9d imu. In: IEEE on Sensors ApplicationsSymposium (SAS), pp. 1–5 (2012)

26. Kuipers, J.B.: Quaternions and rotation sequences : aprimer with applications to orbits, aerospace, and virtualreality. Princeton Univ. Press, Princeton (1999)

27. Lee, G.H., Achtelik, M., Fraundorfer, F., Pollefeys, M.,Siegwart, R.: A benchmarking tool for mav visual poseestimation. In: 11th International Conference on ControlAutomation Robotics & Vision (ICARCV), pp. 1541–1546(2010)

28. Kelly, A.: Linearized error propagation in odometry. Int. J.Robotics Res 23(2), 179–218 (2004)

29. Nourani-Vatani, N., Roberts, J., Srinivasan, M.V.: Practicalvisual odometry for car-like vehicles. In: IEEE Interna-tional Conference on Robotics and Automation. ICRA ’09,pp. 3551–3557 (2009)