Volker Grabe
email: volker.grabe@gmail.com.

Heinrich H Bülthoff

Davide Scaramuzza
email: davide.scaramuzza@ieee.org.

Paolo Robuffo Giordano

Nonlinear Ego-Motion Estimation from Optical Flow for Online Control of a Quadrotor UAV

For the control of Unmanned Aerial Vehicles (UAVs) in GPS-denied environments, cameras have been widely exploited as main sensory modality for addressing the UAV state estimation problem. However, the use of visual information for ego-motion estimation presents several theoretical and practical difficulties, such as data association, occlusions, and lack of direct metric information when exploiting monocular cameras. In this paper, we address these issues by considering a quadrotor UAV equipped with an onboard monocular camera and an Inertial Measurement Unit (IMU). First, we propose a robust ego-motion estimation algorithm for recovering the UAV scaled linear velocity and angular velocity from optical flow by exploiting the so-called continuous homography constraint in presence of planar scenes. Then, we address the problem of retrieving the (unknown) metric scale by fusing the visual information with measurements from the onboard IMU. To this end, two different estimation strategies are proposed and critically compared: a first one exploiting the classical Extended Kalman Filter (EKF) formulation, and a second one based on a novel nonlinear estimation framework. The main advantage of the latter scheme lies in the possibility of imposing a desired transient response to the estimation error when the camera moves with a constant acceleration norm w.r.t. the observed plane. We indeed show that, when compared against the EKF on the same trajectory and sensory data, the nonlinear scheme yields considerably superior performance in terms of convergence rate and predictability of the estimation. The paper is then concluded by an extensive experimental validation, including an onboard closed-loop control of a real quadrotor UAV meant to demonstrate the robustness of our approach in real-world conditions.

I. INTRODUCTION

In recent years, inspection and search and rescue tasks have become one of the most important envisaged applications for Unmanned Aerial Vehicles (UAVs). For instance, smallsize UAVs, such as quadrotors, have been used to investigate disaster sites after earthquakes, such as Christchurch (New Zealand) and Emilia Romagna (Italy), and most prominently the Fukushima Daiichi power plant in Japan. Along similar lines, some large-scale research projects have been recently funded on these and related topics, see, e.g., [1]-[3]. Indeed, thanks to their high agility, pervasiveness and customizability, small UAVs represent an ideal robotic platform for navigating

in harsh and cluttered environments, when operating in either full autonomy or under the (partial) remote control of skilled human operators.

In all cases, a widely-recognized key component for the successful deployment of such systems is a reliable state estimation module able to deal with highly unstructured and/or GPS-denied indoor environments. This typically imposes a major requirement on the system: since the target environment for the considered applications cannot be prepared before the deployment of the vehicle, the UAV is constrained to only rely on onboard sensing and processing capabilities. These constraints have motivated, over the last years, the extensive use of onboard cameras as main sensory modality for state estimation purposes, see [START_REF] Weiss | Monocular Vision for Long-term Micro Aerial Vehicle State Estimation: A Compendium[END_REF], [START_REF] Scaramuzza | Vision-Controlled Micro Flying Robots: from System Design to Autonomous Navigation and Mapping in GPS-denied Environments[END_REF] for a recent overview. Vision indeed provides a rich sensory feedback which could, in principle, yield a full understanding of the surrounding environment. However, as well-known, an effective use of the visual information also presents many theoretical and practical difficulties. For instance, robust data extraction and association across multiple frames is often a major issue in real-world scenarios, especially when comparing images from distant points of view. Also, when relying on monocular cameras (as in most typical settings), any position/velocity information can only be retrieved up to an arbitrary scale factor, which must then be disambiguated by fusing the visual information with independent (metric) measurements from other onboard sensors. This has motivated many recent works on data fusion exploiting the concurrent (metric) measurements from onboard accelerometers embedded in the Inertia Measurement Units (IMUs) present on most flying robotic systems [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[END_REF]- [START_REF] Omari | Metric Visual-Inertial Navigation System Using Single Optical Flow Feature[END_REF].

Existing work on metric camera-based state estimation mostly relies on Simultaneous Localization and Mapping (SLAM) techniques to build and maintain a map of visual features, with the position of the vehicle in the map being estimated in parallel. Acceleration measurements are then used to reconstruct the metric scale of the underlying map [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[END_REF]. This has been achieved in [START_REF] Nützi | Fusion of IMU and Vision for Absolute Scale Estimation in Monocular SLAM[END_REF], [START_REF] Faessler | Automatic Re-Initialization and Failure Recovery for Aggressive Flight with a Monocular Vision-Based Quadrotor[END_REF] using visual-SLAM algorithms, such as PTAM [START_REF] Klein | Parallel Tracking and Mapping for Small AR Workspaces[END_REF] and SVO [START_REF] Forster | SVO: Fast Semi-Direct Monocular Visual Odometry[END_REF] respectively, and, even more remarkably, through the observation of just a single feature over time by providing a closed-form solution for the computation of the metric scale factor [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[END_REF]. However, all these approaches depend on the possibility to continuously track features for an extended period of time. Therefore, the level of robustness required for the UAV control can be hard to guarantee as the visual system can be affected by, e.g., unexpected occlusions or the need of heavy computations for a reliable feature matching. This is not the case, however, when relying on motion estimation from optical flow, as, in this case, data extraction and association is performed on consecutiveand, thus, spatially very close-by-acquired images. Motivated by these considerations, a first contribution of this paper is then the development of an approach based on optical flow decomposition for providing a reliable and robust ego-motion estimation module for safe UAV operation in unstructured environments.

Exploiting purely optical flow, a system capable of hovering and landing on a moving platform was presented in [START_REF] Herissé | Landing a VTOL Unmanned Aerial Vehicle on a Moving Platform Using Optical Flow[END_REF]. However, this work did not consider the issue of determining the unknown scene scale factor since the proposed control approach was implemented by only relying on the non-metric linear velocity directly obtained from a monocular camera. An approach combining optical flow decomposition and sensor fusion for metric scale estimation was instead presented in [START_REF] Honegger | An Open Source and Open Hardware Embedded Metric Optical Flow CMOS Camera for Indoor and Outdoor Applications[END_REF] by developing a small sensor for velocity estimation onboard UAVs. However, the proposed sensor relied on a ground facing sonar for metric scale estimation, thus limiting the vehicle to near ground operations within the range of the sonar. Finally, by exploiting an Extended Kalman Filter (EKF) to fuse optical flow measurements with IMU readings, in [START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF] the authors demonstrated the possibility of estimating the metric scale, sensor biases, and the IMU/camera relative pose. This system was later extended in [START_REF] Weiss | 4DoF Drift Free Navigation Using Inertial Cues and Optical Flow[END_REF]. However, the system was mainly designed for serving as an initialization of the PTAM framework in near hovering mode rather than for closed-loop UAV control over an extended period of time.

It can be noted that most of the presented camera-based state estimation methods rely on the classical EKF framework to fuse together the available sensor readings (e.g., from vision and IMU) for then extracting the metric scale. However, despite being widespread, the use of a EKF-based scheme presents two (often overlooked) drawbacks: (i) it necessarily involves a linearization of the (nonlinear) system dynamics (such as when dealing with visual-inertial estimation problems), and (ii) as a consequence, it does not allow for an explicit characterization of the estimation error behavior (e.g., its convergence rate). Therefore, as an additional contribution, in this paper we propose a novel visual-inertial estimation scheme exploiting optical flow and IMU measurements based on a recently-developed nonlinear observation framework for active structure from motion [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere, and Cylinder[END_REF]. Compared to a classical EKF, the use of this nonlinear filter yields an estimation error dynamics with a fully characterized convergence behavior when the camera is moving with a constant acceleration norm w.r.t. the observed scene. It is then possible, for instance, to actively impose a desired error transient response by suitably acting on the estimation gains and on the UAV motion, as well as to predict the convergence time of the estimation error in terms of percentages of the initial error. Finally, the reported results will also extensively show that the use of this nonlinear filter yields a substantial faster (and more controlled) error convergence compared to a classical and 'fully-informed' EKF, thus making it a viable and robust alternative to other consolidated schemes.

A. Summary of Contributions

Part of the work presented in this paper is based on [START_REF] Grabe | On-board Velocity Estimation and Closed-loop Control of a Quadrotor UAV based on Optical Flow[END_REF]- [START_REF]A Comparison of Scale Estimation Schemes for a Quadrotor UAV based on Optical Flow and IMU Measurements[END_REF]. In this paper, we nevertheless provide a more comprehensive overview with additional details and several extensions.

In particular, we introduce the following novel contributions: (i) a full characterization of the estimation error transient reponse for the proposed nonlinear estimation scheme used to retrieve the metric scale from optical flow and IMU measurements. As explained, this analysis also determines the conditions on estimation gains and UAV motion which then allow to impose a desired convergence behavior to the scale estimation error; (ii) an extensive set of new simulative and experimental results investigating various aspects of the proposed ego-motion estimation scheme, including robustness against noise, trade-off between noise sensitivity and amount of 'control effort' (UAV linear acceleration), as well as an experimental validation of closed-loop velocity control with all computations run onboard and in real-time on a quadrotor UAV.

The rest of the paper is then structured as follows: in Sec. II we first review the proposed ego-motion estimation algorithm from optical flow which provides an estimation of the UAV (non-metric) linear and angular velocity. Subsequently, Sec. III introduces two estimation schemes meant to recover the unknown scale factor by fusing the visual information with the IMU readings: a filter based on the standard EKF machinery, and a novel deterministic nonlinear observer. The two filters are then compared by highlighting, for the latter, the possibility to characterize and actively shape its error transient response. Afterwards, Sec. IV reports and discusses the results of several simulations and experiments aimed at validating and comparing the two ego-motion estimation approaches, and, finally, some experiments of closed-loop control on a quadrotor UAV are presented. Section V then concludes the paper and discusses some open points and future directions.

II. EGO-MOTION ESTIMATION FROM OPTICAL FLOW

The approach adopted in this work for ego-motion estimation from the perceived optical flow is based on the decomposition of the so-called continuous homography matrix [START_REF] Ma | An Invitation to 3-D Vision[END_REF], complemented with the typical measurements obtainable from an onboard IMU. A distinguishing feature of our method w.r.t. most of the previous literature is the use of a continuous approach for motion recovery. In fact, the typical incremental ego-motion estimation algorithms (e.g., the so-called visual odometry [START_REF] Scaramuzza | Visual Odometry: Part I -The First 30 Years and Fundamentals[END_REF]) assume presence of small but finite camera displacements over frames, and are thus based on reconstruction methods involving the discrete epipolar/homography constraints. However, since most cameras acquire images at high rates, we judged more appropriate to adopt a continuous point-of-view in order to recover, at each step, the camera instantaneous linear/angular velocity rather than a finite displacement over time.

In the following we summarize, for the reader's convenience, the main features of the employed ego-motion estimation method whose full details can be found in [START_REF] Grabe | On-board Velocity Estimation and Closed-loop Control of a Quadrotor UAV based on Optical Flow[END_REF], [START_REF]Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF].

A. Review of the Continuous Homography Constraint

Seen from a moving camera, the apparent velocity of a point X ∈ R 3 still in space as a result of the camera motion is

Ẋ = [ω] × X + v (1)
where v ∈ R 3 , ω ∈ R 3 are the camera linear/angular velocity (both expressed in the camera frame), and [ω] × ∈ so(3) is the skew-symmetric matrix associated to vector ω ∈ R 3 . Consider a set of point features located on a common plane of equation n T X = d where n ∈ S 2 is the unit normal vector to the plane, and d ∈ R the orthogonal distance of the plane to the camera frame. By rearranging the plane constraint as 1 d n T X = 1, eq. ( 1) becomes

Ẋ = [ω] × X + v 1 d n T X = [ω] × + 1 d vn T X = HX. (2)
Matrix H ∈ R 3×3 is commonly referred to as the continuous homography matrix: it encodes the camera linear/angular velocity (v, ω), as well as the scene structure (n, d).

Defining λx = X for a scalar depth factor λ as the image of a point X, and exploiting the fact that Ẋ = λx + λu and ẋ = u, where u is the observed velocity of the point x on the image plane, one obtains

u = Hx - λ λ x. (3)
The depth factor λ in (3) can be removed by pre-multiplication of [x] × . This results in the so-called continuous homography constraint [START_REF] Ma | An Invitation to 3-D Vision[END_REF] [x]

× Hx = [x] × u (4)
which involves the measured (x, u) and the (unknown) continuous homography matrix H.

B. The 4-Point Algorithm (Algorithm V1)

Matrix H in (4) can be recovered from a set of N measured pairs (x i , u i ) of detected features x i ∈ R 3 on the image plane and associated feature velocities u i ∈ R 3 . This can be easily obtained as follows: by stacking the elements of H into the vector

H S = [H 11 , H 21 , • • • , H 33 ] ∈ R 9 , one can rewrite (4) as a T i H S = [x i ] × u i (5)
where

a i = x i ⊗ [x i ] × ∈ R 9×3
, and ⊗ stands for the Kronecker product. By then defining

A = [a 1 , • • • , a N ] T ∈ R 3N ×9 and B = [[x 1 ] × u 1 , • • • , [x N ] × u N ] T ∈ R 3N
, one obtains the linear system

AH S = B. (6)
Assuming presence of at least N ≥ 4 measured pairs (x i , u i ), system (6) can be solved in a least-square sense as H S = A † B, with A † denoting the pseudo-inverse of matrix A.

After having recovered H, using standard techniques [START_REF] Ma | An Invitation to 3-D Vision[END_REF] it is further possible to algebraically decompose it into the scaled linear velocity v/d, the angular velocity ω, and the plane normal n. However, it can be shown that, in general, two physically-equivalent solutions are compatible with a given homography matrix H.

Thanks to this decomposition, one can then obtain the quantities (v/d, ω, n) from two consecutive visual observations and without the use of additional sensor readings or previously acquired data structures such as, e.g., a map. This first version of the ego-motion algorithm will be referred to as V1 in all the following developments.

C. Exploiting the known Angular Velocity (Algorithm V2)

Since any typical onboard IMU can directly measure the angular velocity ω IM U , we can consider ω = ω IM U as known from external (i.e., not vision-based) sources. Knowledge of ω can then be used to derotate the perceived optical flow field as

u x u y = u x u y - -x x x y 1 + x 2 x -x y -(1 + x y ) 2 x x x y x x ω, (7)
where the interaction matrix relating u to (v, ω) was exploited [START_REF] Chaumette | Visual Servo Control, Part I. Basic Approaches[END_REF]. This derotation step then reduces matrix H to

H = 1 d vn T . (8)
Since n spans H T and n = 1, we can obtain n from the singular value decomposition H = U ΣV T as the first column of matrix V . The inherent sign ambiguity can be resolved by enforcing n z > 0. Having retrieved n, we then obtain v/d = Hn.

This algorithm, which will be referred to as V2 in the following, requires the observation of at least three feature pairs (x i , u i ) and yields a unique solution for v/d and n.

D. Exploiting the known Angular Velocity and Plane Orientation (Algorithm V3)

In most indoor environments and when considering UAVs with downlooking cameras, the dominant plane can be safely taken as horizontal with, thus, its normal vector n parallel to the gravity vector. Therefore, one can exploit the ability of onboard IMUs to estimate (via internal filtering) the local gravity vector, thus allowing to consider n ≈ n IM U as measured independently from the visual input.

Plugging [START_REF] Omari | Metric Visual-Inertial Navigation System Using Single Optical Flow Feature[END_REF] in (4) yields

[x] × 1 d v = [x] × u n T x . ( 9
)
where now n T x is a known quantity. Letting δ = ([x] × u)/(n T x) ∈ R 3 , one then obtains the following equation linear in v/d (the only unknown left)

[x i ] × v d = δ i , i = 1 . . . N. (10)
A least-square approximation of v/d over all N tracked features can be obtained by stacking all

[x i ] × into the matrix Γ = [[x 1 ] × , • • • , [x N ] × ] T ∈ R 3N ×3 and all δ i into the vector ∆ = [δ 1 , • • • , δ N ] T ∈ R 3N resulting in the linear system Γ v d = ∆ (11)
which can be solved as v/d = Γ † ∆. Note that any two distinct feature point vectors x i , x j will never be parallel due to the perspective projection of the camera. Thus, in principle, only two flow vectors are required to obtain a solution for v/d. However, a more robust estimation is of course obtained by incorporating all observed flow vectors.

This third algorithm will be referred to as V3 in the following.

E. Segmentation of Features

In order to further improve the robustness of the velocity estimation system, we developed a thresholding method to test whether a set of features belongs to a common plane, and whether a new feature is a member of an existing plane [START_REF]Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF].

This method is based on the observation that the 'reprojection' vector

E = B -AH S = B -AA † B ∈ R 3N
must vanish when all observed features belong to a common plane, since [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[END_REF] and its variants admit an exact solution only in this case. Therefore, the quantity E /N (where N is the number of tracked features) can be taken as a 'planarity measure' to decide whether the observed features belong to a common plane. This approach can then be generalized to test newly tracked features against a previously known plane.

Use of this thresholding method can clearly improve the robustness of the velocity estimation algorithms presented in the previous section, since outliers (of the planarity assumption) can be more easily rejected from the maintained set of visual features. Therefore, we proposed in [START_REF]Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF] a RANSAC-based approach meant to find an initial set of features belonging to a predominant plane which is then extended as the UAV explores the scene. In parallel, the system constantly monitors the set of rejected features to detect new or more consistent planes in the environment. To maintain the quality of the feature set, the agreement of each feature with the main segmented plane is also constantly monitored, with outliers being removed as the camera traverses the environment.

Using this filtering technique, we showed in [START_REF]Robust Optical-Flow Based Self-Motion Estimation for a Quadrotor UAV[END_REF] that the estimation of the scaled linear velocity v/d could be improved by a factor of 25% in presence of a dominant ground plane partially covered by additional objects compared to an unfiltered version of the algorithm. This then validated the use of the proposed ego-motion estimation algorithms (which are based on the continuous homography constraint) also in non-perfectly planar scenes as those one can typically find in indoor environments.

F. Final Discussion

We conclude with some final considerations about the choice of basing the proposed ego-motion estimation algorithm upon the homography constraint instead of the other well-known epipolar constraint often exploited in computer and robotic vision. In general, the applicability of the homography vs. epipolar constraint depends on the structure of the environment: methods relying on the epipolar constraint are more appropriate for highly unstructured scenes where a collection of points in general position can always be detected 1 . On the other hand, solutions based on the homography constraint should be favored when dealing with approximately planar scenes. Indeed, in the ideal case of features extracted from a perfect planar scene, the epipolar constraint 'loses rank' and one cannot obtain any longer a unique solution (up to a scalar factor) for the ego-motion recovery [START_REF] Ma | An Invitation to 3-D Vision[END_REF]. The homography constraint, on the other hand, allows to correctly address the case of planar scenes (which is the case considered in this paper).

Furthermore, compared to epipolar-based algorithms, the homography constraint remains better conditioned also in case of stationary flight, e.g., with a small amount of translational motion (indeed, the epipolar constraint vanishes for a zero translation/linear velocity). Therefore, while the epipolar constraint can be more suited for persistently-translating maneuvers or vehicles (e.g., fixed-wing), the homography constraint ought to be more robust for typical indoor (close to stationary) flight regimes of vertical take-off and landing vehicles such as quadrotor UAVs.

These considerations then motivated our choice of the homography constraint for dealing with the ego-motion recovery. Indeed, as explained, in many scenarios, such as indoor hallways or areal coverage, one can safely assume presence of a dominant ground plane spanning most of the observed features. Coupled with the RANSAC-based outlier rejection described in the previous section, our proposed solution then proved to yield accurate results as it will be discussed in the next Sect. IV-B.

III. ONLINE SCALE ESTIMATION

The ego-motion algorithm (in its three variants) presented in the previous section allows to obtain a reliable estimation of the scaled camera linear velocity v/d. In this section, we discuss two estimation schemes meant to recover the plane distance d by fusing the optical flow decomposition with the onboard accelerometer measurements.

A. Equations of Motion

We start deriving the equations of motion relevant to the case under consideration. In the following, we will denote with B, C, I and W the body, camera, IMU and inertial world frames, respectively. The origin of frame B is assumed to be located at the quadrotor barycenter, while frames C and I are supposed to be rigidly attached to B, see Fig. 1. Throughout the text, left superscripts will be exploited to indicate the frames where quantities are expressed in. The symbol X R Y ∈ SO(3) will be used to denote the rotation matrix from frame X to frame Y, and Z p X Y ∈ R 3 to represent the vector from the origin of frame X to the origin of frame Y, expressed in frame Z. We also introduce the following quantities instrumental for the next developments: g ∈ R 3 as the gravity vector, and I f ∈ R 3 , I ω ∈ R 3 as the specific acceleration and angular velocity at the origin of I.

Define C v = C R W W ṗWC as the camera linear velocity in camera frame, and B v = B R W W ṗWB as the body linear velocity in body frame. Since B R C , B R I , B p BC and B p BI are assumed constant, from standard kinematics the following relationships hold

B v = B RC( C v + [ C ω]× C p CB ) = B RC C v + [ B ω]× B p CB , (12) C v = C RI( I a + [ I ω]× I p IC + [ I ω] 2 × I p IC ) -[ C ω]× C v = C RI I a + [ C ω]× C p IC + [ C ω] 2 × C p IC -[ C ω]× C v = C κ -[ C ω]× C v (13)
C ω = C RI I ω (14) C ω = C RI I ω ( 15
)
where I a = I R W W pWI is the linear acceleration experienced by the IMU and

C κ = C R I I a + [ C ω] × C p IC + [ C ω] 2 × C p IC (16)
is the camera linear acceleration w.r.t. W. We note that I a = I f + I g and I g = I R W [0, 0, g] T in case of a horizontal orientation of the world frame, see Fig. 1.

In presence of a planar scene C n T C P + d = 0 one also has (see, e.g., [START_REF] Giordano | 3D Structure Identification from Image Moments[END_REF])

C ṅ = -[ C ω] × C n (17) ḋ = C v T C n. (18)
Finally, according to this notation, the decomposition of the optical flow summarized in the previous Section allows to directly measure the scaled linear velocity C ṽ = C v/d. The estimation schemes presented in the following are then meant to recover the (unmeasurable) value of the plane distance d and the metric linear velocity vector C v.

B. Scale Ambiguity

As in any monocular Structure from Motion (SfM) problem, the inherent scale ambiguity of the observed scene can be resolved only if the camera motion is sufficiently exciting w.r.t. the estimation task. For instance, the depth of a point feature can be recovered only if the camera travels with a non-zero linear velocity not aligned with the projection ray of the observed point, and other constraints exist for different geometrical primitives [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere, and Cylinder[END_REF].

In the case under consideration, a camera traveling with zero linear acceleration ( C κ = 0) does not allow to infer the plane distance d and the linear velocity C v from the measured C v/d: an accelerated motion w.r.t. the observed plane is necessary in order to recover the correct pair ( C v, d). This (known) requirement can be briefly justified as follows: by setting C κ = 0, the dynamics ( 13)-( 18) reduce to

C v = -[ C ω] × C v ḋ = C v T C n , (19)
that is, an expression homogeneous in the pair

( C v, d).
Therefore, given a solution

( C v * (t), d * (t)) of system (19), any other pair (λ C v * (t), λd * (t)
) is also a valid solution, with λ ∈ R being an arbitrary scale factor. The measured output C v/d, however, does not depend on the scale factor λ: hence, whatever the adopted filter, a non-accelerating camera cannot disambiguate the correct pair ( C v, d) from the perceived optical flow as any other pair (λ C v(t), λd(t)) would be equally consistent with the dynamics ( 19) and the measured C v/d. On the other hand, presence of a non-zero (and known) linear acceleration C κ = 0 breaks the homogeneity of ( 19) w.r.t. ( C v, d) and makes it possible to recover the correct scale factor λ during motion. Section III-D will revisit these considerations in the context of Persistency of Excitation of the camera motion during the estimation task.

C. Scale Estimation based on the Extended Kalman Filter

As a first approach to estimate the distance to the planar scene d, we develop a classical EKF upon the equations of motion of the system. In particular, we adopt the discrete version of the EKF, and let index k ∈ N denote the k-th iteration step. For clarity, we will append a right subscript m to identify all those quantities that are directly available through one of the onboard sensors, e.g., specific force I f m and angular velocity I ω m from the IMU, and the scaled linear velocity C ṽm = ( C v/d) m from the camera.

We define the EKF state vector x to consist of the metric camera linear velocity in camera frame C v and the inverse ρ = 1/d of the camera distance to the planar scene:

x = C v ρ , C v ∈ R 3 , ρ ∈ R. ( 20
)
The choice of taking an inverse parameterization ρ = 1/d (in place of just d) for the scene scale is indeed known to yield better results in the context of structure estimation from motion, see, e.g., [START_REF] De Luca | Feature Depth Observation for Image-based Visual Servoing: Theory and Experiments[END_REF], [START_REF] Civera | Inverse Depth Parametrization for Monocular SLAM[END_REF]. This inverse parameterization will also be instrumental in the design of the nonlinear observer of the next Sec. III-D.

Since the onboard IMU directly outputs the specific acceleration I f m and and angular velocity I ω m in its own frame 2 , we rewrite (13) as:

C v = C R I ( I a + [ I ω] × I p IC + [ I ω m ] 2 × I p IC ) -[ C ω m ] × C v ≈ C R I ( I f m + I g + [ I ω m ] 2 × I p IC ) -[ C ω m ] × C v. (21)
As no direct measurement of I ω is possible on most quadrotor setups, and being I ω usually a noisy signal, we approximate I ω ≈ 0 in (21) rather than attempting to recover I ω via a numerical differentiation. Exploiting [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere, and Cylinder[END_REF], the dynamics of ρ is instead given by ρ = -ḋ

d 2 = -ρ 2 C v T C n. (22)
Consequently, the following equations govern the predicted state x[k] in terms of the previous estimated state x[k -1]:

C v[k] = C v[k -1] + T C v[k -1] (23) ρ[k] = ρ[k -1] -T ρ2 [k -1] C v[k -1] T C n[k -1] (24)
where T denotes the sampling time of the filter.

Although most quantities derived in the following steps are time varying, from now on, for the sake of exposition clarity, we will omit the time dependency [k] wherever possible.

To compute the predicted covariance matrix of the system uncertainty Σ

[k] ∈ R 4×4 , we first derive the Jacobian matrix G[k] ∈ R 4×4 G =      ∂ C v[k] ∂ C v[k -1] ∂ C v[k] ∂ ρ[k -1] ∂ ρ[k] ∂ C v[k -1] ∂ ρ[k] ∂ ρ[k -1]      = I 3 -T [ I ω m ] × 0 3×1 -T ρ2 C n T 1 -2T ρ C vT C n . ( 25
)
Matrix Σ[k -1] from the previous step is then propagated as:

Σ[k] = G Σ[k -1]G T + R. (26)
Here, matrix R ∈ R 4×4 is obtained from

R = V cov( I f m ) 0 3×3 0 3×3 cov( I ω m ) V T (27)
where

V =      ∂ C v[k] ∂ I f m ∂ C v[k] ∂ I ω m ∂ ρ[k] ∂ I f m ∂ ρ[k] ∂ I ω m      = T C R I T ( C R I M + [ C v] × C R I ) 0 1×3 0 1×3 ∈ R 4×6 (28) M = ( I ω T m I p IC )I 3 + I ω m I p T IC -2 I p IC I ω T m , (29)
and cov( I f m ) ∈ R 3×3 , cov( I ω m ) ∈ R 3×3 are the covariance matrixes of the accelerometers/gyroscopes sensors in the IMU, which can be experimentally determined.

The predicted state x is then updated whenever a new scaled visual velocity estimate z m = C ṽm = C v/d m becomes available from the optical flow decomposition. Let z be the predicted scaled visual velocity estimation based on the predicted state

x z = C v ρ. (30)
The Kalman gain K ∈ R 4×3 is obtained as

K = ΣJ T (J ΣJ T + cov(z m )) -1 , (31)
where cov(z m ) ∈ R 3×3 is the covariance matrix of the scaled visual velocity measurement, and the Jacobian J ∈ R 3×4 is given by

J = ∂ z[k] ∂ C v[k] ∂ z[k] ∂ ρ[k] = I 3 ρ C v . (32)
Finally, the predicted state x[k] is updated to the estimated state x[k], together with uncertainty matrix Σ[k], as

x = C v ρ = C v ρ + K (z m -z) (33)
Σ = (I 4 -KJ ) Σ. ( 34
)
1) Discussion: Several quantities are needed for the implementation of the proposed EKF. Apart from the estimated state x[k], one needs knowledge of:

1 the constant IMU/camera rotation matrix I R C and displacement vector I p IC ; 2 the IMU angular velocity I ω m ; 3 the scaled camera linear velocity C ṽm = ( C v/d) m ; 4 the IMU linear acceleration I a = I f m + I g; 5 the plane normal C n. The quantities in item 1 are assumed to be known from a preliminary IMU/camera calibration phase, see, e.g., [START_REF] Lobo | Relative Pose Calibration Between Visual and Inertial Sensors[END_REF], while vector I ω in item 2 is available directly from the IMU gyroscope readings. Similarly, vector C ṽm in item 3 is retrieved from the optical flow decomposition described in Sec. II.

Measurement of the linear acceleration I a in item 3 requires the specific acceleration I f m (directly available through the IMU accelerometer readings) and knowledge of the gravity vector I g in IMU frame. An estimation of this latter quantity is also provided by standard IMUs in near-hovering conditions, a fact largely exploited when recovering the UAV attitude from onboard sensing, see, e.g., [START_REF] Mahony | Nonlinear Complementary Filters on the Special Orthogonal Group[END_REF]. In our case, we found the employed IMU able to provide a reliable estimation of I g even when undergoing moderate accelerations thanks to its internal filtering scheme. An experimental validation of these observations is provided in Sec. IV-D.

Finally, C n can be directly recovered from the optical flow decomposition as discussed in Sects. II-B-II-C (this step can also be complemented by the use of filtering techniques such as those discussed in [START_REF] Eudes | Visuo-inertial fusion for homography-based filtering and estimation[END_REF]), or, in case of a horizontal planar scene as often found in indoor environments, by identifying its direction with that of I g. This latter possibility was exploited in all the experiments reported in Sec. IV, which indeed involved a plane normal C n parallel to g (horizontal ground plane).

D. Scale Estimation based on a Nonlinear Estimation Scheme

In this Section, we summarize the derivations of the alternative nonlinear observer for retrieving the plane distance d based on a framework originally proposed in [START_REF] Giordano | 3D Structure Identification from Image Moments[END_REF], [START_REF] De Luca | Feature Depth Observation for Image-based Visual Servoing: Theory and Experiments[END_REF] for dealing with Structure from Motion (SfM) problems, and recently revisited in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], [START_REF] Spica | Active Structure from Motion: Application to Point, Sphere, and Cylinder[END_REF] in the context of nonlinear active estimation. Compared to the previously discussed EKF, this estimation scheme does not require any linearization/approximation step of the system dynamics. This results in an overall cleaner design which, following the analysis reported in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], also allows for a full characterization of the estimation error transient response in case of a camera traveling with a constant linear acceleration norm ( C κ(t) = const). In particular, we will discuss how one can impose to the estimation error a transient response equivalent to that of reference linear second-order system with desired poles by suitably acting on the estimation gains and on the UAV acceleration. This possibility enables the designer, for instance, to choose the needed combination of estimation gains/UAV motion yielding a desired estimation performance, as well as to predict in advance the convergence time of the filter in terms of percentages of the initial error 3 . However, we also note that, as opposed to the EKF, the filter design assumes deterministic dynamics. Therefore, it does not explicitly account for the noise present in the system concerning state prediction and sensor measurements. This point will be thoroughly addressed in the next Sec. IV.

For the sake of exposition, the following developments are here formulated in continuous time. We first recall a classical result of the adaptive control literature, also known as the Persistency of Excitation Lemma [START_REF] Marino | Nonlinear Control Design: Geometric, Adaptive and Robust[END_REF], and upon which the next developments are based.

Lemma 1 (Persistency of Excitation):

Consider the system ξ = -Dξ + Ω T (t)z, ż = -ΛΩ(t)P ξ, (35)
where ξ ∈ R m , z ∈ R p , D > 0, P = P T > 0 such that

D T P + P D = Q, with Q > 0,
and

Λ = Λ T > 0.
If Ω(t) , Ω(t) are uniformly bounded and the persistency of excitation condition is satisfied, i.e., there exist a T > 0 and γ > 0 such that

t+T t Ω(τ )Ω T (τ )dτ ≥ γI > 0, ∀t ≥ t 0 , (36)
then (ξ, z) = (0, 0) is a globally exponentially stable equilibrium point. This result can be exploited as follows: assume a vector

x = [x T 1 x T 2 ] T ∈ R m+p can be split into a measurable component x 1 and an unmeasurable component x 2 . Defining an estimation vector x = [x T 1 xT 2 ] T ∈ R m+p ,

and the corresponding estimation error

e = ξ z = x 1 -x1 x 2 -x2 ,
the goal is to design an update rule for x such that the closedloop error dynamics matches formulation [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF]. When this manipulation is possible, Lemma 1 ensures global exponential convergence of the estimation error e = [ξ T z T ] T to 0, thus allowing to infer the unmeasurable value of x 2 from knowledge of x 1 . The PE condition (36) plays the role of an observability constraint: estimation of x 2 is possible iff matrix Ω(t) ∈ R p×m is sufficiently exciting over time in the sense of (36). We finally note that, being Ω(t) a generic (but known) time-varying quantity, formulation [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF] is not restricted to only span the class of linear systems, but it can easily accommodate nonlinear terms as long as they are embedded in matrix Ω(t).

We now detail how to tailor [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF] to the case under consideration. We start by defining (similarly to what done in Sec. III-C)

x 2 = ρ = 1 d (37)
and

x 1 = C ṽ = C v d = C vx 2 (38)
with, therefore, m = 3 and p = 1. Exploiting ( 22), the dynamics of x 2 is given by

ẋ2 = -x 2 2 C v T C n = -x 2 x T 1 C n. ( 39
)
As for the dynamics of x 1 , using [START_REF] Herissé | Landing a VTOL Unmanned Aerial Vehicle on a Moving Platform Using Optical Flow[END_REF] we have

ẋ1 = C vx 2 + C v ẋ2 = C vx 2 -C vx 2 x T 1 C n = C vx 2 -x 1 x T 1 C n = C κx 2 -[ C ω] × x 1 -x 1 x T 1 C n. (40)
We then define

Ω T (t) = C κ = C R I I a + [ C ω] 2 × C p IC + [ C ω] × C p IC ≈ C R I I a + [ C ω] 2 × C p IC (41)
where, analogously to the EKF case, I ω is neglected since no direct measurement of the UAV angular acceleration is available onboard. With these settings, the update rule for the estimated state x can be designed as

ẋ1 = Ω T (t)x 2 -[ C ω] × x 1 -x 1 x T 1 C n + Dξ ẋ2 = -x 2 x T 1 C n + ΛΩ(t)ξ (42)
with D > 0 and Λ > 0 being symmetric and positive definite gain matrixes. Note that (42) involves only measured and estimated quantities, including a feedback action on ξ = x 1 -x1 , i.e., the measurable component of the error vector. With this choice, the dynamics of the estimation error e = [ξ

T z T ] T then becomes ξ = -Dξ + Ω T (t)z ż = -ΛΩ(t)ξ -zx T 1 C n. (43)
It is easy to verify that, by letting P = I 3 , formulation [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF] is almost fully recovered apart from the spurious scalar term

g(e, t) = -zx T 1 C n (44)
in (43). Nevertheless, exponential convergence of the estimation error e(t) to 0 can still be proven by resorting to Lyapunov theory and by noting that the spurious term g(e, t) is a vanishing perturbation of an otherwise globally exponentially stable nominal system, i.e., g(0, t) = 0, ∀t. We refer the reader to [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF], [START_REF] Giordano | 3D Structure Identification from Image Moments[END_REF], [START_REF] De Luca | Feature Depth Observation for Image-based Visual Servoing: Theory and Experiments[END_REF] for additional discussion and proofs of these facts. We note that the design of observer (42) did not require any linearization step as for the previous EKF thanks to the more general class of (nonlinear) systems spanned by formulation [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF]. It is also worth analyzing, in our specific case, the meaning of the PE condition (36) necessary for obtaining a converging estimation. Being Ω T (t) ∈ R 3 a vector, condition (36) requires that Ω(t) (i.e., at least one component) does not ultimately vanish over time. On the other hand, vector Ω T (t) represents the camera linear acceleration through space w.r.t. the inertial world frame W, see (41). Therefore, we recover the requirement of Sect. III-B stating that estimation of d is possible if and only if the camera undergoes a physical acceleration w.r.t. the observed plane, and, consequently, moving at constant velocity w.r.t. W cannot allow the estimation to converge.

Finally, as done in the previous Sec. III-C1, we list the quantities necessary for implementing the proposed observer (42). In addition to the estimated state x, these are:

1 the constant IMU/camera rotation matrix I R C and displacement vector I p IC ; 2 the IMU angular velocity I ω m ; 3 the scaled camera linear velocity x 1 = C ṽm ; 4 the IMU linear acceleration I a = I f m + I g; 5 the plane normal C n. Thus, the same quantities discussed in Sec. III-C1 are also required for the nonlinear estimation scheme (42).

1) Shaping the Estimation Transient Response: We now apply the theoretical analysis developed in [START_REF] Spica | A Framework for Active Estimation: Application to Structure from Motion[END_REF] to the case at hand, and aimed at characterizing the transient response of the error system (43) in the unperturbed case, i.e., with g(e, t) = 0.

Let U ΣV T = Ω be the singular value decomposition of matrix Ω where Σ = [S 0] ∈ R p×m , S = diag(σ i ) ∈ R p×p , and 0 ≤ σ 1 ≤ . . . ≤ σ p are the singular values of Ω. In the case under consideration (p = 1, m = 3), it is U = 1 and σ 1 = Ω . Let also Λ = αI, α > 0 (scalar gain Λ). By then designing the gain D ∈ R 3×3 as

D = V D 1 0 0 D 2 V T (45) with D 1 ∈ R > 0 and D 2 ∈ R 2×2 > 0, it is possible to show that, under the change of coordinates η = 1 √ ασ 1 z (46)
and in the approximation σ 1 (t) = Ω(t) ≈ const, the estimation error z(t) (when expressed in the coordinates η(t)) obeys the following second-order linear dynamics

η = -D 1 η -ασ 2 1 η, (47)
that is, a (unit-)mass-spring-damper system with stiffness ασ 2 1 and damping D 1 .

The transient response of the estimation error z(t) = x 2 (t)-x2 (t) = 1/d(t) -1/ d(t) can then be imposed by properly 'placing the poles' of system (47). This can be achieved by:

1) taking D 1 = 2
√ ασ 1 so as to obtain a critically-damped state evolution for (47) (real and coincident poles). Note that this choice univocally determines D 1 as a function of the system state (the current value of σ 1 = Ω ); 2) actively enforcing ασ 2 1 (t) = α Ω(t) 2 = σ 2 d = const over time for some desired σ 2 d = 0, that is, by flying with a given non-zero and constant norm of the camera linear acceleration Ω scaled by gain α (a free parameter whose role will be better detailed in the following). From standard linear system theory, these choices then result in the following behavior for the estimation error z(t) = 1/d(t)-1/ d(t)

z(t) = (1 + σ d (t -t 0 ))e -σ d (t-t0) z(t 0 ) ∀t ≥ t 0 . ( 48
)
We conclude noting that, in general, equation (48) represents a reference evolution for z(t) since (i) in real-word conditions it may be hard to maintain exactly a Ω(t) = const over time (condition also needed to render exactly valid the change of coordinates ( 46)), and (ii) this analysis omits the effect of the (vanishing) disturbance g(e, t) in (44). Nevertheless, Sec. IV will show an excellent match between the reported results and the reference behavior (48).

Finally, we comment about the role of gain α (the only free parameter of the nonlinear observer): being σ d = √ α Ω , the same convergence rate for z(t) (dictated by the value of σ d , see (48)) can be equivalently obtained by either accelerating faster or by increasing gain α. While increasing gain α may always appear more convenient in terms of reduced control effort, practical issues such as noise, discretization or quantization errors, may impose an upper limit on the possible value of α, thus necessarily requiring a larger Ω (larger acceleration norm) for obtaining the desired convergence speed. More details about this point will be presented in the next Sec. IV.

IV. EXPERIMENTAL EVALUATION

This section reports the results of several experiments meant to illustrate and validate the proposed framework for egomotion estimation on simulated and real scenarios. After the description of the experimental setup in Sec. IV-A, we replicate the structure of the previous sections by first comparing in Sec. IV-B the three different variants discussed in Sec. II for retrieving the scaled linear and angular velocity from the optical flow decomposition. Then, we proceed to present in Sec. IV-C a series of experiments involving the two scale estimation techniques illustrated in Sec. III. An assessment of the ability of the employed IMU in estimating the gravity direction while the quadrotor undergoes the accelerations needed to recover the unknown scale is also presented in Sec. IV-D. Further considerations about the possibility of predicting (and imposing) an estimation transient behavior for the nonlinear observer of Sec. III-D, and about its robustness against noise, are then discussed in Sec. IV-E and IV-F, respectively. The experimental evaluation is then finally concluded in Sec. IV-G by reporting the results of a closed-loop velocity control on a real quadrotor UAV with all the computations performed onboard.

A. Experimental Setup

For our experiments, we made use of a quadrotor UAV from MikroKopter as experimental platform, and of the free TeleKyb software framework [START_REF] Grabe | The TeleKyb Framework for a Modular and Extendible ROS-based Quadrotor Control[END_REF] as middleware and control software. An external motion tracking system (Vicon4 ) was employed for obtaining the ground truth data and for allowing closedloop control of the UAV in all the following experiments apart from the results reported in Sec. IV-G where autonomous flight was instead achieved without any external aid.

Figures 12depict the location of all relevant sensors and frames. The quadrotor was equipped with an additional 3DM-GX3-25 IMU from MicroStrain (frame I) to provide the measurements of the specific acceleration I f m , of the angular velocity I ω m , and of the gravity vector I g m at 200 Hz. The gravitational vector was estimated internally by the IMU via fusion of the measurements from the accelerometer, rate gyroscopes, temperature sensor and a 3D magnetometer. The visual input was provided by a precalibrated MatrixVision mvBlueFox camera (frame C) mounted downfacing on the UAV, and delivering images of size 752 × 480 pixel. The onboard processing was carried out on a small PC with an Intel Atom processing unit, while velocity command inputs were transmitted to the vehicle using a wireless serial interface. When processing recorded data offline on a desktop computer, the optical flow could be decomposed at 50 Hz. When, instead, relying on the onboard processing unit (as in the experiments of Sec. IV-G), the acquired images were processed online at a frame rate of 17 Hz. In this case, we also found that, in average, the images were delayed by 100 ms w.r.t. the IMU measurements. In order to compensate for this effect, we delayed the processing of the IMU readings accordingly. In order to allow for an efficient feature tracking, the point features were initially detected using a FAST tracker [START_REF] Rosten | Fusing points and lines for high performance tracking[END_REF]. An efficient pyramidal implementation of the Lucas-Kanade algorithm [START_REF] Lucas | An Iterative Image Registration Technique with an Application to Stereo Vision[END_REF] was then used to track the features over all consecutive images. For further guaranteeing a minimum number of observed features while avoiding unnecessary and costly feature re-sampling, the point features were only sampled once the set of currently tracked features did drop below a given threshold. New FAST features were then detected and added to the set until a desired maximum set size was reached.

B. Comparison of Algorithms V1-V3 for Ego-Motion Estimation from Optical Flow

We start presenting a comparison of the three possibilities discussed in Sec. II-B-II-D for recovering v/d and ω during flight. For the reader's convenience, we recall that algorithm V1 (Sec. II-B) is only based on the perceived optical flow for obtaining (v/d, ω), algorithm V2 (Sec. II-C) exploits the additional (independent) measurement of ω from the onboard gyros, and algorithm V3 (Sec. II-D) further exploits the independent knowledge of the scene normal n taken coincident with the gravity vector.

To allow for a controlled and direct comparison of the three possibilities V1-V3, we recorded data while flying along a circular trajectory of 2 m in diameter as shown in Fig. 3. The trajectory was chosen in order to have the UAV accelerating sinusoidally along the three Cartesian directions with a period of 10 s, and a maximum speed and acceleration of 0.6 m/s and 0.296 m/s 2 , respectively. The height varied from 0.5 m to 1.5 m along the trajectory. Additionally, the vehicle was periodically rotated around its body z-axis in the range of [-70 . . . 70] deg. The quadrotor relied on an external motion tracking system to track this precomputed trajectory. Onboard hardware was used to record vision and IMU data during flight. Afterwards, all three algorithms of Sec. II-B-II-D were run on this common dataset to allow for a direct comparison.

Figure 4 summarizes the results of this first experiment. The plots in Figs. 4a-4c show a superimposition of the three estimations of v against the ground truth provided by the external tracking system. In all three cases, the linear velocity v was recovered from the estimated v/d scaled by the ground truth measurement of d. Figure 4d then reports the corresponding error norms.

From these results we can conclude that all the three algorithms V1-V3 yield a satisfactory performance. However, one can also note how the integration of the IMU measurements (i.e., algorithms V2-V3) reduces the mean error of the linear velocity estimation compared to algorithm V1, see Table I. A comparison of the angular velocity ω as retrieved from the visual system (algorithm V1) against the IMU readings and the ground truth of the external tracking system is given in Figs. 4e-4f, with Fig. 4g reporting the corresponding error norm w.r.t. the ground truth. We can note how the IMU provides a less noisy estimate of ω almost free of spikes, while the camera-based estimation is less accurate during low altitude flights.

Figure 4h indicates the origin of the irregularities in the recovered ω. Since the vehicle is moving with a constant speed in world frame, v/d increases with lower altitudes. Therefore, in this case the point features tend to vanish faster from the field of view and can only be tracked over a shorter amount of time. In particular, at 0.5 m altitude and under the given experimental conditions, motion blur starts to affect the tracking and feature re-sampling considerably. Therefore, the system can only maintain a set of 30 to 65 features instead of the, in this case, intended 50 to 100 with a negative effect on the retrieved ω. On the other hand, the estimated v/d results almost insensitive (in the algorithms V2 and V3) w.r.t. the number of tracked features as can be seen in Figs. 4a-4c: here, the estimated v/d in the V2 and V3 cases tracks very well the ground truth despite the various drops in the number of tracked features (Fig. 4h). Table I finally provides a quantitative summary of the reported results. One can again notice the improvements obtained when fusing the decomposition of the perceived optical flow with measurements from the IMU with, in particular, algorithm V3 performing slightly better than V2.

In view of these considerations, all the following results were obtained by relying on algorithm V2 for retrieving v/d from the decomposition of the optical flow, with ω instead provided by the onboard IMU. This choice is motivated by the fact that algorithm V2 grants a performance comparable with algorithm V3 but under milder assumptions on the surrounding environment (no need of a horizontal dominant plane and of an accurate sensing of the gravity direction).

C. Comparison of the two Scale Estimation Schemes

In the following, the two scale estimation schemes of Sec. III-C and Sec. III-D are first evaluated in simulation to compare their performance in ideal conditions. Subsequently, the results obtained on recoded sensor data from real flights are presented. We also compare our solution against a recent related work by qualitatively reproducing the experiments presented in [START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF].

Before starting the discussion, we note that each filter allows for two distinct possibilities for retrieving the metric linear velocity v. Indeed, a first possibility is to just set v = (v/d) m d exploiting the estimated d and the measured (v/d) m from the optical flow decomposition. However, a second possibility is to exploit the internal estimation of v maintained by both filters, that is v from (33) in the EKF case, and v = x1 /x 2 for the nonlinear observer case (see Sect. III-D). Both possibilities have their pros/cons: in general the second possibility may result in a less noisy but also potentially more delayed estimation of v (because of the 'filtering' action of both estimation schemes). Nevertheless, for the sake of obtaining a less noisy estimate, all the following simulation and experiment results rely on this second possibility. 1) Simulated Data: in order to illustrate the convergence of the proposed estimation schemes, we generated a synthetic acceleration profile together with the corresponding (simulated) sensor readings. This resulted in a camera motion similar to the horizontal circular trajectory used for the experimental results of the next Sec. IV-C2, with an associated constant acceleration norm Ω ≈ 0.296 m/s 2 . All generated sensor readings were perturbed with an additive zero-mean Gaussian noise with covariance matrices taken from the real sensor characteristics: 0.00003 I 3 m/s 2 , 0.00002 I 3 rad/s and 0.00005 I 3 1/s for I f m , I ω m , and for the non-metric linear velocity from optical flow ( C v/d) m , respectively. The same covariance matrices were employed in the steps ( 27)-( 31) of the EKF. As for the nonlinear observer, its only free parameter (gain α) was taken as α = 12: this value was chosen so as to obtain, for both filters, the same noise level (that is, variance of the estimation error) in the estimated d(t) after convergence. This was meant to ensure a 'fair' comparison among the EKF and the nonlinear observer in terms of estimation accuracy. The choice of α = 12 then resulted in a value of σ d = √ α Ω ≈ 1.025 for the ideal estimation error convergence in (48).

In order to test the robustness against an unknown scene depth, the initial estimation of the plane distance was chosen as d(t 0 ) = 5 m for both filters against a real distance of d(t 0 ) = 1 m. Furthermore, in the EKF case the initial C v(t 0 ) was taken as C v(t 0 ) = ṽm (t 0 ) d(t 0 ) (i.e., exploiting the first measurement of ṽm = ( C v/d) m scaled by the initial estimation d(t 0 )). In the nonlinear observer case, we instead directly set x1 (t 0 ) = ṽm (t 0 ) (see ( 38)). Finally, the EKF was initialized with Σ(t 0 ) = I 4 for representing the initial level of uncertainty, although we will later show that the EKF performance is basically insensitive to the initial choice of Σ(t 0 ).

Figure 5a reports a comparison of the estimation performance of both the EKF and the nonlinear observer run in parallel on the same UAV trajectory and sensor readings, together with the ground truth for the plane distance d. One can then appreciate the faster convergence of the estimation error for the nonlinear observer case w.r.t. the 'fully-informed' EKF (in the sense of being aware of the exact noise characteristics). This better performance can be ascribed to the lack of linearization of the system dynamics in the nonlinear observer case.

Furthermore, Fig. 5b shows the behavior of the estimation error z(t) = 1/d(t) -1/ d(t) for both filters, with superimposed the 'ideal' transient response (48). We can then note the very good match of this latter ideal response with the behavior of the estimation error z(t) for the nonlinear observer case, thus confirming the theoretical analysis of Sec. III-D. The plot also shows a horizontal band at 10 % of the initial error: according to the ideal reponse (48), the estimation error of the nonlinear observer should have dropped below 10 % of the initial error z(t 0 ) after 3.79 s. The result in Fig. 5b is slightly off this prediction because of the presence of the perturbation term g(z, t) in (44) (also shown in the plot). Indeed, presence of this term (neglected in the analysis) initially slows down the convergence rate of d(t) w.r.t. its predicted behavior. Note that, ideally, one should have had g(z, t) = 0 in this case, since v was always perpendicular to the plane normal n (see again (44)). However, the simulated noisy measurements resulted in the presence of a small v z which gave rise to the initial non-null value of g(z, t). Additional details about the possibility of predicting the convergence time of the nonlinear observer are also given in Sec. IV-E. The linear velocities estimated by both observers are shown in comparison in Fig. 6. As with the estimation of the scale factor d, the estimation for the metric velocity v converges faster in the nonlinear observer case. However, both scale estimation schemes provide very reliable velocity measurements after 20 s as shown in Fig. 7 where the norm of the velocity estimation errors are reported. Finally, Fig. 8 shows the behavior of the distance d(t) estimated by the EKF when initializing the state covariance Σ(t 0 ) with values between Σ(t 0 ) = 10 -6 I 4 and Σ(t 0 ) = 10 provided by the nonlinear observer. One can then verify how the EKF convergence is always slower than in the nonlinear observer case, and how any 'reasonable' initial Σ(t 0 ) has only a small effect on the EKF performance.

Table II finally summarizes the quantitative results for the comparison of both scale estimation approaches. The reported Root Mean Square (RMS) error for both systems was computed after 20 sec. of flight, i.e., after both estimates have converged.

2) Recorded Data: the two scale estimation approaches were also compared by processing a dataset collected during a real quadrotor flight. The chosen trajectory was similar to the circular one used in Sec. IV-B but rotated on a horizontal plane in order to yield a constant height during motion. The chosen trajectory was again characterized by an acceleration of Ω ≈ 0.296 m/s 2 , thus resulting in a σ d = √ α Ω ≈ 1.025 m/s 2 for the ideal system (48). However, on the real quadrotor the actual accelerations were found slightly higher than those expected from the ideal circular trajectory due to additional constant regulations necessary to overcome small real-world disturbances. All sensor offsets were calibrated before the recording of the dataset, and the covariance matrices of I f m , I ω m and ( C v/d) m were estimated over a period of 60 s. Both filters were initialized as described in the previous Sec. IV-C1, thus with an initial d(t 0 ) = 5 m against a real height of d(t 0 ) ≈ 1 m. The gain for the nonlinear observer was again selected as α = 12 to ensure a similar noise level for the scale estimation from the EKF and the nonlinear observer after convergence. Figure 9a shows the behavior of the estimated plane distance d(t) for both filters, with superimposed the ground truth d(t).

The behavior of the estimation error z(t) = 1/d(t) -1/ d(t) is shown in Fig. 9b, and the behavior of d(t) -d(t) is also reported in Fig. 9c for a better appreciatoin of the convergence behavior. Again, from these plots one can note the good match among the estimation error of the nonlinear observer against the ideal response (48). On this dataset from real sensor measurements, the mismatch between the predicted and actual convergence rate of the nonlinear estimator was smaller than in the previous case of simulated data. This can be explained by the fact that the real acceleration of the vehicle was slightly higher than the acceleration expected from the commanded circular trajectory, and this in turn compensated for the initial Note how the EKF convergence is always slower than in the nonlinear observer case (dashed line) despite the wide range of initial Σ(t0). Furthermore, the EKF convergence results are almost insensitive to any 'reasonable' initial choice of Σ(t0) (this plot should also be compared against Fig. 5a).

'disturbing' effects of the perturbation g (thanks to the higher level of excitation of the actual trajectory). The estimated metric linear velocities are shown in Fig. 10. The error against the ground truth (depicted in Fig. 11) shows a fast convergence rate for the nonlinear observer. Nevertheless, the EKF yielded a reliable output after approximately 20 s as well. All quantitative results can be compared in Tab. II.

As an additional validation of our approach, we also tested both algorithms on an inclined circular trajectory similar to the one depicted in Fig. 3 in order to deal with the case of a timevarying plane distance d(t). The plots shown in Figs. 12-13 again fully confirm that both the scene depth and the vertical component of the linear velocity can be reliably estimated despite the more challenging UAV motion (time-varying d(t)).

Motivated by these good results in comparison with the EKF, all the following experiments only involve the nonlinear observer scheme.

3) Comparison to previous work: for a direct comparison with a state-of-the-art scale estimation approach, we applied the nonlinear observer scheme under experimental conditions similar to those used in [START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF]. Therefore, the camera-IMU system was moved along a trajectory consisting of small sinusoidal hand-held motions. This resulted in a vehicle trajectory with an amplitude of about 0.1 m/s and a frequency of approximately 1 Hz at a height of 0.5 m. All three directions were tested individually while motion along the other axes was avoided.

Figure 14 shows the estimated metric velocity against ground truth for the nonlinear observer. On this trajectory, we found an RMS value of [0.0074, 0.0095, 0.0114] m/s for the three Cartesian directions, respectively. This allows us to compare our results to the figures of [0.028, 0.035, 0.025] m/s reported in [START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF]. Therefore, using the nonlinear observer, we could obtain an average improvement of 320 % compared to the results reported in [START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF]. All results are summarized in Tab. II. Although the experimental conditions are obviously different since the original dataset is not available, we believe these results do indicate the effectiveness of the proposed nonlinear observer in dealing with visual-inertial scale and ego-motion estimation vs. other consolidated approaches.

D. Estimation of the Gravity Vector

Both scale estimation schemes require knowledge of the direction of the gravity vector g, which, as explained in Sec. III-C1, is assumed provided by the IMU itself. In order to verify the accuracy of this gravity estimation, we compared the IMU estimation of g against the ground truth obtained from the motion tracking system during the circular flights conducted for the experiments of Sec. IV-C2.

The comparison of the x and y components of the normalized gravity vector g/ g are shown in Fig. 15. In average, the error of the gravity direction estimated by the IMU vs. the ground truth is smaller than 1 deg. This error is smaller than the accuracy with which we were able to define the object frame of the quadrotor in the motion tracking system. Therefore, we can consider the estimated g from the onboard IMU suitable for our needs. As shown in Figs. 5b-9c and discussed in Sec. IV-B-IV-C, keeping a constant acceleration norm Ω(t) allows to impose to the estimation error z(t) = 1/d(t)-1/ d(t) of the nonlinear observer a behavior equivalent to the ideal response (48). This possibility allows for a more general observation. Given a desired time for reaching some percentage of the initial error z(t 0 ), and choosing a gain α, one can exploit (48) to determine the needed acceleration norm to achieve this goal. Similarly, for a given trajectory with a known acceleration norm, and a given gain α, one can determine the time needed to reach a desired percentage of the initial estimation error. Such a relation between acceleration norm and convergence time is plotted in Fig. 16 for the three cases of reaching 10 %, 1 %, and 0.1 % of the initial error under the (arbitrary) choice α = 12. Thus, for a bounded initial estimation error, i.e., with the maximum distance to the planar scene known before launch, one can univocally predict how long to fly (with constant acceleration norm) for having a guaranteed accuracy in the estimated distance. We note that this analysis can, of course, be done for any choice of gain α.

F. Noise Robustness of the Nonlinear Observer

As opposed to the fully informed EKF, the nonlinear observer handles presence of noise in the sensor readings in an indirect way. In particular, the single gain α regulates the 'aggressiveness' of the filter, influencing both the convergence speed and its sensitivity to noise. As explained, in all the previous experiments we tuned α so as to obtain the same noise level (variance of the estimation error after convergence) in the nonlinear observer and EKF cases.

To test the robustness of the nonlinear observer to increased levels of sensor noise for the same choice of α, we increased the simulated noise by a factor of 50 for both the IMU readings and the scaled velocity v/d from the optical flow decomposition. Figure 17a shows the influence of different sensor noise levels on the estimation error d(t) -d(t). Despite the high noise level, the filter demonstrates a good level of robustness in generating a consistent state estimation.

Furthermore, from the theoretical analysis of Sec. III-D, the convergence rate of the nonlinear observer in (48) is determined by the quantity σ d = Ω √ α. Thus, one can always trade smaller accelerations with a higher gain α for obtaining the same (ideal) convergence rate. In this sense, we compared a circular flight with an acceleration norm of Ω ≈ 0.296 m/s 2 and a choice of α = 12 to two flights with acceleration norms 0.148 m/s 2 and 0.037 m/s 2 , and the gain α chosen so as to yield the same σ d (i.e., same error convergence rate). The results are reported in Fig. 17b where we can note, as expected, that by keeping the same σ d one 15: Experimental results -x and y components of the normalized gravity vector g/ g estimated by the internal sensor fusion algorithm of the employed IMU against the ground truth obtained from the motion capture system. In average, the error between the IMU estimation and the ground truth is in the range of 1 deg despite the accelerated motion undergone by the UAV. obtains the same transient behavior for the estimation error z(t), although a higher noise level is clearly induced by the larger employed α. The plot can be read as follows: on a trajectory with an acceleration norm of 0.296 m/s 2 , the error will drop below 10% of its initial value within 3.79 s and below 1% within 6.47 s.

G. Closed-Loop Control of a Real Quadrotor UAV

As a final validation of the overall framework, we made use of the estimated metric velocity to 'close the loop' and control the UAV motion in real time. For this experiment, we combined algorithm V2 for the optical flow decomposition The variance values of σ 2 V = 0.0007 and σ 2 I = 0.0004 for the visual and inertial sensor data, respectively, correspond to the actual noise found on the real sensors. The noise was then increased 50 times for a comparison. (b) plot showing the trade-off between 'control effort' (norm of the acceleration Ω ) and gain α. In case of lower accelerations, the same convergence rate can be re-established using a higher gain α but at the expense of an increased noise level as clear from the plot. Gain α can then be used to tune the 'aggressiveness' of the filter vs. a given amount of control effort Ω .

with the scale estimation obtained from the nonlinear observer. The vehicle was commanded using a gamepad to send velocity commands via a wireless link.

Figure 18 shows a section of a longer flight relying purely on onboard sensors. To test the robustness of the system, we moved an object through the field of view of the down-facing camera at time t = 125 s, causing some disturbances in the estimated velocity. Starting from time t = 134 s, the vehicle was additionally commanded to move in the vertical direction, causing the height, and therefore the distance to the plane, to change. The plots show some temporary over-and underestimations of the linear velocity due to the abrupt commanded motions, but otherwise the proposed approach is always able to adapt to the changing height and recover a reliable estimation.

A video from one of these experiments (Extension 1) is also attached to the paper. Both external views of the vehicle and the image stream from the down-facing camera are shown while the vehicle navigates purely based on onboard sensors and processing capabilities.

We then believe these experiments contribute to demonstrate that the presented velocity-based state estimation pipeline can be used for the closed-loop control of a real quadrotor UAV using solely onboard sensors.

V. CONCLUSIONS AND FUTURE WORK

In this paper, we addressed the need of a reliable onboard ego-motion estimation for quadrotor UAVs to overcome the boundaries of controlled indoor environments (such as when relying on external motion tracking systems for recovering the UAV state). To this end, we first discussed three variants of an algorithm based on the continuous homography constraint to obtain an estimation of the UAV scaled linear velocity and angular velocity from the decomposition of the perceived optical flow. This step, indeed, allows to retrieve ego-motion information independently of maps, known landmarks, and the need for tracking features over an extended period of time. Subsequently, we extensively discussed the issue of estimating the (unknown) metric scale factor by fusing the scaled velocity retrieved from optical flow with the high frequency accelerometer readings of an onboard IMU. Scale estimation was achieved by proposing two estimation schemes: a first one based on a classical EKF and a second one on a novel nonlinear observation framework. Simulated and real experimental data were presented to assess and compare the performance of both filters in ideal and real conditions. When compared to the EKF, the nonlinear observer demonstrated a consistent better performance in terms of convergence rate of the scale estimation error. Furthermore, the proposed theoretical analysis showed the possibility to actively impose (and thus predict) the error transient response of the nonlinear observer by suitably acting on the estimation gains and UAV motion (the norm of its acceleration). This analysis was, again, fully confirmed by the reported simulative/experimental results in several conditions, also involving different levels of sensor noise for testing the robustness of the approach.

With the advantage of a fast and predictable convergence for recovering the metric UAV linear velocity, the nonlinear observer proved to be a suitable choice for then implementing a fully onboard control of flying vehicles. In particular, we successfully demonstrated the reliability of the proposed framework in closed-loop experiments on a quadrotor UAV.

A. Future Work

Despite the convincing results, we are considering to extend our work in several ways. A first possibility is to further exploit the awareness of the error convergence behavior when using the nonlinear filter. Inspired by what presented in [START_REF] Achtelik | Path Planning for Motion Dependent State Estimation on Micro Aerial Vehicles[END_REF] in the context of offline path planning, one could devise an online strategy meant to adjust the input UAV trajectory (e.g., the velocity commands) in order to maintain a desired level of excitement, in our case represented by the quantity σ d = √ α Ω . This could result in, e.g., execution of small circular trajectories when the vehicle is commanded to hover in place with, instead, a more precise tracking of the desired/commanded velocities when undergoing more aggressive manoeuvres. On a similar note, we are currently investigating the possibility to dynamically adapt gain α for the nonlinear observer. Indeed, in the presence of noisy sensor readings, it might be desirable to start with a high value of α for imposing an initial quick error convergence, and to then reduce it when a sufficient convergence level is reached so as to obtain a smoother state estimation (see also Fig. 17b). Finally, it would also be worth to investigate the possibility to extend the structure of the nonlinear observer in order to account for possible biases in the employed measurements: this is, for instance, the case of the squared term x 1 x T 1 in (42) which, being x 1 = v/d a noisy measurement, will introduce a non-compensated bias in the observer dynamics. While presence of this bias had, in practice, a negligible effect in the reported results, an extension able to compensate for it online would clearly provide an interesting theoretical and practical improvement (especially in situations involving higher level of noise in the measurements).

APPENDIX A: INDEX TO MULTIMEDIA EXTENSIONS

The multimedia extensions to this article are at: http://www. ijrr.org.

Extension Type Description 1

Video Experiments of UAV closed-loop control with only onboard sensing and processing power

Fig. 1 :

 1 Fig. 1: Locations of the IMU (I), camera (C), body (B) and world frames (W) relative to each other. Frames I, B and C are assumed to be rigidly linked to each other. The world frame W is oriented horizontally with its z-axis pointing down, following the NED convention.

Fig. 2 :

 2 Fig. 2: Experimental setup with the highlighted location of IMU and camera. The x-axis of the body frame is oriented along the red metal beam.

Fig. 3 :

 3 Fig. 3: Circular trajectory used in the experiments for comparing the three algorithms V1, V2, V3 of Sec. II. The trajectory has a diameter of 2 m and lies on a plane at 26.5 deg w.r.t the ground. The UAV completes one revolution every 10 s. The height varies between 0.5 m and 1.5 m. A similar trajectory, but lying on a horizontal plane, has also been used for the experiments of Sec. IV-C

  Comparison of the errors in retrieving the scaled linear velocity and angular velocity for the three algorithms of Sec. II-B-II-D: the pure visual estimation (V1), the inclusion of the readings from the rate gyros (V2), and the additional integration of the measured normal vector taken coincident with the gravity direction (V3). In the case of algorithms V2 and V3, the angular velocity is measured from the IMU.

Fig. 5 :

 5 Fig. 5: Simulated results -Estimated plane distance d using a simulated dataset. (a) Estimated distance d(t) and (b) estimation error z(t) = (1/d(t) -1/ d(t)) for the nonlinear observer and the EKF filter. In (b) the ideal convergence of the nonlinear observer as per (48) is superimposed together with the influence of the neglected disturbace g(e, t) given in (44). The vertical dashed line in (b) denotes the predicted time at which the estimation error of the nonlinear observer should have dropped below the threshold of 10% of the initial error (highlighted with a horizontal dashed line).

Fig. 6 :Fig. 7 : 7 EKFFig. 8 :

 6778 Fig. 6: Simulated results -Estimated linear velocity from simulated data in the (a) x, (b) y and (c) z components. The estimation provided the nonlinear observer converges faster, but both state estimators provide reliable results after convergence is reached.

Fig. 9 :Fig. 10 :Fig. 11 :

 91011 Fig. 9: Experimental results -(a) estimation of d(t), (b) estimation error 1/d(t) -1/ d(t) and (c) estimation error d(t) -d(t) against the ground truth exploiting a recoded dataset. The initial d(t0) is set to 5 m against a real distance of approximately d(t0) = 1 m. The ideal transient response of the nonlinear observer as per (48) is also shown in (b-c) with a dashed line. Note, again, the very good match between the predicted and actual behavior of the estimation error for the nonlinear observer case, as well as the faster overall convergence w.r.t. the EKF scheme

Fig. 12 :Fig. 13 :

 1213 Fig. 12: Experimental results -Results from flying along an inclined circular trajectory, thus involving a time-varying d(t). (a) Estimated d(t) for the nonlinear observer and EKF, (b) Estimation error d(t) -d(t) and ideal convergence behavior (48) for the nonlinear observer, and (c) norm of the velocity estimation error against the ground truth. Note, again, the very good performance of the nonlinear observer in recovering d(t) and v(t) despite the more challenging UAV motion (as well as, again, the almost perfect match between the actual and ideal transient response of the estimation error)

Fig. 14 :

 14 Fig.14: Experimental results -Estimated linear velocity applying the nonlinear observer on the experimental conditions similar to those used in[START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF]. The results for the (a) x, (b) y and (c) z directions are plotted against the ground truth obtained from the tracking system.

Fig.

  Fig.15: Experimental results -x and y components of the normalized gravity vector g/ g estimated by the internal sensor fusion algorithm of the employed IMU against the ground truth obtained from the motion capture system. In average, the error between the IMU estimation and the ground truth is in the range of 1 deg despite the accelerated motion undergone by the UAV.

1 Fig. 16 :

 116 Fig.16: Relation between the required 'excitement' Ω √ α and convergence time t needed to reach the same estimation accuracy. The plot was generated for a gain α = 12. The three curves show the relation between the time needed to reach a percentage = [10%, 1%, 0.1%] of the initial error vs Ω √ α. The plot can be read as follows: on a trajectory with an acceleration norm of 0.296 m/s 2 , the error will drop below 10% of its initial value within 3.79 s and below 1% within 6.47 s.

ΩFig. 17 :

 17 Fig.17: Experimental results -Influence of sensor noise on the nonlinear observer scheme. (a) Robustness of the nonlinear observer to different noise levels. The variance values of σ 2 V = 0.0007 and σ 2 I = 0.0004 for the visual and inertial sensor data, respectively, correspond to the actual noise found on the real sensors. The noise was then increased 50 times for a comparison. (b) plot showing the trade-off between 'control effort' (norm of the acceleration Ω ) and gain α. In case of lower accelerations, the same convergence rate can be re-established using a higher gain α but at the expense of an increased noise level as clear from the plot. Gain α can then be used to tune the 'aggressiveness' of the filter vs. a given amount of control effort Ω .

Fig. 18 :

 18 Fig. 18: Experimental results -Estimated and ground truth linear velocity during a closed loop flight under velocity control for the (a) x, (b) y, and (c) z axes. An object was moved through the field of view at t = 125 s inducing a transient disturbance on the estimated velocity.

  9 I 4 against the estimation EKF nonlin. obs.

	Simulated data
	RMS error after convergence (scale)	0.0075 m	0.0078 m
	RMS error after convergence (velocity) 0.0071 m/s 0.0111 m/s
	Recorded data
	RMS error after convergence (scale)	0.0923 m	0.0988 m
	RMS error after convergence (velocity) 0.1160 m/s 0.0945 m/s
	Comparison aganinst [15]
	RMS error x-axis (velocity)	0.0101 m/s 0.0074 m/s
	RMS error y-axis (velocity)	0.0141 m/s 0.0095 m/s
	RMS error z-axis (velocity)	0.0107 m/s 0.0114 m/s
	Improvement over [15] (avg)	250 %	323 %

TABLE II :

 II Comparison of the two scale estimation approaches presented in this work. The results are discussed in Sec. IV-C.

Specifically, the points must not lie on some particular quadratic surfaces which, however, include planes as special case, see[START_REF] Ma | An Invitation to 3-D Vision[END_REF].

We assume the IMU is able to self-calibrate by an initial estimation of the measurement biases (indeed, this is often the case of many commercial IMUs, including the one employed in the experiments reported in Sect. IV). The interested reader is referred to, e.g.,[START_REF] Weiss | Monocular Vision for Long-term Micro Aerial Vehicle State Estimation: A Compendium[END_REF],[START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale, and Bias Determination[END_REF],[START_REF] Weiss | Realtime Onboard Visual-Inertial State Estimation and Self-Calibration of MAVs in Unknown Environments[END_REF] for other estimation approaches addressing the concurrent IMU bias and structure estimation during flight.

Therefore, given an upper bounded initial error d(t 0 ) -d(t 0 ), one can, for instance, plan in advance the duration of the UAV motion so as to yield a guaranteed accuracy in the estimated plane distance.

www.vicon.com.

VI. ACKNOWLEDGMENTS

The authors would like to thank Dr. Antonio Franchi and Martin Riedel for their valuable suggestions and contribution on the development of the underlying software framework. Part of Heinrich Bülthoff's research was supported by the Brain Korea 21 PLUS Program through the National Research Foundation of Korea funded by the Ministry of Education. Correspondence should be directed to Heinrich H. Bülthoff.