Agostino Martinelli
email: agostino.martinelli@ieee.org

Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination

Agostino Martinelli

Abstract-This paper investigates the problem of vision and inertial data fusion. A sensor assembling constituted by one monocular camera, three orthogonal accelerometers and three orthogonal gyroscopes is considered. The first paper contribution is the analytical derivation of all the observable modes, i.e. all the physical quantities that can be determined by only using the information in the sensor data acquired during a short time interval. Specifically, the observable modes are the speed and attitude (roll and pitch angles), the absolute scale and the biases affecting the inertial measurements. This holds even in the case when the camera only observes a single point feature. The analytical derivation of the aforementioned observable modes is based on a non standard observability analysis, which fully accounts the system non linearities. The second contribution is the analytical derivation of closed-form solutions which analytically express all the aforementioned observable modes in terms of the visual and inertial measurements collected during a very short time interval. This allows introducing a very simple and powerful new method able to simultaneously estimate all the observable modes without the need of any initialization or a priori knowledge. Both the observability analysis and the derivation of the closed-form solutions are carried out in several different contexts, including the case of biased and unbiased inertial measurements, the case of a single and multiple features, and in presence and absence of gravity. In addition, in all these contexts, the minimum number of camera images necessary for the observability is derived. The performance of the proposed approach is evaluated via extensive Monte Carlo simulations and real experiments.

Index Terms-Sensor Fusion, Vision-aided Inertial Navigation, Computer Vision, Non linear Observability, Aerial Robotics

I. INTRODUCTION

In recent years, vision and inertial sensing have received great attention by the mobile robotics community. These sensors require no external infrastructure and this is a key advantage for robots operating in unknown environments where GPS signals are shadowed. Additionally, these sensors have very interesting complementarities and together provide rich information to build a system capable of vision-aided inertial navigation and mapping.

When fusing vision and inertial measurements, the following two issues must be addressed:

1) find all the physical quantities that the information contained in the sensor data allows us to estimate;

2) find a reliable and efficient method to estimate these physical quantities starting from the raw sensor data. Throughout this paper, we will call these physical quantities the Observable Modes.

It is very reasonable to expect that, when fusing vision and inertial measurements, the absolute scale is an observable mode and can be obtained by a closed-form solution. Let us consider the trivial case where a vehicle, equipped with a bearing sensor (e.g. a camera) and an accelerometer, moves on a line (see fig 1). If the initial speed in A is known, by integrating the data from the accelerometer, it is possible to determine the vehicle speed during the subsequent time steps and then the distances A -B and B -C by integrating the speed. The lengths A -F and B -F are obtained by a simple triangulation by using the two angles β A and β B from the bearing sensor. Let us now assume that the initial speed v A is unknown. In this case, all the segment lengths can be obtained in terms of v A . In other words, we obtain the analytical expression of A -F and B -F in terms of the unknown v A and all the sensor measurements performed while the vehicle navigates from A to B. By repeating the same computation with the bearing measurements in A and C, we have a further analytical expression for the segment A -F , in terms of the unknown v A and the sensor measurements performed while the vehicle navigates from A to C. The two expressions for A -F provide an equation in the unknown v A . By solving this equation we finally obtain all the lengths in terms of the measurements performed by the accelerometer and the bearing sensor. The previous example is very simple because of several unrealistic restrictions. First of all, the motion is constrained on a line. Additionally, the accelerometer provides gravity-free and unbiased measurements. In this paper we will relax these restrictions by considering the case of a vehicle equipped with IMU1 and bearing sensors. We want to know which are the observable modes, namely the physical quantities that can be determined without any a priori knowledge (i.e. by only collecting the data from the sensors during a short time interval). For instance, are the absolute scale, the vehicle speed and the vehicle orientation observable modes? Are they observable modes even in the case of biased IMU measurements? Are the biases (affecting the IMU measurements) observable modes? And more importantly: is it possible to determine all these quantities by a closed form solution (as in the simple unrealistic example previously provided)? And, if yes, what is the minimum number of camera images necessary for this determination?

An answer to the first three questions can be found by applying the method introduced in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], where a non standard observability analysis, based on the new concept of continuous symmetry, has been introduced. The advantages of this non standard observability analysis is that, in contrast to previous approaches, it is able not only to check whether a given state is observable or not, but, in the negative case, it is also able to detect the quantities which are observable. In particular, by analyzing the continuous symmetries of a given system, it is possible to obtain a system of partial differential equations. The observable modes are all the independent solutions of this system of partial differential equations. In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], this new concept of continuous symmetry has been adopted to deal with a calibration problem in the framework of wheeled robotics. In [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF], this concept has been adopted to deal with the problem of vision and inertial data fusion. Specifically, the observable modes have been provided in the case of one feature and in the case of unbiased IMU measurements. Additionally, a closedform solution has been derived in this special case and the performance of an estimator based on an Extended Kalman Filter has also been discussed. In this paper, we also provide the analytical derivation of the observable modes starting from the theory developed in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. Additionally, also new realistic contexts are considered, by including the case of biased and unbiased inertial measurements, the case of single and multiple features, and in presence and absence of gravity.

The paper is organized as follows. Section III illustrates and summarizes the basic steps of the method introduced in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], by dealing with a simple 2D localization problem. Section IV provides a mathematical description of the system. Starting from this description, in sections V and VI the observability analysis is performed. Then, in section VII, we provide closedform expressions of the observable modes in terms of the sensor measurements. The performance of the method in estimating the observable modes is evaluated by using synthetic and real data (section VIII). Finally, conclusions are provided in section IX.

II. RELATED WORKS

The problem of fusing vision and inertial data has been extensively investigated in the past. A special issue of the International Journal of Robotics Research has recently been devoted to this important topic [START_REF] Dias | 2nd Workshop on Integration of Vision and Inertial Sensors[END_REF]. In [START_REF] Corke | An Introduction to Inertial and Visual Sensing[END_REF], a tutorial introduction to the vision and inertial sensing is presented. This work provides a biological point of view and it illustrates how vision and inertial sensors have useful complementarities allowing them to cover the respective limitations and deficiencies. In [START_REF] Quian | Reduction of inherent ambiguities in structure from motion problem using inertial data[END_REF] the inertial measurements are used in order to reduce the ambiguities in the structure from motion problem. Recent works investigate the observability properties of the vision-aided inertial navigation system [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF], [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF] and [START_REF] Mourikis | A Multi-State Constrained Kalman filter for Vision-aided Inertial Navigation[END_REF]. These works show that the absolute roll and pitch angles of the vehicle are observable modes while the yaw angle is unobservable. This result is consistent with the experimental results obtained in [START_REF] Bryson | Building a Robust Implementation of Bearing-only Inertial SLAM for a UAV[END_REF] which clearly show how the roll and pitch angles remain more consistent than the heading. In [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF], the authors provide a theoretical investigation to analytically derive the motion conditions under which the vehicle state is observable. This analysis also includes the conditions under which the parameters describing the transformation camera-IMU are identifiable. On the other hand, a general theoretical investigation able to also derive the minimum number of camera observations2 necessary for the state determination still lacks. The results presented in section VI address precisely these limitations. In addition, in section V, the observability analysis is performed in several contexts by also including the case of biased inertial measurements.

The majority of the approaches so far introduced, perform the fusion of vision and inertial sensors by filter-based algorithms. In [START_REF] Armesto | Vincze Fast Ego-motion Estimation with Multi-rate Fusion of Inertial and Vision[END_REF], these sensors are used to perform egomotion estimation. The sensor fusion is obtained by an Extended Kalman Filter (EKF ) and by an Unscented Kalman Filter (U KF ). The approach proposed in [START_REF] Gemeiner | Simultaneous Motion and Structure Estimation by Fusion of Inertial and Vision Data[END_REF] extends the previous one by also estimating the structure of the environment where the motion occurs. In particular, new landmarks are inserted on line into the estimated map. This approach has been validated by conducting experiments in a known environment where a ground truth was available. Also, in [START_REF] Veth | Fusing low-cost image and inertial sensors for passive navigation[END_REF] an EKF has been adopted. In this case, the proposed algorithm estimates a state containing the robot speed, position and attitude, together with the inertial sensor biases and the location of the features of interest. In the framework of airbone SLAM, an EKF has been adopted in [START_REF] Kim | Real-time implementation of airborne inertial-SLAM[END_REF] to perform 3D-SLAM by fusing inertial and vision measurements. It was observed that any inconsistent attitude update severely affects any SLAM solution. The authors proposed to separate attitude update from position and velocity update. Alternatively, they proposed to use additional velocity observations, such as air velocity observation.

When using an EKF , an important issue which arises is the initialization problem. Indeed, because of the system nonlinearities, an erroneous initialization can irreparably damage the entire estimation process. This problem has been consid-ered in [START_REF] Lupton | Removing scale biases and ambiguity from 6DoF monocular SLAM using inertial[END_REF]. In particular, the proposed method is able to estimate the absolute scale by using a square root information filter. Additionally, the same authors proposed an EKF which does not suffer from the initialization of the speed and of the orientation [START_REF] Lupton | Efficient Integration of Inertial Observations into Visual SLAM without Initialization[END_REF].

In [START_REF] Mourikis | A Multi-State Constrained Kalman filter for Vision-aided Inertial Navigation[END_REF] it is introduced a measurement model that is able to express the geometric constraints that arise when the same feature is observed from multiple camera poses. This measurement model does not require to include the feature position in the state which is estimated by an EKF . A similar idea is adopted in [START_REF] Zachariah | Camera-aided inertial navigation using epipolar points[END_REF]. Also in this case, the problem of estimating the location of each feature is avoided, by using epipolar points on the image plane.

There are very few methods able to perform the fusion of image and inertial measurements without a filter-based approach. One algorithm of this type has been suggested in [START_REF] Strelow | Motion estimation from image and inertial measurements[END_REF]. This algorithm is a batch method which performs SLAM from image and inertial measurements. Specifically, it minimizes a cost function by using the Leven-Marquardt algorithm. This minimization process starts by initializing the velocities, the gravity and the biases to zero. In [START_REF] Folkesson | SIFT Based Graphical SLAM on a Packbot, Field and Service Robotics[END_REF] the graphical SLAM approach has been suggested to fuse the data from many different sensors: encoder, inertial, vision and GPS.

To the best of our knowledge, no prior work has addressed the problem of determining the trajectory of a platform in closed form, by only using visual and inertial measurements. Section VII addresses precisely this important problem by providing closed-form expressions of all the observable modes in several different contexts. These solutions have the advantage of not requiring any prior information about the state.

Finally, an important issue which arises when inertial and vision sensors are simultaneously used, is the problem of the extrinsic calibration, i.e. the estimation of the relative pose of these sensors. This problem has been approached in the past and several iterative and non-iterative solutions have been proposed. In [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF] the extrinsic calibration has been performed by using an EKF . Non-iterative solutions have been proposed in [START_REF] Hol | Modeling and Calibration of Inertial and Vision Sensors[END_REF] and [START_REF] Lobo | Relative pose calibration between visual and inertial sensors[END_REF].

III. OBSERVABLE MODES AND CONTINUOUS SYMMETRIES

When a state is not observable, there are in general infinite initial states reproducing exactly the same inputs and outputs. Let us consider for instance, the 2D localization problem when the vehicle moves along a corridor, equipped with odometry sensors and sensors able to perform relative observations (e.g. bearing and range sensors). In this situation, all the initial states differing for a shift along the corridor, reproduce exactly the same inputs and outputs. Intuitively, we remark that the entire system has one continuous symmetry that is the invariance of the corridor with respect to a shift. It is obvious that the only quantities that we can estimate (i.e. the observable modes) are invariant with respect to this continuous symmetry (i.e. the vehicle orientation and the distance of the vehicle from the corridor walls). The previous consideration regarding this simple localization problem is quite trivial and it's not required to introduce special mathematical tools. However, there are cases where deriving the observable modes is a very challenging task. The key to deal with these cases is to first provide a mathematical definition of continuous symmetry able to generalize the intuitive idea of symmetry. In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], a procedure which allows us to analytically derive the observable modes for a generic system, has been introduced. This procedure is based on the concept of continuous symmetry, whose mathematical definition has also been provided. In this section we remind the reader the basic concepts characterizing the theory developed in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. For the sake of clarity, these concepts will be illustrated by referring to a simple localization problem, which is introduced in section III-A.

A. A Simple Localization Problem

We consider a mobile robot moving in a 2D-environment. The configuration of the robot in a global reference frame, can be characterized through the vector [x R , y R , θ R ] T where x R and y R are the cartesian robot coordinates, and θ R is the robot orientation. It is also possible to characterize the robot configuration by using the polar coordinates, i.e. D ≡ x 2 R + y 2 R and φ R ≡ arctan 2(y R , x R ). The dynamics are described by the following non-linear differential equations:

   ẋR = v cos θ R ẏR = v sin θ R θR = ω or     Ḋ = v cos(θ R -φ R ) φR = v D sin(θ R -φ R ) θR = ω (1)
where v and ω are the linear and the rotational robot speed respectively. The robot is equipped with proprioceptive sensors which are able to evaluate these two speeds. We assume that a point feature exists in our environment and, without loss of generality, we fix the global reference frame onto it (see figure 2a). The robot is also equipped with a bearing sensor (e.g. a camera), able to evaluate the bearing angle of the point feature in its own frame. Therefore, our system has the following output (see fig. 2a): To check whether the robot configuration [x R , y R , θ R ] T is observable or not, we have to prove that it is possible to uniquely reconstruct the initial robot configuration by knowing the input controls and the outputs (observations) in a given time interval. When at the initial time, the bearing angle β of the origin is available, the robot can be everywhere in the plane but, for each position, only one orientation provides the right bearing β. In fig. 2b all the three positions A, B and C are compatible with the observation β, provided that the robot orientation satisfies (2). In particular, the orientation is the same for A and B but not for C.

y = β ≡ π -θ R + atan2(y R , x R ) = π -θ R + φ R (2) a b
Let us suppose that the robot moves according to the inputs v(t) and ω(t). With the exception of the special motion consisting of a line passing by the origin, by only performing a further bearing observation it is possible to distinguish all the points belonging to the same line passing by the origin. In fig. 3a the two initial positions in A and B do not reproduce the same observations (β A = β B ). On the other hand, all the initial positions whose distance from the origin is the same, cannot be distinguished independently of the chosen trajectory. In fig. 3b, the two indicated trajectories provide the same bearing observations at any time. Therefore, the dimension of the undistinguishable region is 1 and the dimension of the largest observable subsystem is We remark that the system has a continuous symmetry: the system inputs (v(t) and ω(t)), and outputs (y(t)), are invariant with respect to a rotation of the global frame about the vertical axis (in the next section we will provide a mathematical definition for a general continuous symmetry). Based on the fact that the dimension of the largest observable subsystem is two, we know that we can only estimate two independent modes. In addition, these two modes must satisfy the aforementioned system invariance, i.e. they must be rotation invariant. A possible choice is provided by the two quantities D and θ in figure 2a (θ ≡ θ R -atan2(y R , x R )).

3 -1 = 2.
The new system is characterized by the following equations:

  Ḋ = v cos θ θ = ω - v D sin θ y = β = π -θ (3)
which express the link between the new state [D, θ] T and the proprioceptive data (v, ω) and the exteroceptive data (β). The detection of the two modes (D and θ) and the derivation of the equations in (3) is fundamental. Indeed, estimating the original state brings inconsistencies with catastrophic consequences.

In the next subsections we remind the reader some concepts in the theory by Hermann and Krener in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] and some basic tools introduced in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] in order to perform the same analysis in the case of more complex systems. This will allow us to derive the observable modes when fusing monocular vision and IMU sensor measurements.

B. Observability Rank Criterion

A general characterization for systems in the framework of autonomous navigation, is provided by the following two equations, which describe the dynamics and the observation respectively:

     Ṡ = f (S, u) = f 0 (S) + L i=1 f i (S)u i y = h(S) (4)
where S ∈ Σ ⊆ n is the state, u = [u 1 , u 2 , ..., u L ] T are the system inputs, y ∈ is the output (we are considering a scalar output for the sake of clarity; the extension to a multi dimensional output is straightforward). The system defined by (1-2) (both in cartesian and in polar coordinates) and the one defined by (3) can be characterized by (4). For instance, for the system in (1) in polar coordinates, we have:

S = [D, φ R , θ R ] T , f 0 = [0, 0, 0] T , L = 2, u 1 = v, u 2 = ω, f 1 (S) = [cos(θ R -φ R ), sin(θ R -φ R ) D , 0] T , f 2 (S) = [0, 0, 1] T , h(S) = π -θ R + φ R .
We indicate the k th order Lie derivative of a field Λ along the vector fields v i1 , v i2 , ..., v ik with L k v i1 , v i2 , ..., v iK Λ. The definition of the Lie derivative is provided by the following two equations:

L 0 Λ = Λ, L k+1 vi 1 ,...,vi k+1 Λ = ∇ S L k vi 1 ,...,vi k Λ . v i k+1 (5)
where the symbol "." denotes the scalar product and ∇ S the gradient operation with respect to the state S. We remark that the Lie derivatives quantify the impact of changes in the control input (u i ) on the output function (h). Additionally, we denote with dL k f i1 , ..., f ik h, the gradient of the corresponding Lie derivative (i.e. dL k f i1 , ..., f ik h ≡ ∇ S L k f i1 , ..., f ik h), and, we denote with dΩ, the space spanned by all these gradients.

In this notation, the observability rank criterion can be expressed in the following way: The dimension of the largest observable sub-system at a given S 0 is equal to the dimension of dΩ.

We consider again the simple example introduced in III-A, and we show that by using the observability rank criterion, we find the same result obtained by following intuitive reasoning (i.e. that the dimension of the largest observable subsystem is 2).

The computation of the rank for the system in (1-2) is straightforward. Let us use the polar coordinates. From (2), we obtain:

L 0 h = π -θ R + φ R whose gradient is dL 0 h ≡ w 1 = [0, 1, -1]. The first order Lie derivatives are: L 1 f 1 h = sin(θ R -φ R ) D and L 1 f 2 h = -1.
We have:

dL 1 f 1 h ≡ w 2 = [-sin(θ R -φ R ) D 2 , -cos(θ R -φ R ) D , cos(θ R -φ R ) D
]. It is easy to realize that each vector w i obtained by extending the previous computation to every Lie derivative order, has the structure:

w i = [ i , ς i , -ς i ].
Indeed, every Lie derivative will depend on θ R and φ R only through the quantity θ R -φ R , whose sign changes with respect to the change θ R ↔ φ R . Therefore, the rank of the matrix

Γ ≡ w T 1 , w T 2 , ..., w T i , ... (6)
is equal to two. We conclude that the largest observable subsystem has dimension two as derived in section III-A.

C. Continuous Symmetries

We refer to the input output system given in [START_REF] Dias | 2nd Workshop on Integration of Vision and Inertial Sensors[END_REF]. In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], we introduced the following definition of continuous symmetry: Definition 1 (Continuous Symmetry) The vector field w s (S) (S ∈ Σ) is a continuous symmetry in S for the system defined in (4) if and only if it is a non null vector belonging to the null space of the matrix whose lines are the gradients of all the Lie derivatives computed in S.

We discuss again the simple example provided in section III-A. We show that the previous definition corresponds to a global rotation.

For the system defined in (1-2) only one continuous symmetry exists given, in polar coordinates, by the vector w s = [0, 1, 1] T (i.e. belonging to the null space of the matrix Γ in ( 6)). Let us provide an intuitive interpretation of this continuous symmetry. It is possible to see that this symmetry corresponds to an infinitesimal rotation. Indeed, an infinitesimal rotation of magnitude about the vertical axis changes the state as follows [START_REF] Goldstein | Classical Mechanics[END_REF]:

  D φ R θ R   →   D φ R θ R   +   0 1 1   =   D φ R θ R   + w s
In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] we proved the following fundamental property: Property 1 g(S) is an observable mode if and only if its gradient is orthogonal to all the symmetries. This property can be expressed by a system of partial differential equations, one for each symmetry:

n i=1 w si (S) ∂g ∂S i = 0 (7)
where w si (S) is the i th component of the symmetry w s .

In other words, for every symmetry there is an associated partial differential equation which must be satisfied by all the observable modes.

We use [START_REF] Goldstein | Classical Mechanics[END_REF] to derive the two observable modes for the system discussed in section III-A. As previously mentioned, this system only has the symmetry [0, 1, 1] T . Hence, the associated equation ( 7) becomes:

∂g ∂φ R + ∂g ∂θ R = 0
and two independent solutions are g = D and g = θ R -φ R . This is the same result we obtained in section III-A.

We conclude this section by summarizing the main steps illustrated in this section to detect the observability properties of a given input-output system. The first step consists in the derivation of all the continuous symmetries. This is obtained by computing the analytical expression of the Lie derivatives 3 . Then, according to property 1, a system of partial differential equations is obtained and the observability properties are obtained by solving this system of partial differential equations. Indeed, all the independent observable modes are all the independent solutions of this system.

IV. THE CONSIDERED SYSTEM

Let us consider a sensor assembling constituted by a monocular camera and IM U sensors. The IMU consists of three orthogonal accelerometers and three orthogonal gyroscopes. We assume that the transformations among the camera frame and the IMU frames are known (we can assume that the local frame coincides with the camera frame). In the following, we will use the word vehicle to refer to this sensor assembling. The IM U provides the vehicle angular speed and acceleration. Actually, regarding the acceleration, the one perceived by the accelerometer (A) is not simply the vehicle acceleration (A v ). It also contains the gravitational acceleration (A g ). In particular, we have A = A v -A g since, when the camera does not accelerate (i.e. A v is zero) the accelerometer perceives an acceleration which is the same of an object accelerated upward in the absence of gravity.

We will use uppercase letters when the vectors are expressed in the local frame and lowercase letters when they are expressed in the global frame. Hence, regarding the gravity we have: a g = [0, 0, -g] T , being g 9.8 ms -2 .

We assume that the camera is observing a point feature during a given time interval. We fix a global frame attached to this feature. The vehicle and the feature are displayed in fig [START_REF] Dias | 2nd Workshop on Integration of Vision and Inertial Sensors[END_REF]. Finally, we will adopt a quaternion to represent the vehicle orientation. Indeed, even if this representation is redundant, it is very powerful since the dynamics can be expressed in a very easy and compact notation [START_REF]Quaternions and rotation Sequences: a Primer with Applications to Orbits, Aerospace, and Virtual Reality[END_REF].

Our system is characterized by the state [r, v, q] T where r = [r x , r y , r z ] T is the 3D vehicle position, v is its time derivative, i.e. the vehicle speed in the global frame (v ≡ dr dt ), q = q t + iq x + jq y + kq z is a unitary quaternion (i.e. satisfying q 2 t + q 2 x + q 2 y + q 2 z = 1) and characterizes the vehicle orientation. The analytical expression of the dynamics and the camera observations can be easily provided by expressing all the 3D vectors as imaginary quaternions. In practice, given a 3D vector w = [w x , w y , w z ] T we associate with it the imaginary quaternion ŵ ≡ 0+iw x +jw y +kw z . The dynamics of the state [r, v, q] T are:

       ṙ = v v = q Âv q * = q Âq * + âg q = 1 2 q Ω (8)
being q * the conjugate of q, q * = q t -iq x -jq y -kq z . We now want to express the camera observations in terms of the same state ([r, v, q] T ). We remark that the camera provides the direction of the feature in the local frame. In other words, it provides the unit vector F |F | (see fig. 4). Hence, we can assume that the camera provides the two ratios y 1 = Fx Fz and y 2 = Fy Fz , being F = [F x , F y , F z ] T . We need to express F in terms of [r, v, q] T . We note that the position of the feature in the frame with the same orientation of the global frame but shifted in such a way that its origin coincides with the one of the local frame is -r. Therefore, F is obtained by the quaternion product F = -q * rq. The observation function provided by the camera is:

h cam (r, v, q) = [y 1 , y 2 ] T = (q * rq) x (q * rq) z , (q * rq) y (q * rq) z T ( 9
)
where the pedices x, y and z indicate respectively the i, j and k component of the corresponding quaternion. We have also to consider the constraint q * q = 1. This can be dealt as a further observation (system output):

h const (r, v, q) = q * q (10)

A. The Case with Multiple Features

We consider the case when the camera observes N f features, simultaneously. We fix the global frame on one of the features. Let us denote with d i the 3D vector which contains the cartesian coordinates of the i th feature (i = 0, 1, ..., N f -1). We assume that the global frame is attached to the 0 th feature, i.e. d 0 = [0 0 0] T . The new system is characterized by the state [r, v, q, d1 , ..., dN f -1 ] T , whose dimension is 7 + 3N f . The dynamics of this state are given by ( 8) together with the equations:

ḋi = [0 0 0] T i = 1, ..., N f -1 (11)
The position F i of the i th feature in the local frame is obtained by the quaternion product Fi = q * ( di -r)q. The corresponding observation function is:

h i cam = (q * ( di -r)q) x (q * ( di -r)q) z , (q * ( di -r)q) y (q * ( di -r)q) z T i = 0, 1, ..., N f -1 (12
) which coincides with the observation in (9) when i = 0. Summarizing, the case of N f features is described by the state [r, v, q, d1 , ..., dN f -1 ] T , whose dynamics are given in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] and [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF] and the observations are given in [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF] and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF].

B. The Case with Bias

We consider the case when the data provided by the IM U are biased. In other words, we assume that the measurements provided by the three accelerometers and the three gyroscopes are affected by an error which is not zero-mean. Let us denote with A bias and with Ω bias the two 3D-vectors whose components are the mean values of the measurement errors from the accelerometers and the gyroscopes, respectively. The two vectors A bias and Ω bias are time-dependent. However, during a short time interval, it is reasonable to consider them to be constant. Under these hypotheses, the dynamics in (8) become:

             ṙ = v v = q Âv q * = q Âq * + q Âbias q * + âg q = 1 2 q Ω + 1 2 q Ωbias Ȧbias = Ωbias = [0 0 0] T (13)
Note that these equations only hold for short time intervals.

In the following, we will use these equations only when this hypothesis is satisfied (in particular, during time intervals allowing the camera to perform at most ten consecutive observations).

V. OBSERVABILITY PROPERTIES

We investigate the observability properties of the system whose dynamics are given in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] and whose observations are given in ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]. For the sake of clarity, we discuss both the case without gravity (V-A) and with gravity (V-B). Moreover, in V-C we discuss the case when the camera is observing simultaneously more than one feature, namely we investigate the observability properties of the system defined by ( 8), [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF], [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF] and [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF]. Then, the case when the IMU sensors are affected by a bias is investigated (V-D).

The observability analysis performed in this section takes into account all the degrees of freedom allowed by the dynamics in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]. In other words, the observability of the modes here derived, could require the vehicle to move along all these degrees of freedom. The modes derived in this section could become unobservable when the vehicle performs special motions. In section VI we discuss the observability properties for special vehicle motions.

A. The Case without Gravity

Let us set g = 0 in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]. By directly computing the Lie derivatives and their gradients, it is possible to detect three independent symmetries for the resulting system. They are:

w Rotx s = 0 -r z r y 0 -v z v y - q x 2 q t 2 - q z 2 q y 2 T ( 14
)
w Roty s = r z 0 -r x v z 0 -v x - q y 2 q z 2 q t 2 - q x 2 T w Rotz s = -r y r x 0 -v y v x 0 - q z 2 - q y 2 q x 2 q t 2 T
According to definition 1, these vectors are orthogonal to all the gradients of all the Lie derivatives. These symmetries could also be derived by remarking the system invariance with respect to rotations about all the three axes. For instance, an infinitesimal rotation of magnitude about the vertical axis changes the state as follows [START_REF] Goldstein | Classical Mechanics[END_REF]:

  r x r y r z   →   r x r y r z   +   -r y r x 0     v x v y v z   →   v x v y v z   +   -v y v x 0       q t q x q y q z     →     q t q x q y q z     + 2     -q z -q y q x q t    
that is:

  r v q   →   r v q   + w Rotz s
On the other hand, without computing the Lie derivatives, we could not conclude that the rotational symmetries are all the symmetries for the considered system. In order to be sure that they are all the symmetries, we must detect 10 -3 = 7 independent Lie derivatives. In appendix A, we provide a possible choice of 7 independent Lie derivatives. Summarizing, we detected all the symmetries by proceeding in two separate steps. In the first, we used the system invariance under rotations which allowed us to immediately detect three symmetries. Then, by providing 7 independent Lie derivatives, we concluded that these are all the symmetries.

According to property 1, for every symmetry there is an associated partial differential equation (the one provided in [START_REF] Goldstein | Classical Mechanics[END_REF]). Hence, every observable mode must satisfy simultaneously all the three partial differential equations. Since our system is defined by 10 variables, the number of independent solutions satisfying all the three partial differential equations is 10 -3 = 7 [START_REF] John | Partial Differential Equations[END_REF]. On the other hand, their derivation, once the three symmetries are detected, is easy. Indeed, it is immediate to prove that the distance of the feature from the camera, i.e. |r|, is a solution of the three equations (this can be checked by substitution for the partial differential equations associated with the symmetries in ( 14) but can also be proved by remarking that the absolute scale is invariant under rotations). This means that the distance of the feature is observable and it is one among the 7 independent solutions. On the other hand, since the camera provides the position of the feature in the local frame up to a scale factor, having the distance means that the feature position in the local frame is also observable. Therefore, the three components of the feature position in the local frame are three independent solutions. By using quaternions, we can say that three independent solutions are provided by the components of the imaginary quaternion q * rq. Additionally, since the three partial differential equations are invariant under the transformation r ↔ v, three other independent solutions are the components of the imaginary quaternion q * vq. Physically, this means that the vehicle speed in the local frame is also observable. Finally, the last solution is q * q since it is directly observed (see equation [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]; it can be in any case verified that it satisfies the three partial differential equations).

The analytical results derived in this subsection can be summarized with the following property:

Property 2 (Observable Modes without Gravity) Let us consider the system defined by ( 8), ( 9) and ( 10) in absence of gravity (i.e. g = 0). All the independent observable modes are 7 and they are the three components of the imaginary quaternion q * rq (i.e. the position of the observed feature in the local frame), the three components of the imaginary quaternion q * vq (i.e. the vehicle speed in the local frame) and the product q * q (i.e. the norm of the the quaternion).

B. The Case with Gravity

We investigate the observability properties when g = 0.

The presence of the gravity breaks two of the three rotational symmetries. In other words, the system remains invariant only with respect to rotations about the vertical axis. This means that w Rotx s and w Roty s are no longer symmetries for the new system. By directly computing the Lie derivatives, we were able to find nine independent Lie derivatives (the computation is similar to the one illustrated in appendix A). Hence, the system has 10 -9 = 1 symmetry which is w Rotz s . The partial differential equation associated with w Rotz s is:

-2r y ∂Λ ∂r x + 2r x ∂Λ ∂r y -2v y ∂Λ ∂v x + 2v x ∂Λ ∂v y + (15)
-q z ∂Λ ∂q t -q y ∂Λ ∂q x + q x ∂Λ ∂q y + q t ∂Λ ∂q z = 0

The number of independent solutions Λ = Λ(r x , r y , r z , v x , v y , v z , q t , q x , q y , q z ) is equal to the number of variables (i.e. 10) minus the number of equations (i.e. 1) [START_REF] John | Partial Differential Equations[END_REF]. Hence, in this case we have two additional observable modes. They are:

Q r ≡ q t q x + q y q z 1 -2(q 2
x + q 2 y )

;

Q p ≡ q t q y -q z q x ( 16
)
Also for these two solutions it is possible to find a physical meaning. They are related to the roll and pitch angles [START_REF]Quaternions and rotation Sequences: a Primer with Applications to Orbits, Aerospace, and Virtual Reality[END_REF]. In particular, the first solution provides the roll angle which is R = arctan(2Q r ). The latter provides the pitch angle which is P = arcsin(2Q p ). Finally, we remark that the expression of the yaw, Y = arctan 2 qtqz+qxqy 1-2(q 2 y +q 2 z ) , does not satisfy [START_REF] John | Partial Differential Equations[END_REF]. The analytical results derived in this subsection can be summarized with the following property: Property 3 (Observable Modes with Gravity) Let us consider the system defined by ( 8), ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]. All the independent observable modes are 9 and they are the 7 observable modes for the case without gravity together with the roll and pitch angles.

C. The Case with Multiple Features

Let us suppose that the vehicle is observing N f > 1 features, simultaneously. The new system is characterized by the (7+3N f )dimensional state [r, v, q, d1 , ..., dN f -1 ] T , whose dynamics are given in ( 8) and ( 11) and the observations are given in ( 12) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF].

It is immediate to realize that all the camera observations are invariant with respect to the same symmetries found in the case of one single feature (for instance, the camera observations do not change when the initial state [r, v, q, d1 , ..., dN f -1 ] T is rotated about the vertical axis). Hence, in presence of gravity, the yaw angle is still unobservable. In absence of gravity, also the roll and pitch angles are unobservable. Hence, in presence of gravity, the number of independent modes cannot exceed 7 + 3N f -1 = 6 + 3N f . In absence of gravity, this number cannot exceed 7 + 3N f -3 = 4 + 3N f .

On the basis of the results obtained in the previous subsections, we know that the position of each feature in the local frame provides 3 observable modes. Also, the vehicle speed in the local frame provides 3 observable modes. In addition, an observable mode is the norm of the quaternion. Therefore, in both the cases with and without gravity, we have 3N f + 4 observable modes. In absence of gravity, these are all the observable modes. In presence of gravity, also the roll and pitch angles are observable modes, since they are observable modes with a single feature.

The analytical results derived in this subsection can be summarized with the following property: Property 4 (Observable Modes with Multiple Features) Let us consider the system defined by ( 8), [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF], [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF] and [START_REF] Kelly | Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration[END_REF]. All the independent observable modes are the components of the imaginary quaternion q * ( di -r)q, i = 0, 1, ..., N f -1 (i.e. the position of the observed features in the local frame), the three components of the imaginary quaternion q * vq (i.e. the vehicle speed in the local frame) and the product q * q (i.e. the norm of the quaternion). In addition, in presence of gravity, also the roll and pitch angles are observable modes.

D. The Case with Bias

In this subsection we will prove that, even when the camera only observes a single feature, the biases affecting the accelerometers and the gyroscopes are observable.

The system we are considering is defined by the state: [r v q A bias Ω bias ] T , whose dimension is 16. This state satisfies the dynamics in [START_REF] Kim | Real-time implementation of airborne inertial-SLAM[END_REF]. Finally, this system is characterized by the observations given in ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF].

We know that the state is not observable. Indeed, even without bias, we know that it is not possible to estimate the yaw angle (section V-B). In other words, also this system is invariant with respect to rotations about the vertical axis. Hence, its observable modes must satisfy the equation in [START_REF] John | Partial Differential Equations[END_REF], where, now, Λ also depends on the components of A bias and Ω bias . On the other hand, we do not know if the system has additional symmetries in which case the observable modes must satisfy additional partial differential equations, simultaneously. In order to prove that the system has a single symmetry, we must provide 15 independent Lie derivatives. By a direct computation, performed by using the symbolic Matlab computational tool, we were able to find the following 15 independent Lie derivatives:

L 0 y 1 , L 0 y 2 , L 0 h const , L 1 f 0 y 1 , L 1 f 0 y 2 , L 3 f 0 , f 0 , f 6 y 2 .
As previously mentioned, we know that we cannot have more than 15 independent Lie derivatives (otherwise, the yaw angle would be observable). Note that in the previous computation the expression of the vector fields f 0 , f 1 , ..., f 6 is not the one given in appendix A. The right one must be computed starting from the dynamics in [START_REF] Kim | Real-time implementation of airborne inertial-SLAM[END_REF].

The fact that we have 15 independent Lie derivatives means that there are no additional symmetries and, the independent observable modes, are the independent solutions of [START_REF] John | Partial Differential Equations[END_REF]. They are: the 9 solutions provided in V-B and the six components of the two vectors A bias and Ω bias (note that these components are trivial solutions of ( 15)).

The analytical results derived in this subsection can be summarized with the following property: Property 5 (Observable Modes in Presence of Bias) Let us consider the system defined by ( 13), ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]. All the independent observable modes are the same as in the case without bias and the six components of the two bias vectors A bias and Ω bias .

E. Unknown Gravity

The results provided in the previous sections are obtained by assuming that the magnitude of the gravitational acceleration (g) is a priori known. In [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF] we prove that g is among the observable modes even in the worst case when the inertial sensors are affected by a bias and when only a single feature is available. In other words, the following property holds: Property 6 (Observability of gravity) The gravity vector is observable even in the case of biased inertial measurements and when a single feature is available.

VI. OBSERVABILITY FOR SPECIAL TRAJECTORIES AND FEW CAMERA IMAGES

The goal of this section is to discuss the following two issues:

1) Derivation of the observability properties for special vehicle trajectories; 2) Derivation of the minimum number of camera images necessary for the observability of the modes derived in section V. As we will see, the second issue can be dealt starting from the results obtained by dealing with the first issue.

A. Special Trajectories

We are interested in deriving the observability properties for special trajectories. Mathematically, this can be done by introducing in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF] the constraints characterizing the trajectory we want to consider. Then, it suffices to apply the method described in section III to the system characterized by the new dynamics and the same observations ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]. We only consider two special cases since they allow us to derive important necessary conditions on the minimum number of camera images (see theorem 1). However, there are many other special motions/feature configurations, for which the observability properties degenerate. Some of them will be discussed for the case of two point features in three camera images (section VII-B).

The following property holds:

Property 7 (Observability with constant acceleration) When the vehicle moves with constant acceleration all the modes derived in section V are observable except the magnitude of the gravitational acceleration.

Proof: The proof is provided in [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF] A special case of constant acceleration is the case of constant speed. In this case we have a nice property when the magnitude of the gravity is a priori known: Property 8 (Observability with constant speed) When the magnitude of the gravity is known and the vehicle moves with constant speed all the modes derived in section V are observable up to a scale factor. Proof: Our system is characterized by the dynamics given in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF], where the second equation is replaced by v = 0 and with the parameter g a priori known. The system outputs are given in ( 9) and ( 10) together with the observations provided by the accelerometers (in this case  = -q * âg q). We want to derive the observable modes of this system. According to the method illustrated in section III, we need, first of all, to detect the system symmetries. Instead of computing the Lie derivatives, we remark that, with respect to the case of a general motion (investigated in section V-B), the system is characterized by a further symmetry. Indeed, the new dynamics are invariant with respect to the change r → λr, v → λv, being λ a real number. In addition, also the observations are invariant with respect to the same change 4 . We conclude that, when the vehicle does not accelerate, the system does not contain the information to determine the absolute scale 5 . This result also holds in the case of multiple features. Indeed, the same invariance also characterizes the equations in ( 11) and ( 12) by also considering d i → λd i , i = 0, 1, ..., N f -1

B. Minimum number of camera observations

The observability analysis performed so far, assumes that the observation is provided continuously during a given time interval. However, the following property, allows us to obtain necessary conditions on the number of camera observations. Property 9 Let us consider the systems defined in section IV. When the observability of a mode requires the vehicle to move with a non-constant speed, this mode cannot be determined by two camera images. Similarly, when the observability of a mode requires the vehicle to move with a non-constant acceleration, this mode cannot be determined by three camera images.

Proof: The proof is provided in [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF] A consequence of properties 7, 8 and 9 is: Theorem 1 (Minimum number of camera images) In order to estimate the observable modes the camera must perform at least three observations (i.e. the observability requires to have at least three images taken from three distinct camera poses). When the magnitude of the gravitational acceleration (g) is unknown, the minimum number of camera images becomes four.

Proof: The first part of this theorem is a simple consequence of properties 8 and 9. The second part of this theorem is a simple consequence of properties 7 and 9.

In most of cases, the magnitude of the gravitational acceleration (g) is known with good accuracy. Hence, considering the case of unknown gravity, could seem useless. On the other hand, considering this case has a very practical importance (see property 12 at the end of the next section).

VII. CLOSED-FORM SOLUTIONS TO DETERMINE ALL THE OBSERVABLE MODES

We provide closed form solutions which directly express the observable modes in terms of the sensor measurements collected during a short time interval. For the sake of clarity, we start by providing the closed-form solution in the case without gravity (VII-A). Then, we provide the solution in presence of gravity (VII-B) and bias (VII-C). We also discuss the case of multiple features. 5 Mathematically, this can be seen by proving that the expression of the scale factor (i.e. r 2

x + r 2 y + r 2 z ) is not a solution of the partial differential equation associated to w scale s .

A. The case without Gravity 1) Single Feature: We start by discussing the case of one feature. Property 2 states that the sensor data collected during a given time interval contain the information to estimate the vehicle speed and the position of the feature in the local frame. Hence, we start by expressing the dynamics and the observation in this frame. We have:

Ḟ = M F -V V = M V + A ( 17
)
where F is the position of the feature in the local frame and V is the vehicle speed in the same frame. The matrix M depends on the angular speed:

M ≡   0 Ω z -Ω y -Ω z 0 Ω x Ω y -Ω x 0  
The validity of ( 17) can be checked by a direct substitution, i.e. by using F = -q * rq, V = q * vq and by computing their time derivatives by means of ( 8).

In the local frame, the observation in ( 9) is:

h cam = [y 1 , y 2 ] T = F x F z , F y F z T (18)
Let us consider a given time interval, [T 0 , T 0 +T ]. Our goal is to estimate the position of the feature and the vehicle speed in the local frame at T 0 , i.e. F 0 ≡ F (T 0 ) and V 0 ≡ V (T 0 ), by only using the data from the camera and the IM U during the interval [T 0 , T 0 +T ]. The measurements provided by the IM U are usually delivered at a very high frequency (∼ 100 Hz). This allows us to integrate the equations in [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF]. This seems to be useless since we do not know the initial state [F 0 , V 0 ] T . In fact, our goal is to estimate [F 0 , V 0 ] T . The basic idea is the following. We numerically integrate the equations in [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF] by leaving symbolic the unknown components of the initial state. In other words, we obtain for every time t > T 0 the analytical expression of the state [F (t), V (t)] T in terms of its initial value [F 0 , V 0 ] T . The following fundamental property holds:

Property 10 The position of the feature at any time, F (t), linearly depends on the initial feature position, F 0 , and on the initial vehicle speed, V 0 . In other words:

F (t) = C F (t)F 0 + C V (t)V 0 + C B (t) (19)
where C F (t), C V (t) are 3 × 3 matrices and C B (t) is a 3D-vector. In addition, C F (t) and C V (t) only depend on

Ω(τ ), τ ∈ [T 0 , t].
Proof: See appendix B where C F , C V and C B are computed

We consider the components of F (t), i.e. F x (t; F 0 , V 0 ) F y (t; F 0 , V 0 ) and F z (t; F 0 , V 0 ). By using [START_REF] Lupton | Removing scale biases and ambiguity from 6DoF monocular SLAM using inertial[END_REF] we obtain:

F x (t; F 0 , V 0 ) = y 1 (t) F z (t; F 0 , V 0 ) (20)
F y (t; F 0 , V 0 ) = y 2 (t) F z (t; F 0 , V 0 )
These are two independent equations in our six unknowns (which are the components of F 0 and V 0 ). On the basis of property 10, the components of F (t) are linear on the unknowns. Hence, the equations in ( 20) are linear and, by having at least n obs = 3 camera observations, we can easily obtain the initial state [F 0 , V 0 ] T . In [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF] we analyze the case n obs = 3 and we prove that the 6 equations are independent (with the exception of special cases whose probability is zero). Hence, in this case, the components of F 0 and V 0 are obtained by inverting a (6 × 6) matrix. For larger n obs , it suffices to compute the pseudoinverse of a (2n obs × 6) matrix.

2) Multiple Features: Let us consider the case when the camera observes N f features. Let us denote their position in the local frame with F i , i = 0, 1, ..., N f -1. On the basis of property 4, we know that we can estimate the state [F 0 , F 1 , ..., F Nf -1 , V ] whose dynamics are given by [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF] with the first equation repeated for all the features. The camera observation model is the one in [START_REF] Lupton | Removing scale biases and ambiguity from 6DoF monocular SLAM using inertial[END_REF], repeated for all the features. Each camera observation consists of 2N f measurements, y i 1 , y i 2 , i = 0, 1, ..., N f -1. By proceeding as in the case of one feature, we obtain a system of linear equations similar to the one in [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF]. The number of unknowns are now 3N f + 3. By considering n obs camera observations, the number of equations are 2n obs N f . When n obs = 2, we have 4N f equations. For N f ≥ 3 the number of equations is larger than the number of unknowns, i.e. 4N f ≥ 3N f + 3 when N f ≥ 3. On the other hand, on the basis of theorem 1, we know that these equations are not independent. Hence, the minimum number of observations is 3 for any value of N f . However, a higher value of N f will increase the precision of the estimation.

B. The case with Gravity

1) Single Feature: As in the previous subsection, we start by considering the case of a single feature. On the basis of property 3, we know that the sensor data collected during a given time interval, contain the information to estimate the vehicle speed and the position of the feature in the local frame, and, the absolute roll and pitch angles. We express the dynamics and the observation in the local frame. We have:

   Ḟ = M F -V V = M V + A + A g q = mq (21)
where q is the four vector whose components are the components of the quaternion q, i.e. q = [q t , q x , q y , q z ] T . The matrix M is provided in VII-A and the matrix m is:

m ≡ 1 2     0 -Ω x -Ω y -Ω z Ω x 0 Ω z -Ω y Ω y -Ω z 0 Ω x Ω z Ω y -Ω x 0    
A g is the gravitational acceleration in the local frame, i.e. Âg = q * âg q. We remark that, because of the gravity, the first two equations in ( 21) cannot be separated from the equations describing the dynamics of the quaternion, in contrast to the case without gravity.

Let us consider a given time interval, [T 0 , T 0 + T ]. In contrast to the previous case, our goal is now to also estimate the absolute roll and pitch angles at the time T 0 . In other words, the goal is the estimation of the state [F 0 , V 0 , R 0 , P 0 ] T , by only using the data from the camera and the IM U during the interval [T 0 , T 0 + T ]. We proceed as in the previous case. We numerically integrate the equations in ( 21) by leaving symbolic the unknown components of the initial state. On the other hand, the components of q(T 0 ) are not observable since the yaw angle is not observable. In order to proceed as in the previous subsection, we need to know how the position of the feature at the time t, i.e. F (t), depends on [F 0 , V 0 , R 0 , P 0 ] T . We have the following fundamental property, which extends property 10 to the case with gravity:

Property 11 The position of the feature at any time, F (t), linearly depends on the initial feature position, F 0 , on the initial vehicle speed, V 0 , and on the three quantities: χ α ≡ 2g(q t0 q y0 -q x0 q z0 ), χ β ≡ -2g(q t0 q x0 + q y0 q z0 ) and χ γ ≡ 2g(q 2 x0 + q 2 y0 ) -g. In other words:

F (t) = C F (t)F 0 + C V (t)V 0 + C χ (t)χ g + C B (t) (22)
where

χ g ≡ [χ α , χ β , χ γ ] T is the gravity vector in the local frame at time T 0 , C F (t), C V (t), C χ (t) are 3 × 3 matrices and C B (t) is a 3D-vector. In addition, C F (t), C V (t) and C χ (t) only depend on Ω(τ ), τ ∈ [T 0 , t].
Proof: See appendix C where C F , C V , C χ and C B are computed By proceeding as in the case without gravity we obtain the analogous of equations [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF]. The new equations also depend on the vector χ g :

F x (t; F 0 , V 0 , χ g ) = y 1 (t) F z (t; F 0 , V 0 , χ g ) (23) F y (t; F 0 , V 0 , χ g ) = y 2 (t) F z (t; F 0 , V 0 , χ g )
i.e., each camera observation occurred at the time t ∈ [T 0 , T 0 + T ] provides two equations in the nine unknowns (which are the components of F 0 , V 0 and χ g ). On the basis of property 11, the components of F (t) are linear on the unknowns. Hence, the equations in ( 23) are linear and, by having at least n obs = 5 camera observations, we can easily obtain the initial state [F 0 , V 0 , χ g ] T . In particular, when n obs ≥ 5, the components of F 0 , V 0 and χ g are obtained by computing the pseudoinverse of a (2n obs × 9) matrix.

2) Single feature; exploiting additional information: On the basis of property 3, we know that, regarding the vehicle orientation, only the roll and pitch angles are observable modes. Hence, it must be possible to express the components of the vector χ g only in terms of these two angles. In appendix D we provide these expressions. These expressions contain additional information to estimate [F 0 , V 0 , χ g ] T . Indeed, the components of χ g are three but they only depend on two quantities. An important consequence due to this additional information is that it is possible to estimate [F 0 , V 0 , χ g ] T even when the camera only performs n obs = 4 observations. On the other hand, when more than four observations are available (n obs ≥ 5), the expressions in (37) can be adopted to improve the precision. We discuss the case of n obs = 4 observations and we provide a procedure to perform the estimation. When n obs = 4, the equations in [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF] are eight. Hence, it is not possible to determine the components of F 0 , V 0 and χ g by a simple matrix inversion. However, it is possible to prove that these equations are in general independent [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF]. Let us denote by Ax = b the linear system in (23) (i.e., the entries of the nine-dimensional column vector x are the components of the vectors F 0 , V 0 and χ g ). The rank of the matrix A is 8. Let us denote by n the unit vector spanning the null space of A (whose dimension is 1). The linear system Ax = b has infinite solution. Each solution satisfies the following equation: x = A * b+γn, being A * the pseudoinverse of A and γ a scalar number. The determination of γ is obtained by enforcing the constraint that the norm of the vector formed by the last three elements of x is equal to g.

| ¶ (A * b + γn)| = g, ¶ ≡ [0 3×6 , I 3 ] (24)
where 0 n×m is the n × m matrix whose entries are all zero and I 3 is the identity 3 × 3 matrix. The equation in ( 24) is a quadratic polynomial in γ and has two real roots. Hence, we obtain two discrete solutions for x.

In the case we have n obs ≥ 5, the value of x is obtained by using the 2n obs (≥ 10) equations in [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF] (it suffices to compute the pseudoinverse of A, whose dimension is (2n obs ×9)). Then, the equations in (37) are used to obtain the roll and pitch angles. We have:

P = arcsin χ α g , R = -arcsin χ β g 2 -χ 2 α (25)
The procedure described in this case of n obs ≥ 5 does not exploit a possible knowledge of the magnitude of the gravitational acceleration. This can be done by minimizing the cost function:

c(x) = |Ax -b| 2 (26)
under the constraint |χ g | = g. This minimization problem can be solved by using the method of Lagrange multipliers.

3) Multiple Features: Let us consider the case where the camera observes N f features. As in the previous section, we denote their position in the local frame with F i , i = 0, 1, ..., N f -1. On the basis of property 4 we know that we can estimate the state

[F 0 , F 1 , ..., F Nf -1 , V, χ g ]. Each camera observation consists of 2N f measurements, y i 1 , y i 2 , i = 0, 1, ..., N f -1.
By proceeding as in the case of one feature, we obtain a system of linear equations similar to the one in [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF]. The number of unknowns are now 3N f + 6. We have the following property: Property 12 When the number of camera images is less or equal to three (n obs ≤ 3) the rank of the matrix characterizing the linear system in ( 23) is always smaller than the number of unknowns, independently of the number of features.

Proof: According to theorem 1, when n obs = 3 the value of g, i.e. the magnitude of the vector χ g , cannot be determined. Hence, χ g cannot be determined by simply solving the linear system in [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF]. This means that the rank of the matrix characterizing that linear system is always smaller than the number of unknowns Let us consider the case of two points features in three camera images. The unknowns are 12: the position of the two features in the local frame (6 unknowns), the vehicle speed in the local frame (3 unknowns) and the gravity vector in the local frame (3 unknowns). The number of equations in ( 23) is also 12. On the other hand, because of property 12, the rank of the matrix characterizing the linear system in ( 23) is less than 12. In [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF] we prove that this rank is in general equal to 11 with the exception of the following special cases (when it is less than 11):

1) at least one of the camera pose is aligned with the two other features; 2) all the camera poses and the two features belong to the same plane. In general, i.e. when the rank is 11, the estimation can be performed by using the value of g which must be a priori known. Enforcing |χ g | = g is obtained by solving equation [START_REF] Mourikis | A Multi-State Constrained Kalman filter for Vision-aided Inertial Navigation[END_REF], with ¶ = [0 3×(3N f +3) , I 3 ] = [0 3×6 , I 3 ]. Hence, as in the case of a single feature in four images, two distinct solutions are obtained.

Property 12 states that when n obs = 3, the determination of the observable modes cannot be obtained by computing a pseudoinverse also when the number of features is larger than two. On the other hand, it is possible to show that, with the exception of special cases, the observable modes can be determined by enforcing |χ g | = g. Hence, when n obs = 3 and N f ≥ 2, two distinct solutions are in general obtained. When n obs ≥ 4, the determination of the observable modes can be performed by the computation of a pseudoinverse, provided that the number of equations is at least as the number of unknowns and that the vehicle poses and the positions of the features do not satisfy special conditions, whose probability is zero (for instance when all the features and all the camera poses lie on the same plane).

4) Multiple features; exploiting additional information: As discussed in the second part of VII-B2, it is possible to exploit an a priori knowledge of the magnitude of the gravity to improve the precision. The procedure consists of the minimization of the cost function in [START_REF] Scaramuzza | A toolbox for easy calibrating omnidirectional cameras[END_REF], as for the case of one single feature.

C. The Case with Bias

We derive a closed-form solution only when the accelerometers are affected by a bias, i.e. we will consider the case A bias = [0 0 0] T and Ω bias = [0 0 0] T . Indeed, all the matrices appearing in [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF] depend on Ω(τ ) and therefore on Ω bias . Hence, when Ω bias is unknown, the dependence of F (t) on all the unknowns (F 0 , V 0 , χ g and Ω bias ) becomes non linear making more complex their derivation. In contrast, when the bias on the accelerometers is unknown, we obtain the following property, which extends property 11:

Property 13 The position of the feature at any time, F (t), linearly depends on the initial feature position, F 0 , on the initial vehicle speed, V 0 , on χ g and on the bias on the accelerometers A bias . In other words:

F (t) = (27) = C F (t)F 0 + C V (t)V 0 + C χ (t)χ g + C A bias (t)A bias + C B (t)
where χ g ≡ [χ α , χ β , χ γ ] T and C F (t), C V (t), C χ (t), C A bias (t) are 3 × 3 matrices and C B (t) is a 3D-vector. In addition, C F (t), C V (t), C χ (t) and C A bias (t) only depend on Ω(τ ), τ ∈ [T 0 , t].

Proof: See the last paragraph of appendix C By proceeding as in the case without bias we obtain the analogous of equations [START_REF] Mirzaei | A Kalman Filter-based Algorithm for IMU-Camera Calibration: Observability Analysis and Performance Evaluation[END_REF]. The new equations also depend on the vector A bias :

F x (t; F 0 , V 0 , χ g , A bias ) = y 1 (t) F z (t; F 0 , V 0 , χ g , A bias ) (28) F y (t; F 0 , V 0 , χ g , A bias ) = y 2 (t) F z (t; F 0 , V 0 , χ g , A bias )
i.e., each camera observation occurred at the time t ∈ [T 0 , T 0 + T ] provides two equations in the 12 unknowns (which are the components of F 0 , V 0 , χ g and A bias ). On the basis of property 13, the components of F (t) are linear on the unknowns. Hence, the equations in [START_REF] Scaramuzza | 1-Point-RANSAC Structure from Motion for Vehicle-Mounted Cameras by Exploiting Non-Holonomic Constraints[END_REF] are linear and they allow us to determine the state [F 0 , V 0 , χ g , A bias ] T .

VIII. PERFORMANCE EVALUATION

We evaluate the performance of the proposed strategy by using both synthetic and real data. The advantage of simulations is that the ground truth is perfectly known and this allows us a quantitative evaluation of the proposed strategy. We also investigate the accuracy of the proposed approach in the case where the data from the accelerometers are affected by a bias. This will be considered in a single simulation discussed in VIII-A3. In all the other simulations and in the experiments, we assume unbiased inertial measurements.

A. Accuracy of the Algorithm via Monte Carlo Simulations

We simulate many different trajectories in 3D. For all the simulations we use the proposed strategy to estimate the distance of the N f observed features

(d i ≡ |d i -r| = F i , i = 0, 1, ...N f -1), the speed of the camera (v ≡ |v| = v 2 x + v 2 y + v 2 z = V 2 x + V 2 y + V 2 z
) and the roll and the pitch angles (R ≡ arctan(2Q r ) and P ≡ arcsin(2Q p )). Specifically, in all the simulations the values of the estimated d i , v, R, P will be compared with the ground truth values.

1) Simulated Trajectories: The trajectories are generated by randomly generating the linear and angular acceleration of the camera at 100 Hz. In particular, at each time step, the three components of the linear acceleration and the angular speed are generated as Gaussian independent variables whose mean values will be denoted respectively with µ a and µ ω and whose variances will be denoted respectively with σ 2 a and σ 2 ω . By performing many simulations we observed that the precision of the proposed strategy in estimating the roll and pitch angles is almost independent of µ ω , σ 2 ω and σ 2 a . On the other hand, the precision on the estimated d i and v significantly depends on µ a and also depends on σ 2 a . This is not surprising. Indeed, according to property 8, when the camera moves at constant speed, the absolute scale cannot be estimated. Hence, we expect that when µ a becomes smaller the precision on the estimation of d i and v becomes worse. We set the parameters in order to be close to a real case (as in the experiment discussed in VIII-B; see also figure 7 b): σ a = 1 ms -2 , µ ω = 0 deg s -1 and σ ω = 1 deg s -1 . Regarding µ a we considered the following two values µ a = 0 ms -2 and µ a = 0.3 ms -2 . The initial vehicle position is at the origin. We adopt many different values for the initial speed. In the simulations here provided it is set equal to: [0.3, 0.3, 0.3]ms -1 .

2) Simulated Sensors: Starting from the performed trajectory, the true angular speed and the linear acceleration are computed at each time step of 0.01s (respectively, at the time step i, we denote them with Ω true Regarding the camera, the provided readings are generated in the following way. By knowing the true trajectory, the true bearing angles of the feature in the camera frame are computed. They are computed each 0.3s. Then, the camera readings are generated by adding to the true values zero-mean Gaussian errors whose variance is equal to (1 deg) 2 for all the readings.

3) Simulation Results: We start by showing the results related to an illustrative case, where the vehicle performs a 3D trajectory. In particular, the simulated vehicle moves during 100s. Figure 5 a displays the vehicle trajectory together with the position of the point features.

The camera observes all the features whose distance is smaller than 5m. In this simulation, the parameters characterizing the error on the IMU are set as follows: σ gyro = 1 deg s -1 and σ acc = 0.03ms -2 . The number of observations for every estimation is n obs = 8. time interval necessary to perform a single estimation is 2.4s.

Note that the value of the bias is changing very slowly with time and it can be assumed constant during every estimation process. In order to have more quantitative results we performed many simulations. We considered different scenarios by varying the number of observed features (N f ), the values of σ gyro and σ acc , the number of observations n obs and the parameter µ a which characterizes the motion. Regarding N f , we performed simulations with 1 ≤ N f ≤ 10. We found that there is a significant precision improvement by passing from N f = 1 to N f = 2 while, for larger N f , the precision improvement is negligible. For this reason, in this section only the results for N f = 1 and N f = 2 are provided. The position of the features was randomly generated with a uniform distribution on the box centered on the origin and with size 5m. Figure 8 summarizes the results of this investigation by displaying the estimation error vs the number of camera observations (n obs ). 16 subplots are provided. From the bottom to the top they display the error on the pitch angle, the roll angle, the vehicle speed and the distance of the observed features, respectively. From the left to the right they regard the case of N f = 1, µ a = 0 ms -2 , N f = 1, µ a = 0.3 ms -2 , N f = 2, µ a = 0 ms -2 and N f = 2, µ a = 0.3 ms -2 . Every subplot displays 4 distinct curves, which correspond to 4 different settings of the sensor noise (i.e. the values of σ gyro and σ acc ). From the bottom to the top, the sensor noise increase. In particular, from the bottom to the top of every subplot the values are: σ gyro = 0.3 deg s -1 σ acc = 0.01 ms -2 , σ gyro = 1 deg s -1 σ acc = 0.03 ms -2 , σ gyro = 3 deg s -1 σ acc = 0.1 ms -2 and σ gyro = 10 deg s -1 σ acc = 0.3 ms -2 . Each value is computed by running 100 Monte Carlo simulations. Regarding the distance d, the provided error (the four pictures at the top) is averaged on the two features when N f = 2. As stated in section VII-B3, when N f = 2, three observations allow performing the estimation. This is the reason why the smallest n obs is 3 when N f = 2 (the subplots in the last two columns). Regarding the case of a single feature, as explained in section VII-B2, the smallest n obs is 4.

Figure 8 clearly shows that, when the vehicle motion is characterized by a low acceleration (µ a = 0ms -2 , first and third column) the precision on the vehicle speed and on the absolute scale is worse than for the case of higher acceleration (µ a = 0.3ms -2 , second and fourth column). On the other hand, for the roll and the pitch angles, the precision increases by decreasing the acceleration. Finally, we performed Monte Carlo simulations in order to investigate the statistical properties of the noise resulting from the estimation procedure. These simulations clearly show that the proposed procedure is not bias-affected and that the noise is well approximated by a Gaussian distribution. For the sake of brevity, only the case of the roll angle is shown. Figure 9 displays the error distribution together with the best Gaussian fit (solid line). This plot is obtained by counting for each bin of 0.2deg the number of simulations which provide an error on the roll angle falling in the considered bin. Then, the plotted points are normalized by enforcing the area to be 1. The number of simulations is 10 4 . In every simulation, the procedure uses four consecutive camera images and two point features. The variances characterizing the sensors are σ gyro = 10 deg s -1 σ acc = 0.3 ms -2 , i.e. they are set as in the worst case considered in the simulations shown in fig. 8. Similar results have been obtained for the other estimated quantities and by using other noise sensor settings.

B. Performance Evaluation with Real Data

We evaluate the performance of the proposed algorithm by using two distinct data sets, the first is in 2D and the second in 3D. For the sake of brevity, we show the results obtained with the 3D data set. The results obtained with the 2D data set can be found in [START_REF] Martinelli | Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination by Fusing Vision and Inertial Measurements[END_REF].

The data have been provided by the autonomous system laboratory at ETHZ in Zurich. The data are provided together with a reliable ground-truth, which has been obtained by performing the experiments at the ETH Zurich Flying Machine Arena [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF], which is equipped with a Vicon motion capture system. The visual and inertial data are obtained with a monochrome USB-camera gathering 752 × 480 images at 15Hz and a Crossbow VG400CC-200 IMU providing the data at 75 Hz. The camera field of view is 150 deg. The calibration of the camera was obtained by using the omnidirectional camera toolkit by Scaramuzza [START_REF] Scaramuzza | A toolbox for easy calibrating omnidirectional cameras[END_REF]. Finally, the extrinsic calibration between the camera and the IMU has been obtained by using the strategy introduced in [START_REF] Lobo | Relative pose calibration between visual and inertial sensors[END_REF]. The experiment here analyzed lasted for about 250s. accelerometers and three orthogonal gyroscopes. We provided two main contributions:

1) The analytical derivation of all the observable modes, i.e. all the physical quantities that can be determined by only using the information in the sensor data acquired during a short time interval; 2) The analytical derivation of closed-form solutions which analytically express the observable modes in terms of the sensor measurements collected during a very short time interval. The first contribution has been discussed in section V and VI. These sections provide quantitative results in many different contexts, including the case of biased and unbiased inertial measurements, the case of a single and multiple features, and in presence and absence of gravity. In our opinion, there are cases where the provided results are not intuitive. Property 5 states that, by only observing one single feature, there is all the necessary information to determine the speed in the local frame, the position of the feature in the same frame, the absolute roll and pitch angles and the biases affecting the inertial measurements. This is a non intuitive result. In addition, the minimum number of camera images necessary to perform the state determination has been provided.

The second contribution provides closed form expressions which allow us to simultaneously determine all the observable modes without the need of any initialization or a priori knowledge. In particular, only few camera observations are necessary. This is a key advantage since it allows us to quickly recover the observable modes even after a kidnapping. In mobile robotics, and in particular in aerial navigation, this becomes a fundamental advantage. Another important aspect of these closed form expressions is that they can even work by only using a single feature. This allows us to design very efficient and robust computation methods, such as 1-point RANSAC [START_REF] Scaramuzza | Real-Time Monocular Visual Odometry for On-Road Vehicles with 1-Point RANSAC[END_REF], [START_REF] Scaramuzza | 1-Point-RANSAC Structure from Motion for Vehicle-Mounted Cameras by Exploiting Non-Holonomic Constraints[END_REF], to prune false matches and outliers.

The performance of the proposed approach has been evaluated via extensive Monte Carlo simulations and real experiments.

Future works will be devoted to extend the proposed estimation approach by also taking into account varying sensor accuracies in order to give preferential weighting to the more accurate sensor in the results. Additionally, the approach could be extended to incorporate the benefit of a possible previous knowledge on the state. To this regard, we remark that he proposed procedure is not optimal. The mentioned key advantage that it is able to determine the observable modes by only using the sensor data provided during a short interval, has the drawback that it does not exploit a possible previous information on these modes. We also want to analytically investigate the independence of the equations in the closed form solutions in presence of bias. In particular, we want to investigate the cases when the number of observations and features are the minimum required to perform the estimation on the basis of the observability analysis. Currently, this analysis has been done in the case without bias (section VII-B2).

APPENDIX A NUMBER OF INDEPENDENT LIE DERIVATIVES FOR THE SYSTEM ANALYZED IN V-A

The system is characterized by the state: [r v q] T , whose dimension is 10. The dynamics are given in (8) (without the term âg , since we are considering the case g = 0) and the observations are given in ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF]. In order to compute the Lie derivatives, we need to express the dynamics as in (4). We have L = 6 and the six inputs are the three components of the acceleration, A, and the three components of the angular speed, Ω. Hence: u 1 = A x , u 2 = A y , u 3 = A z , u 4 = Ω x , u 5 = Ω y , u 6 = Ω z . The seven vector functions f 0 , f 1 , ..., f 6 are:

f 0 = [v x , v y , v z , 0 7 ] T f 1 = [0 3 , q 2 t +q 2
x -q 2 y -q 2 z , 2q t q z +2q y q x , -2q t q y +2q z q x , 0 4 ] T f 2 = [0 3 , -2q t q z +2q y q x , q 2 t +q 2 y -q 2 z -q 2 x , 2q t q x +2q z q y , 0 4 ] T f 3 = [0 3 , 2q t q y +2q z q x , -2q t q x +2q z q y , q 2 t +q 2 z -q 2 x -q 2 y , 0 4 ] T f 4 = [0 6 , -1/2q x , 1/2q t , 1/2q z , -1/2q y ] T f 5 = [0 6 , -1/2q y , -1/2q z , 1/2q t , 1/2q x ] T f 6 = [0 6 , -1/2q z , 1/2q y , -1/2q x , 1/2q t ] T where we denoted with 0 n the vector line whose dimension is n and whose entries are all zeros.

We must compute the Lie derivatives of all the three observations function given in ( 9) and [START_REF] Jones | Inertial Structure From Motion with Autocalibration[END_REF] with respect to all the vector fields. By a direct computation, performed by using the symbolic Matlab computational tool, we were able to find the following 7 independent Lie derivatives:

L 0 y 1 , L 0 y 2 , L 0 h const , L 1 f 0 y 1 , L 1 f 0 y 2 , L 2 f 0 , f 0 y 1 , L 2 f 0 , f 1 y 1 .
We know that we cannot have more than 7 independent Lie derivatives (otherwise, we would have less than three symmetries). Hence, the number of independent Lie derivatives is 7. We use these expressions to obtain χ α = 2g(q t q y -q x q z ), χ β = -2g(q t q x + q y q z ) and χ γ = 2g(q 2 x + q 2 y ) -g in terms of the roll, pitch and yaw angles. As expected on the basis of property 3, they only depend on the roll and pitch angles. By a direct substitution we obtain: χ α = g sin P, χ β = -g sin R cos P, χ γ = -g cos R cos P (37)

q t =

Fig. 1 .

 1 Fig. 1. A vehicle equipped with an accelerometer and a camera moves on a line. The camera performs three observations of the feature in F , repsectively from the points A, B and C.

Fig. 2 .

 2 Fig. 2. A simple localization problem. The robot is equipped with odometry and bearing sensors able to evaluate the angle β. In b, the three initial robot configurations are compatible with the same initial observation (β).

Fig. 3 .

 3 Fig. 3. In a the two initial positions (A and B) do not reproduce the same observations (β A = β B ). In b the two indicated trajectories provide the same bearing observations at any time.

Fig. 4 .

 4 Fig. 4. The feature position (F ), the vehicle acceleration (Av) the vehicle angular speed (Ω) and the gravitational acceleration (Ag).

i

  and A true v i ). Starting from them, the IMU sensors are simulated by randomly generating the angular speed and the linear acceleration at each step according to the following:Ω i = N Ω true i , P Ωi and A i = N A true v i -A gi -A biasi , P Ai where: • N indicates the Normal distribution whose first entry is the mean value and the second its covariance matrix; • P Ωi and P Ai are the covariance matrices characterizing the accuracy of the IM U ; • A gi is the gravitational acceleration in the local frame and A bias i is the bias affecting the data from the accelerometer. In all the simulations we set both the matrices P Ωi and P Ai diagonal and in particular: P Ωi = σ 2 gyro I 3 and P Ai = σ 2 acc I 3 , where I 3 is the identity 3 × 3 matrix. We considered several values for σ gyro and σ acc , in particular: σ gyro ∈ [0.3, 10] deg s -1 and σ acc ∈ [0.01, 0.3]ms -2 .

Fig. 5 .

 5 Fig. 5. In a: typical 3D motion generated in our simulations; the red stars indicate the point features. In b: the true (blue dots) and the estimated (red disks) vehicle speed.

Fig. 6 .Fig. 7 .

 67 Fig. 6. Roll (left) and pitch (right) angles during the simulated experiment. The blue dots are the ground truth and the red disks the estimated values.

Fig. 8 .

 8 Fig. 8. Error on the observable modes averaged on 100 simulations.

Fig. 9 .

 9 Fig. 9. Distribution of the noise on the roll angle (blue line) and its best Gaussian fit (red line).

Figure 10 aFig. 10 .

 1010 Fig. 10. In a: the trajectory (ground truth) in the 3D real data set during the time interval [200, 240]s. In b: the vehicle speed in the real 3D experiment. Blue dots are the ground truth and red disks the estimated values.

Fig. 11 .

 11 Fig. 11. Roll (left) and pitch (right) angles in the real 3D experiment. Blue dots are the ground truth and red disks the estimated values.

Throughout this paper, we will adopt the term IMU (Inertial Measurement Unit) to indicate the sensor assembling constituted by three orthogonal accelerometers and three orthogonal gyroscopes.

Throughout this paper, we will adopt the term camera observation to mean the bearing measurements provided by the camera from a single pose, i.e. obtained by a single image.

In section V we will see that sometimes the symmetries can easily be derived from physical considerations, i.e. by remarking the system invariance under several transformations. This allows us to avoid the computation of high order Lie derivative

L 2 f 0 , f 0 y 1 , L 2 f 0 , f 1 y 1 , L 2 f 0 , f 4 y 1 , L 2 f 0 , f 0 y 2 , L 2 f 0 , f 4 y 2 , L 2 f 0 , f 5 y 2 , L 3 f 0 , f 0 , f 5 y 1 , L 3 f 0 , f 0 , f 6 y 1 , L 3 f 0 , f 0 , f 2 y 2 ,

Note that this invariance corresponds to the continuous symmetry: w scale s = [rx, ry, rz, vx, vy, vz, 0, 0, 0, 0] T , which would have been obtained by the Lie derivatives and definition 1.

This work was supported by the European Project FP7-ICT-2007-3.2.2 Cognitive Systems, Interaction, and Robotics under the contract #231855 (sFLY). We also acknowledge the Autonomous System Lab at ETHZ in Zurich for providing us a 3D data set which includes a very reliable ground truth.

COMPUTATION OF F (t) AND V (t) IN THE CASE WITHOUT

GRAVITY

We provide the expression of F (t) and V (t) in terms of the the initial values F (T 0 ) = F 0 and V (T 0 ) = V 0 and the acceleration A(τ ) and angular speed Ω(τ ), τ ∈ [T 0 , t].

By discretizing the second equation in [START_REF] Lupashin | A simple learning strategy for high-speed quadrocopter multi-flips[END_REF] and by denoting with j the j th time step (corresponding with t j ), we obtain V j = (I 3 + M j dt j )V j-1 + A j dt j , where M j is the matrix M provided in section VII at the time step j, I 3 is the identity matrix 3 × 3 and dt j = t j -t j-1 .

The previous expression for V j provides the following expression in terms of the initial conditions:

where:

is the rotation matrix between the local frame at time T 0 and the local frame at time t j . In the same way we finally obtain the expression of F j in terms of the initial conditions:

Hence, we have

We provide the expression of F (t) and V (t) in terms of the the initial values F (T 0 ) = F 0 , V (T 0 ) = V 0 , q(T 0 ) = q 0 and the acceleration A(τ ) and angular speed Ω(τ ), τ ∈ [T 0 , t]. As we will see, the dependence on the initial quaternion q 0 is only through the three quantities: χ α ≡ 2g(q t0 q y0 -q x0 q z0 ), χ β ≡ -2g(q t0 q x0 +q y0 q z0 ) and χ γ ≡ 2g(q 2 x0 +q 2 y0 )-g, which are the component of the gravity vector in the local frame at time T 0 . In addition, this dependence is linear as it is linear the dependence on F 0 and V 0 .

Before integrating the second equation in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], as in the appendix B, we consider the new term A g , which depends on the quaternion. In particular, we separate in this term the time-dependent part from the part which is time-independent. Specifically, we introduce the quaternion p(t) such that q(t) = q 0 p(t): Âg (t) = q(t) * âg q(t) = p(t) * q * 0 âg q 0 p(t). p(t) satisfies the same time differential equation as q(t), i.e. ṗ = 1 2 p Ω, but, p(0) = 1. Let us denote with χ g the 3D vector associated with the quaternion q * 0 âg q 0 , i.e. χg ≡ q * 0 âg q 0 . By a direct computation we obtain:

and A g (t) = Ξ(t)χ g , where Ξ(t) is given in [START_REF] Veth | Fusing low-cost image and inertial sensors for passive navigation[END_REF]. We integrate the second equation in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], obtaining:

where

The previous expression for V j provides the following expression in terms of the initial conditions:

In the same way we finally obtain the expression of F j in terms of the initial conditions:

Hence, we have:

dt k and the matrix Ξ j , given in [START_REF] Veth | Fusing low-cost image and inertial sensors for passive navigation[END_REF], is computed by only using the gyro's measurements in the time-interval

In the case where the tri-axial accelerometer is affected by a bias, the derivation of the expression of F j is very similar. The only difference is that in (33), the term B j also includes the bias A bias . In particular we have B j = A j +Ξ j χ g + A bias . The expression of F j differs from the one in (36) since it includes a new term:

THE ROLL AND PITCH ANGLES

Let us consider the unit quaternion: q t + q x i + q y j + q z k. By denoting with R, P and Y respectively the roll, pitch and yaw angles, we have [START_REF]Quaternions and rotation Sequences: a Primer with Applications to Orbits, Aerospace, and Virtual Reality[END_REF]: