Agostino Martinelli
email: agostino.martinelli@inria.fr

Alessandro Renzaglia
email: alessandro.renzaglia@inria.fr

Alexander Oliva
email: alexander.oliva@inria.fr

Cooperative Visual-Inertial Sensor Fusion: Fundamental Equations and State Determination in Closed-Form

Keywords: Visual-Inertial Sensor Fusion, Observability, Cooperative Sensor Fusion, Closed-Form Solution

 to the cooperative case. The impact of the presence of the bias on the performance of this closedform solution is also investigated and a simple and effective method to obtain the gyroscope bias is proposed. Extensive simulations clearly show that the proposed method is successful. It is worth noting that it is possible to automatically retrieve the absolute scale and simultaneously calibrate the gyroscopes not only with-

Introduction

When a team of mobile robots cooperates to fulfill a task, an optimal localization strategy must take advantage of relative observations (detection of other robots). This problem has been considered in the past by following different approaches and it is often referred as Cooperative Localization. In Cooperative Localization (CL), several communicating robots use relative measurements (such as distance, bearing and orientation between the robots) to jointly estimate their poses. This problem has been investigated for a long time and several approaches have been introduced in earlier works [START_REF] Grabowski | Heterogeneous Teams of Modular Robots for Mapping and Exploration[END_REF][START_REF] Howard | Localization for Mobile Robot Teams Using Maximum Likelihood Estimation[END_REF][START_REF] Kato | Identifying and Localizing Robots in a Multi-Robot System Environment[END_REF][START_REF] Martinelli | Multi-Robot Localization Using Relative Observations[END_REF][START_REF] Rekleitis | Multi-robot cooperative localization: a study of trade-offs between efficiency and accuracy[END_REF][START_REF] Roumeliotis | Distributed Multirobot Localization[END_REF][START_REF] Spletzer | A Bounded Uncertainty Approach to Multi-Robot Localization[END_REF]. Then, a great effort has been devoted to decentralize the computation among the team members and, simultaneously, to minimize the communication among the robots without deteriorating the localization performance [3, 14, 20-22, 25, 27, 43]. Specific cases of cooperative localization have been considered both in 2D and in 3D.

For instance, in the framework of Micro Aerial Vehicles (MAV), a critical issue is to limit the number of on-board sensors to reduce weight and power consumption. Several methods consider the use of bearing-only sensors [START_REF] Michieletto | Bearing rigidity theory in SE (3)[END_REF][START_REF] Philippe | I see you, you see me: Cooperative localization through bearing-only mutually observing robots[END_REF][START_REF] Tron | Rigid Components Identification and Rigidity Enforcement in Bearing-Only Localization using the Graph Cycle Basis[END_REF][START_REF] Zelazo | Bearing-only formation control using an SE(2) rigidity theory[END_REF] or only range measurements [START_REF] Zhou | Robot-to-robot relative pose estimation from range measurements[END_REF]. A common setup is otherwise to combine a monocular camera with an Inertial Measurements Unit (IMU). On top of being cheap, these sensors have very interesting complementarities. Additionally, they can operate in indoor environments, where Global Positioning System (GPS) signals are shadowed.

The problem of fusing visual and inertial data for single robots has been extensively investigated in the past [START_REF] Armesto | Fast ego-motion estimation with multi-rate fusion of inertial and vision[END_REF][START_REF] Forster | SVO: Fast semi-direct monocular visual odometry[END_REF][START_REF] Huang | On the complexity and consistency of UKF-based SLAM[END_REF][START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF][START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF]. Recently, this sensor fusion problem has been successfully addressed by enforcing observability constraints [START_REF] Hesch | Consistency analysis and improvement of vision-aided inertial navigation[END_REF][START_REF] Huang | Towards consistent visual-inertial navigation[END_REF], and by using optimizationbased approaches [START_REF] Forster | IMU preintegration on manifold for efficient visualinertial maximum-a-posteriori estimation[END_REF][START_REF] Huang | An observability-constrained sliding window filter for slam[END_REF][START_REF] Indelman | Information fusion in navigation systems via factor graph based incremental smoothing[END_REF][START_REF] Leutenegger | Keyframe-based visual-inertial odometry using nonlinear optimization[END_REF][START_REF] Lupton | Visual-inertial-aided navigation for high-dynamic motion in built environments without initial conditions[END_REF][START_REF] Mourikis | A multi-state constraint Kalman filter for vision-aided inertial navigation[END_REF][START_REF] Mourikis | A dual-layer estimator architecture for long-term localization[END_REF]. These optimization methods outperform filter-based algorithms in terms of accuracy due to their capability of relinearizing past states. On the other hand, the optimization process can be affected by the presence of local minima. For this reason, a closed-form solution able to automatically determine the state without initialization has been introduced [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF][START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF].

Visual and inertial sensors have also been used in a cooperative scenario to estimate the relative state [START_REF] Achtelik | Collaborative stereo[END_REF] and for cooperative mapping [START_REF] Guo | Large-Scale Cooperative 3D Visual-Inertial Mapping in a Manhattan World[END_REF]. However, in the cooperative case, a solution able to automatically determine the state without initialization (as in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF][START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF]) is still missing.

Any estimation approach, either filter based or optimization based, is built upon the fundamental equations that fully characterize the considered sensor fusion problem. These equations are the differential equations that describe the dynamics of the observable state together with the equations that express the observations in terms of this observable state. Hence, to successfully solve a given estimation problem, the first step to be accomplished is the determination of the observable state. Regarding the single-agent visual-inertial sensor fusion problem, this state has been analytically derived by many authors and it consists of the absolute scale, the speed expressed in the local frame and the absolute roll and pitch angles. This result even holds if only a single point feature is available in the environment.

In this paper we study the visual-inertial sensor fusion problem in the cooperative case. We investigate the extreme case where no point features are available. Additionally, we consider the critical case of only two agents. In other words, we are interested in investigating the minimal case. If we prove that the absolute scale is observable, we can conclude that it is observable in all the other cases. Each agent is equipped with an Inertial Measurement Unit (IMU) and a monocular camera. By using the monocular camera, each agent can observe the other agent. Note that, we do not assume that these camera observations contain metric information (due for instance to the known size of the observed agent). The two agents can operate far from each other and a single camera observation only consists of the bearing of the observed agent in the frame of the observer. In other words, each agent acts as a moving point feature with respect to the other agent.

The first questions we wish to answer are: Is it possible to retrieve the absolute scale in these conditions?

And the absolute roll and pitch angles? More generally, we want to determine the entire observable state, i.e., all the physical quantities that it is possible to determine by only using the information contained in the sensor data (from the two cameras and the two IMUs) during a short time interval. In [START_REF] Martinelli | Cooperative Visual-Inertial Sensor Fusion: Fundamental Equations[END_REF] we provided the answers to these questions in the case when the inertial measurements are unbiased. These results are provided in section 3. Then, in section 5, we provide a full answer even in presence of biased measurements (both the ones from the accelerometers and the ones from the gyroscopes) and we also obtain that it suffices that only one agent is equipped with a camera. In addition, it suffices that this camera is a linear camera, i.e., which only provides the azimuth of the other agent in its local frame.

Note that part of these questions have already been answered in [START_REF] Achtelik | Collaborative stereo[END_REF]. However, the results here provided in sections 3-5 are more general for the following reasons:

-They account for the bias on all the inertial measurements (both on the accelerometers and the gyroscopes); -As mentioned above, we also prove that the same observability properties hold when only one of the agents is equipped with a camera and that this single camera can even be a linear camera; -In [START_REF] Achtelik | Collaborative stereo[END_REF] it is proved that the relative state is observable while here it is also proved that no other states are observable (e.g., the absolute roll and pitch of each agent is unobservable); -In [START_REF] Achtelik | Collaborative stereo[END_REF] it is assumed that the camera directly provides the relative position (up to a scale) and the relative orientation. In our derivation we do not require the latter assumption. In other words, it suffices that the camera detects one single point on the observed agent (which represents the origin of its local frame). This does not require more restrictive assumptions (e.g., the two agents can operate very far from each other).

In section 4 we provide the basic equations that describe the cooperative visual-inertial sensor fusion problem. These equations are:

-The differential equations that describe the dynamics of the observable state expressed only in terms of the components of the observable state and the accelerations and the angular speeds (i.e., the quantities measured by the two IMUs);

-The equations that provide the analytic expression of the two camera observations in terms of the components of the observable state.

These are the fundamental equations that fully characterize the problem of fusing visual and inertial data in the cooperative case. These equations can then be used to build any method (e.g., filter-based or optimizationbased) to carry out the state estimation. In [START_REF] Martinelli | Cooperative Visual-Inertial Sensor Fusion: Fundamental Equations[END_REF] we used them to introduce an EKF-based estimation method.

Note that an EKF-based estimation method was also introduced in [START_REF] Achtelik | Collaborative stereo[END_REF]. In that work, the authors did not need the equations derived in section 4 because, as mentioned above, they used a more restrictive camera model, which assumes that the camera also provides the relative orientation (we relax this assumption). In this paper we use these fundamental equations to obtain a closed-form determination of the observable state in terms of the measurements delivered during a short time interval by the cameras and the IMUs that belong to the two agents. This solution is provided in section 6. This is precisely the extension of the closed-form solution in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] to the cooperative case. For clarity sake, in section 6 we directly provide the solution by addressing the reader to the appendix B for its analytic derivation (and to [START_REF] Martinelli | Closed-form Solution to Cooperative Visual-Inertial Structure from Motion[END_REF] for further technical details). Then, the paper demonstrates the efficiency of this solution. A closed-form solution directly returns the state in terms of the measurements collected during a short time interval and, thus, does not require any initialization. We perform simulations with plausible MAV motions and synthetic noisy sensor data (section 7). This allows us to identify limitations of the solution and bring modifications to overcome them. In practice, we perform exactly the same investigation done in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF] for the case of a single agent. Specifically, we investigate the impact of biased inertial measurements. We show that a large bias on the accelerometer does not significantly worsen the performance (section 7.5). One major limitation is the impact of biased gyroscope measurements (section 7.6). In other words, the performance becomes very poor in presence of a bias on the gyroscopes of the two agents and, in practice, the overall method can only be successfully used with very precise -and expensive -gyroscopes. In section 8, we introduce a simple method that automatically estimates both these biases. By adding this new method for the bias estimation to the solution presented in section 6, we obtain results that are equivalent to the ones in absence of bias (section 8.1). Note that the implementation of the closed-form solution requires that two MAVs observe one each other. This could seem restrictive since most MAVs do not have omni-directional cameras, but rather a front fac-ing camera with a limited field of view. However, the big advantage of a closed form solution is that, if at a given time it fails (loss of visual contact or any other unmodeled event), this does not have any impact on its performance at successive times. In addition, it suffices that the two MAVs observe one each other not more than 10 times during a time period of not more than 4 seconds.

Finally, it is important to note that, even though in this paper we particularly focus on multi-MAV systems, this method is suitable for any kind of robots moving in 3D that operate in extreme conditions (e.g., GPSdenied environments, absence of point features, etc.) and need to recover the absolute scale in few seconds. The solution does not need initialization. Additionally, it is robust to the bias and automatically calibrates the gyroscopes.

The system

We consider two vehicles that move in a 3D environment. Each vehicle is equipped with an Inertial Measurement Unit (IMU), which consists of three orthogonal accelerometers and three orthogonal gyroscopes. Additionally, each vehicle is equipped with a monocular camera. We assume that, for each vehicle, all the sensors share the same frame. Without loss of generality, we define the vehicle local frame as this common frame. The accelerometer sensors perceive both the gravity and the inertial acceleration in the local frame. The gyroscopes provide the angular speed in the local frame. Finally, the monocular camera of each vehicle provides the bearing of the other vehicle in its local frame (see Fig. 1 for an illustration). Additionally, we assume that the z-axis of the global frame is aligned with the direction of the gravity. We adopt the following notations:

r1 = [r 1

x , r 1 y , r 1 z ] and r2 = [r 2

x , r 2 y , r 2 z ] are the positions of the two vehicles in the global frame;

-v 1 = [v 1 x , v 1 y , v 1 z ] and v 2 = [v 2 x , v 2 y , v 2
z ] are the velocities of the two vehicles in the global frame; q 1 = q 1 t + q 1 x i + q 1 y j + q 1 z k and q 2 = q 2 t + q 2 x i + q 2 y j + q 2 z k are the two unit quaternions that describe the rotations between the global and the two local frames, respectively 1 .

In the following, for each vector defined in the 3D space, the subscript q will be adopted to denote the corresponding imaginary quaternion. For instance, regarding the position of the first vehicle, we have: r 1 q = 0 + r 1 x i + r 1 y j + r 1 z k. Additionally, we denote by A 1 , A 2 , Ω 1 and Ω 2 the following physical quantities:

-A rob = [A rob
x , A rob y , A rob z ], (rob = 1, 2), is the vehicle acceleration perceived by the IMU mounted on the first and the second vehicle (this includes both the inertial acceleration and gravity);

-Ω rob = Ω rob x Ω rob y Ω rob z , (rob = 1, 2)
, is the angular speed of the first and the second vehicle expressed in the respective local frame (and

Ω rob q = 0 + Ω rob x i + Ω rob y j + Ω rob z k).
The dynamics of the first/second vehicle are:

     ṙrob q = v rob q vrob q = q rob A rob q (q rob ) * -gk qrob = 1 2 q rob Ω rob q (1)
where g is the magnitude of the gravity, rob = 1, 2, and k is the fourth fundamental quaternion unit (k = 0 + 0 i + 0 j + 1 k).

The monocular camera on the first vehicle provides the position of the second vehicle in the local frame of the first vehicle, up to a scale. The position of the second vehicle in the local frame of the first vehicle is given by the three components of the following imaginary quaternion:

p 1 q = (q 1 ) * (r 2 q -r 1 q ) q 1 . (2)
Hence, the first camera provides the quaternion p 1 q up to a scale. For the observability analysis, it is convenient to use the ratios of its components:

h 1 [h 1 u , h 1 v ] T = [p 1 q ] x [p 1 q ] z , [p 1 q ] y [p 1 q ] z T ( 3
)
where the subscripts x, y and z indicate respectively the i, j and k component of the corresponding quaternion.

Similarly, the second camera provides:

h 2 [h 2 u , h 2 v ] T = [p 2 q ] x [p 2 q ] z , [p 2 q ] y [p 2 q ] z T ( 4
)
where p 2 q is the imaginary quaternion whose three components are the position of the first vehicle in the local frame of the second, namely:

p 2 q = (q 2 ) * (r 1 q -r 2 q ) q 2 . ( 5
)
Note that, using the ratios in ( 3) and ( 4) as observations can generate problems due to singularities and, when the camera measurements are used to estimate a state, it is more preferable to adopt different quantities (e.g., the two bearing angles, i.e., the azimuth and the zenith). For the observability analysis, this problem does not arise.

Observable state

The goal of this subsection is to obtain the entire observable state for the system defined in section 2. First of all, we characterize this system by the following state:

X = [(r 1 ) T , (v 1 ) T , q 1 , (r 2 ) T , (v 2 ) T , q 2 ] T . (6)
The dimension of this state is equal to 20. Actually, the components of this state are not independent. Both q 1 and q 2 are unit quaternions. In other words, we have:

(q 1 t ) 2 + (q 1 x ) 2 + (q 1 y ) 2 + (q 1 z ) 2 = (q 2 t ) 2 + (q 2 x ) 2 + (q 2 y ) 2 + (q 2 z ) 2 = 1 . (7)
The dynamics of the state defined in [START_REF] Grabowski | Heterogeneous Teams of Modular Robots for Mapping and Exploration[END_REF] are given by [START_REF] Achtelik | Collaborative stereo[END_REF]. The observation functions are the four scalar functions

h 1 u h 1 v h 2 u h 2 v
given by equations [START_REF] Armesto | Fast ego-motion estimation with multi-rate fusion of inertial and vision[END_REF][START_REF] Carrillo-Arce | Decentralized multi-robot cooperative localization using covariance intersection[END_REF][START_REF] Forster | SVO: Fast semi-direct monocular visual odometry[END_REF][START_REF] Forster | IMU preintegration on manifold for efficient visualinertial maximum-a-posteriori estimation[END_REF]. Additionally, we need to add the two observation functions that express the constraint that the two quaternions, q 1 and q 2 , are unit quaternions. The two additional observations are:

h rob const (X) (q rob t ) 2 + (q rob x ) 2 + (q rob y ) 2 + (q rob z ) 2 = 1 (8) with rob = 1, 2.
We investigate the observability properties of this system. Since both the dynamics and the six observations are nonlinear with respect to the state, we use the observability rank condition in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]. The dynamics are affine in the inputs, i.e., they have the expression

Ẋ = f 0 (X) + 12 i=1 f i (X)u i (9)
where u i are the system inputs, which are the quantities measured by the two IMUs. Specifically, we set:

u 1 , u 2 , u 3 the three components of A 1 ; u 4 , u 5 , u 6 the three components of Ω 1 ; u 7 , u 8 , u 9 the three components of A2 ; u 10 , u 11 , u 12 the three components of Ω 2 .

Then, by comparing (1) with ( 9) it is immediate to obtain the analytic expression of all the vector fields

f 0 , f 1 , • • • f 12 ;
for instance, we have:

f 0 = [v 1 x , v 1 y , v 1 z , 0, 0, -g, 0 4 , v 2 x , v 2 y , v 2 z , 0, 0, -g, 0 4 ] T f 1 = [0 3 , (q 1 t ) 2 + (q 1 x ) 2 -(q 1 y ) 2 -(q 1 z ) 2 , 2(q 1 t q 1 z + q 1 x q 1 y ), 2(q 1 x q 1 z -q 1 t q 1 y ), 0 14 ] T f 4 = 1 2 [0 6 , -q 1 x , q 1 t , q 1 z , -q 1 y , 0 10 ] T
where 0 n is the n-line zero vector.

For systems with the dynamics given in ( 9) the application of the observability rank condition can be automatically done by a recursive algorithm. In particular, this algorithm automatically returns the observable codistribution 2 by computing the Lie derivatives of all the system outputs along all the vector fields that characterize the dynamics. In the following, we provide a very simple description of the observability rank condition for systems with the dynamics given in (9), i.e., dynamics nonlinear in the state and affine in the inputs (for a detailed description the reader is addressed to the first chapter of [START_REF] Isidori | Nonlinear Control Systems[END_REF]). In accordance with the observability rank condition, the observable codistribution provides all the observability properties. The dimension of this vector space (the observable coditribution) cannot exceed the dimension of the state X. If this dimension is equal to the dimension of the state X, this means that the entire state is observable (actually, weakly locally observable [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]). If this dimension is smaller than the dimension of the state X, the entire state is not observable and it is possible to detect the observable states by computing its Killing vectors in order to obtain the system symmetries [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. The recursive algorithm that returns the observable codistribution, for systems with the dynamics given in [START_REF] Hesch | Consistency analysis and improvement of vision-aided inertial navigation[END_REF], is the following:

Algorithm 1 Observable codistribution Λ Set Λ 0 = span{∇h 1 u , ∇h 1 v , ∇h 2 u , ∇h 2 v , ∇h 1 const , ∇h 2 const } while Λ m = Λ m-1 do Λ m = Λ m-1 + L f0 Λ m-1 + 12 i=1 L fi Λ m-1 end while
where Λ m , with m ≥ 1, is the codistribution at the mth step and the symbol ∇ denotes the gradient with respect to the state X. We remind the reader that the Lie derivative of a scalar function h(X) along the vector field f (X) is defined as follows:

L f h ∇h • f
which is the product of the row vector ∇h with the column vector f . Hence, it is a scalar function. Additionally, by definition of Lie derivative of covectors, we have: L f ∇h = ∇L f h. Finally, given two vector spaces V 1 and V 2 , we denoted by V 1 + V 2 their sum, i.e., the span of all the generators of both V 1 and V 2 .

In [START_REF] Isidori | Nonlinear Control Systems[END_REF] it is proved that algorithm 1 converges. In particular, it is proved that it has converged when Λ m = Λ m-1 . An interesting consequence of this result is that the convergence is achieved in at most n-1 steps, where n is the dimension of the state (see lemmas 1.9.1, 1.9.2 and 1.9.6 in [START_REF] Isidori | Nonlinear Control Systems[END_REF]).

We provide few insights to figure out how Algorithm 1 works and in particular how it can be implemented in practice. As we mentioned above, the observable codistribution is the span of line vectors. In practice, Algorithm 1 builds a matrix whose lines are these vectors (e.g., at the first step we include the six lines:

∇h 1 u , ∇h 1 v , ∇h 2 u , ∇h 2 v , ∇h 1 const , ∇h 2 const ).
At each subsequent step, we include a new set of lines and we compute the rank of the matrix (the new set of lines is obtained by computing the Lie derivatives of all the lines of the matrix along all the directions allowed by the dynamics, i.e., along all the vector fields f 0 , f 1 , • • • f 12 ). The algorithm has converged when the rank of the matrix remains equal to the rank of the matrix at the previous step. Note that each line will be a symbolic function of the state. To compute the rank we use the symbolic tool of MATLAB. In particular, we use the functions "rank" and "null". The latter provide the killing vectors of the matrix which are precisely the symmetries of the system, once the algorithm has converged [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]. If the set of the killing vectors of the matrix only consists of the null vector, this means that the systems does not have any symmetry and the entire state is observable.

For the specific case, we obtain that the algorithm converges at the third step, i.e., the observable codistribution is the span of the differentials of the previous Lie derivatives up to the second order. In particular, its dimension is 11 and, a choice of eleven Lie derivatives is:

L 0 h 1 u , L 0 h 1 v , L 0 h 2 u , L 0 h 2 v , L 0 h 1 const , L 0 h 2 const , L 1 f0 h 1 u , L 1 f0 h 1 v , L 1 f0 h 2 u , L 2 f0f0 h 1 u , L 2 f0f1 h 1 u
3 . Once we have obtained the observable codistribution, the next step is to obtain the observable state. This state has eleven components. Obviously, a possible choice would be the state that contains the previous eleven Lie derivatives. On the other hand, their expression is too complex and it is much more preferable to find an easier state, whose components have a clear physical meaning. By analytically computing the continuous symmetries of our system (i.e., the Killing vectors of the previous observable codistribution, [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]), we detect the following independent observable modes:

-The position of the second vehicle in the local frame of the first vehicle (three observable modes); -The velocity of the second vehicle in the local frame of the first vehicle (three observable modes); -The three Euler angles that characterize the rotation between the two local frames (three observable modes); -Trivially, the norm of the two quaternions (two observable modes).

Therefore, we can fully characterize our system by a state whose components are the previous observable modes. It must be possible to express the dynamics of this state only in terms of its components and the twelve system inputs. Additionally, also the camera observations must be expressed only in terms of these nine components. This is actually trivial, since the first camera provides the first three components of this state, up to a scale. The second camera, provides the same unit vector rotated according to the previous three Euler angles. Regarding the dynamics, its derivation is a bit more complex. We provide all these analytic expressions in the next section. We conclude this section with the following three important remarks.

-The absolute roll and pitch angles of each vehicle are not observable. This is a consequence of the fact that no feature in the environment has been considered. The observation consists only of the bearing angles of each vehicle in the local frame of the other vehicle.

The presence of the gravity, which determines the observability of the absolute roll and pitch in the case of a single vehicle, acts in the same way on the two IMUs and its effect on the system observability vanishes, since it cannot be distinguished from the inertial acceleration. 3 Higher order Lie derivatives are recursively computed. For instance, for the second order Lie derivative L 2 f0f1 h we have

L 2 f0f1 h = ∇(L f0 h) • f 1 = [∇(∇h • f 0 )] • f 1
-The choice of the above 11 independent Lie derivatives is not unique. In particular, it is possible to avoid the Lie derivatives of the functions that correspond to one of the two cameras (e.g., h 2 u and h 2 v ). This means that we obtain the same observability properties when only one of the MAVs is equipped with a camera. In addition, it is also possible to avoid the Lie derivatives of the function h 1 v . This means that we obtain the same observability properties when only one MAV is equipped with a camera and this camera is a linear camera able to only provide the azimuth of the other MAV in its local frame. In section 5 we obtain that the same result holds even in presence of a bias on the inertial measurements (see also appendix A for computation details).

-In order to have 11 eleven independent Lie derivatives, at least one of them must be computed along a direction that corresponds to one of the axes of at least one of the two accelerometers (i.e., one direction among f 1 , f 2 , f 3 , f 7 , f 8 and f 9 ). Any selection that does not include at least one of them provides a codistribution whose dimension is smaller than 11. In particular, there will be a symmetry for this codistribution that corresponds to a scale invariance. This means that a necessary condition for the observability of the absolute scale is that the relative acceleration between the two MAVs does not vanish. Note that this same condition was found in [START_REF] Achtelik | Collaborative stereo[END_REF] (the fact that in [START_REF] Achtelik | Collaborative stereo[END_REF] the camera is assumed to directly provide the relative orientation does not impact the observability of the scale).

Fundamental Equations

In accordance with the observability analysis carried out in the previous section, we characterize our system by the following state:

S = [R T , V T , q] T (10)
where:

-R is the position of the second vehicle in the local frame of the first vehicle; -V is the velocity of the second vehicle in the frame of the first vehicle (note that this velocity is not simply the time derivative of R because of the rotations accomplished by the first local frame); q is the unit quaternion that describes the relative rotation between the two local frames.

In other words, the imaginary quaternions associated to R and V are: R q = (q 1 ) * (r 2 q -r 1 q )q 1 (11)

V q = (q 1 ) * (v 2 q -v 1 q )q 1 (12)
and q = (q 1 ) * q 2 (13)

The fundamental equations of the cooperative visualinertial sensor fusion problem are obtained by differentiating the previous three quantities with respect to time and by using (1) in order to express the dynamics in terms of the components of the state in [START_REF] Howard | Localization for Mobile Robot Teams Using Maximum Likelihood Estimation[END_REF] and the components of A 1 , A 2 , Ω 1 , Ω 2 . After some analytic computation, we obtain:

       Ṙq = 1 2 (Ω 1 q ) * R q + 1 2 R q Ω 1 q + V q Vq = 1 2 (Ω 1 q ) * V q + 1 2 V q Ω 1 q + qA 2 q q * -A 1 q q = 1 2 (Ω 1 q ) * q + 1 2 qΩ 2 q ( 14
)
As desired, the dynamics of the state is expressed only in terms of the components of the state and the system inputs (the angular speeds and the accelerations of both the vehicles). Finally, the camera observations can be immediately expressed in terms of the state in [START_REF] Howard | Localization for Mobile Robot Teams Using Maximum Likelihood Estimation[END_REF].

The first camera provides the vector R up to a scale.

Regarding the second camera, we first need the position of the first vehicle in the second local frame. The components of this position are the components of the following imaginary quaternion: -q * R q q. The second camera provides this position up to a scale.

In the last part of this section we provide the same equations, without using quaternions. We characterize our system by the two 3D vectors R and V , as before. Instead of the quaternion q, we use the matrix O that characterizes the rotation between the two local frames. From [START_REF] Indelman | Graph-based distributed cooperative navigation for a general multi-robot measurement model[END_REF] it is immediate to obtain the dynamics of this state. They are:

    Ṙ = Ω 1 × R + V V = Ω 1 × V + OA 2 -A 1 Ȯ = Ω 1 T × O + O Ω 2 × ( 15
)
where Ω rob × , rob = 1, 2, are the skew-symmetric matrices associated to the vectors Ω rob :

Ω rob × =   0 Ω rob z -Ω rob y -Ω rob z 0 -Ω rob x Ω rob y -Ω rob x 0   (16)
Finally, the two cameras provide the two vectors, R and -O T R, up to a scale.

The cooperative visual-inertial sensor fusion problem is fully characterized by the dynamics equations given in [START_REF] Isidori | Nonlinear Control Systems[END_REF] and the two observations given by R and -O T R, up to a scale. These equations allow us to build any estimation strategy: filter-based, optimization-based or a closed-form solution, i.e. a solution that extends the solution given in [START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] to the cooperative case.

Observable state in presence of bias

The goal of this section is to obtain the observable state when the inertial measurements are corrupted by the biases. Specifically, we introduce the following four vectors:

B 1 A , B 2 A , B 1 Ω and B 2 Ω . B 1 A and B 2
A are the biases on the accelerometers of the first and the second vehicle and B 1 Ω and B 2 Ω are the biases on the gyroscopes. Since the presence of the bias cannot improve the observability properties, we characterize our system by including in the observable state that holds in absence of bias (i.e., the state mentioned in the previous subsection), all the 12 components of the 4 bias vectors. If we prove that this state is observable, we can conclude that it is the entire observable state, i.e., any other physical quantity independent from its components is unobservable. Additionally, we will consider the case when only the first agent is equipped with a camera. Again, by proving that in these conditions the previous state is observable, we can conclude that the same observable state characterizes the case of two cameras.

Both the biases on the gyroscopes and on the accelerometers are time dependent. However, they change very slowly with time. In particular, they are modelled as random-walk processes driven by the zero-mean, white Gaussian noise

n 1 B Ω , n 2 B Ω , n 1 B A , n 2
B A , respectively. To characterize our system we define the extended state S E by including the bias in the state [START_REF] Howard | Localization for Mobile Robot Teams Using Maximum Likelihood Estimation[END_REF]:

S E = [R T , V T , q, (B 1 Ω ) T , (B 1 A ) T , (B 2 Ω ) T , (B 2 A ) T ] T . (17)
The dimension of this state is equal to 22. Actually, the components of this state are not independent, since q is a unit quaternion. In other words, we have:

(q t ) 2 + (q x ) 2 + (q y ) 2 + (q z ) 2 = 1 . ( 18
)
The dynamics of the state defined in [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF] are given by the following equations:

        Ṙ = Ω 1 × R + V V = Ω 1 × V + OA 2 -A 1 q = - 1 2 Ω 1 q q + 1 2 qΩ 2 q Ḃ1 Ω = n 1 B Ω , Ḃ1 A = n 1 B A , Ḃ2 Ω = n 2 B Ω , Ḃ2 A = n 2 B A (19)
where:

-

Ω 1 = Ω 1 + B 1 Ω , A 1 = A 1 + B 1 A , Ω 2 = Ω 2 + B 2 Ω , A 2 = A 2 + B 2
A . -The matrix O, can be uniquely expressed in terms of the components of the quaternion q.

-Ω 1 q is the imaginary quaternion associated with Ω 1 , i.e.,:

Ω 1 q = 0 + Ω 1 x i + Ω 1 y j + Ω 1 z k.
The same holds for Ω 2 q Note that, in the interval of few seconds, the time derivatives of the biases (last equation in [START_REF] Kato | Identifying and Localizing Robots in a Multi-Robot System Environment[END_REF]) can be set to zero. Since we will consider time intervals no longer than 4 seconds (and, as it will be shown, this will allow us to auto calibrate the inertial sensors with very high accuracy), we can assume that the biases are constant during the considered time interval (the same assumption is made in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF]).

The observation functions are the two scalar functions h u h v :

h [h u , h v ] T = R x R z , R y R z T . (20)
Additionally, we need to add the observation function that expresses the constraint that q is a unit quaternion. The additional observation is:

h const (X) (q t ) 2 + (q x ) 2 + (q y ) 2 + (q z ) 2 (21)
The analytic derivation of this system observability is provided in appendix A. We summarize its result:

The system defined above is observable (i.e., the state in ( 17) is observable). This holds both in the case when both the MAVs are equipped with a camera and in the case when only one MAV is equipped with a camera. Additionally, the observable state remains the same even in the case when the camera is a linear camera, i.e., it only provides the azimuth of the other MAV in its local frame. Finally, as in the case without bias, a necessary condition for the observability of the absolute scale is that the relative acceleration between the two MAVs does not vanish.

Closed-form solution

In this section we provide a closed-form solution that allows us to determine R, V and O by only using the measurements provided by the visual and the inertial sensors during a short time interval. In this section, we only provide the solution. The analytic derivation is provided in appendix B and more details about this derivation are available in [START_REF] Martinelli | Closed-form Solution to Cooperative Visual-Inertial Structure from Motion[END_REF]. Additionally, for brevity sake, we only deal with the case when only the first MAV is equipped with a camera. The case when both the MAVs are equipped with a camera is very similar (both for the analytic derivation and the solution) and can be found in [START_REF] Martinelli | Closed-form Solution to Cooperative Visual-Inertial Structure from Motion[END_REF]. Note that this solution is obtained by assuming noiseless and unbiased measurements. Hence, it is exact only in the noiseless and unbiased case. On the other hand, the impact of the bias on its performance will be evaluated in the next section. As we will see, it is precisely the strong sensitivity on the bias that will allow us to determine the bias itself (as in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF]).

Let us consider a given time interval (t A , t B ). Let us denote by R A , V A and O A , the values of R, V and O at time t A . Our goal is to obtain R A , V A and O A in closed-form, only in terms of the measurements provided during the considered time interval. Note that, the length of the considered time interval (i.e., t B -t A ) is very small (4 seconds). We assume that, during our time interval, the camera performs n observations at the times t j , (j = 1,

• • • , n), with t 1 = t A and t n = t B .
Let us denote by M 1 (t) and M 2 (t) the orthonormal matrices that characterize the rotations made by the first and the second MAV, respectively, between t A and t ∈ (t A , t B ). M 1 (t) and M 2 (t) can be computed by integrating the following first order differential equations:

Ṁ1 = Ω 1 T × M 1 Ṁ2 = Ω 2 T × M 2 (22)
with initial conditions: M 1 (t A ) = M 2 (t A ) = I 3 , (I 3 is the 3 × 3 identity matrix) and Ω 1 × and Ω 2 × are the matrices defined in [START_REF] Indelman | Information fusion in navigation systems via factor graph based incremental smoothing[END_REF]. Note that, since t B -t A does not exceed 4 seconds, these two matrices can be obtained with very high accuracy by using the measurements from the gyroscopes delivered in the considered time interval. In particular, the drift due to the noise in the gyroscope measurements is negligible. Regarding the bias, in Section 8 we will show that it can be removed. Let us introduce the following two vector quantities:

β 1 (t) = t t A τ t A M 1 (τ )A 1 (τ )dτ dτ (23)
β 2 (t) = t t A τ t A M 2 (τ )A 2 (τ )dτ dτ
Note that these vectors are computed by only using the IMU measurements delivered in the interval (t A , t). In particular, the matrices M 1,2 (τ ) are obtained by integrating the differential equations in [START_REF] Leung | Decentralized localization of sparsely-communicating robot networks: A centralized equivalent approach[END_REF] in the interval (t A , τ ), and this only requires the gyroscope measurements in this interval. Now we are ready to provide the extension of the closed-form solution in [START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] to the cooperative case. We obtain the components of R A , V A and O A by simply solving the linear system:

Ξx = b (24)
where:

-Ξ is a matrix with dimension 3n × (15 + n) given in [START_REF] Luft | Recursive decentralized collaborative localization for sparsely communicating robots[END_REF] (top of next page), with:

-0 33 the 3×3 zero matrix, 0 3 the zero 3×1 vector.

µ 1 , • • • , µ n the unit vectors provided by the camera (i.e., the directions of the second MAV in the frame of the first MAV at times t 1 , • • • , t n ) rotated by pre-multiplying them by the matrix

M 1 (t j ) T . -∆ j ≡ t j -t 1 = t j -t A (j = 2, • • • , n). -β 2 xj , β 2 yj and β 2 zj (j = 2, • • • , n
) the three components of the vector β 2 (t j ). x is the vector that contains all the unknowns, i.e.:

x

≡ [R T A , V T A , O A11 , O A21 , O A31 , O A12 , O A22 , O A32 , O A13 , O A23 , O A33 , λ 1 , • • • , λ n ] T (25)
where λ 1 , • • • , λ n , are the distances between the two MAVs at the times

t 1 , • • • , t n . -b is a vector with dimension 3n: b ≡ [β 1 T 1 , β 1 T 2 , • • • , β 1 T j , • • • , β 1 T n ] T (26)
where β 1 j = β 1 (t j ), with j = 1, • • • , n. In the case when both the MAVs are equipped with a camera and the observations are synchronized (i.e., both the cameras return the direction of the other MAV at the same times t 1 , • • • , t n ), the solution is given always by solving the linear system in [START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF]. The new expressions of Ξ, x and b are available in [START_REF] Martinelli | Closed-form Solution to Cooperative Visual-Inertial Structure from Motion[END_REF].

We conclude this section by remarking that all the components of the matrix Ξ and the vector b depend only on the measurements from the IMUs and the camera delivered during the time interval (t A , t B ). As a result, the solution is able to obtain the entire observable state without any prior knowledge (e.g., initialization). In particular, it provides the state as a simple expression of the measurements delivered during the time interval (t A , t B ).

It is also worth remarking that the communication needed between the two MAVs is very limited. Specifically, if MAV 2 performs the implementation, MAV 1 must provide the quantities β 1 in (23) and the unit vectors µ previously defined. The crucial advantage of this solution is that such data exchange is not required at the high frequency of the inertial sensors: only the β 1 and µ at the times of the camera images used are necessary (over a period of 3-4 seconds less than ten images are sufficient).

Finally, note that we include in x all the entries of the matrix O A . This means that, by obtaining the state through the inversion of the linear system in [START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF], we are considering independent the entries of O A . The fact that the matrix O A is orthonormal, means that we are ignoring 6 quadratic equations, i.e., the equations that express the fact that each column of the matrix is a unit vector (3 equations) and that the three columns are orthogonal one each other (3 equations). We are currently working on this important issue. We need to define a new state that only includes independent components. Then, by using the results obtained in this section (and in appendix B) it is possible to obtain a new equations system, which will be different from the one in [START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF]. In particular, we already found that, by a suitable choice of the new state and some analytic compuation, instead of a linear system, the new equations system will include three polynomial equations of second degree and several linear equations.

Limitations of the Closed-Form Solution

The goal of this section is to find out the limitations of the solution provided in section 6 when it is adopted in a real scenario. In particular, special attention will be devoted to the case of a MAV equipped with low-cost camera and IMU sensors. For this reason, this section evaluates the impact of the following sources of error on the performance:

1. Varying noise on the camera and inertial measurements (section 7.2); 2. Erroneous camera extrinsic calibration, i.e., imperfect knowledge of the transformation between the camera and the IMU frame (section 7.3); 3. Erroneous synchronization between the two cameras (section 7.4); 4. Bias on the accelerometers (section 7.5); 5. Bias on the gyroscopes (section 7.6).

Simulation setup

We simulate two MAVs that execute random trajectories. Specifically, the trajectories are simulated as follows. Each trial lasts 4 s. The first MAV starts at the origin and the second MAV starts at a random position, normally distributed, centered at the origin, and with covariance matrix 1 m 2 I 3 . The initial velocities are randomly generated. Specifically, their values are normally distributed, with zero mean, and covariance matrix 1 (m/s) 2 I 3 . Finally, the initial orientations are characterized by the roll, pitch and yaw angles. These

Ξ =             I 3 0 33 0 33 0 33 0 33 -µ 1 0 3 • • • • • • • • • • • • 0 3 I 3 ∆ 2 I 3 β 2 x2 I 3 β 2 y2 I 3 β 2 z2 I 3 0 3 -µ 2 0 3 • • • • • • • • • 0 3 I 3 ∆ 3 I 3 β 2 x3 I 3 β 2 y3 I 3 β 2 z3 I 3 0 3 0 3 -µ 3 0 3 • • • • • • 0 3 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • I 3 ∆ j I 3 β 2 xj I 3 β 2 yj I 3 β 2 zj I 3 0 3 • • • 0 3 -µ j 0 3 • • • 0 3 • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • • I 3 ∆ n-1 I 3 β 2 x n-1 I 3 β 2 y n-1 I 3 β 2 z n-1 I 3 0 3 • • • • • • • • • 0 3 -µ n-1 0 3 I 3 ∆nI 3 β 2 xn I 3 β 2 yn I 3 β 2 zn I 3 0 3 • • • • • • • • • • • • 0 3 -µn             (27)
are also randomly generated, with zero mean and covariance matrix (50 deg) 2 I 3 .

The angular speeds, i.e. Ω 1 and Ω 2 , are Gaussian. Specifically, their values at each step of 0.1s follow a zero-mean Gaussian distribution with covariance matrix equal to (30deg) 2 I 3 , where I 3 is the identity 3 × 3 matrix. At each time step, the two MAV inertial accelerations are generated as random vectors with zero-mean Gaussian distribution. In particular, the covariance matrix of this distribution is set equal to (1ms -2 ) 2 I 3 .

The MAVs are equipped with inertial sensors able to measure at the frequency of 0.5kHz the acceleration (the sum of the gravity and the inertial acceleration) and the angular speed. These measurements are affected by errors. Specifically, each measurement is generated by adding to the true value a random error that follows a Gaussian distribution. The mean value of this error is zero. The standard deviation will be denoted by σ Accel for the accelerometer and σ Gyro for the gyroscope (these values will be specified for each result). Regarding the camera measurements, they are generated at a lower frequency. Specifically, the measurements are generated at 5Hz. Also these measurements are affected by errors. Specifically, each measurement is generated by adding to the true value a random error that follows a zero-mean Gaussian distribution, with standard deviation σ Cam .

To evaluate the performance, we define the metrics as follows. The error on the absolute scale is defined as the relative error averaged over all the estimated distances between the MAVs at the times of the camera measurements (t 1 , • • • , t n ), i.e.:

Err scale 1 n n i=1 |λ est i -λ true i | λ true i
For the speed, the error is defined as

Err V V est A -V true A V true A
Finally, for the relative orientation, the error is computed by averaging on the roll pitch and yaw, that define the relative rotation between the two local frames. In the next subsections, we will present the results obtained with the closed-form solution provided in Section 6 on the simulated data. In section 8, we introduce a simple method to autocalibrate the bias.

Performance with varying sensor noise

In Fig. 2, we show the performance of the Closed-Form solution in estimating absolute scale, relative speed and relative orientation. The performance is given as a function of the duration of the time interval (t B -t A ). In this case, the sensor noise is set as follows: σ Accel = 0.03 ms -2 , σ Gyro = 0.1 deg s -1 and σ Cam = 1 deg. All the values are averaged over 1000 trials. From the results, it is clear how the evaluations improve as we increase the duration of the integration time. Fig. 3 displays the relative error for the same quantities showed in Fig. 2 but for a variable noise on the inertial measurements. Specifically, σ Accel = (s • 0.03) ms -2 , σ Gyro = (s • 0.1) deg s -1 and σ Cam = (s • 0.5) deg. In this case, the two agents observe one each other over 3 seconds. The general behavior remains the same. Note that the noise is very large (standard sensors are charac- terized by s 1). The performance remains very good also for very large noise.

Performance with imperfect camera extrinsic calibration

Figures 4 and5 display the relative error for the same quantities showed in Fig. 3 but for a variable error in the camera extrinsic calibration. Specifically, Fig. 4 displays the results when the camera and the IMU frames are not perfectly aligned (this holds for both MAVs). The x-y planes of the two frames make a variable angle and the performance is provided when this angle is in the range (0, 15) deg. We remark that this source of error has the same effect on the scale, on the speed and on the relative orientation. A misalignment of 6 deg produces a 10% relative error. Fig. 5 displays the results when the origin of the camera frame does not coincide with the origin of the IMU frame. In particular, the origin of the former has coordinates ρ[1, 1, 1]/ √ 3 and the performance is provided when ρ is in the range (0, 0.1) m. We remark that this source of error does not impact the performance on the relative speed and the relative orientation.

7.4 Performance with imperfect synchronization between the two cameras Fig. 6 displays the relative error for the same quantities showed in Fig. 3 but for a variable synchronization error between the two cameras. Specifically, the measurements of the second camera are generated with a delay of ∆t seconds. The performance remains good (relative error smaller than 8%) for ∆t ≤ 0.02 s.

Fig. 4 As in Fig. 2 but for a variable angle between the camera and the IMU frame (of both MAVs). The two agents observe one each other over 3 seconds. 6 As in Fig. 2 but for a variable synchronization error (∆t, in seconds). The two agents observe one each other over 3 seconds.

Impact of accelerometer bias on the performance

In order to visualize the impact of the accelerometer bias on the performance, we corrupt the accelerometer measurements by a bias (Fig. 7). Despite a high accelerometer bias, the closed-form solution still provides good results. Note that, even in the case of a bias with magnitude 0.1ms -1 (black dashed line in Fig. 7), the error attains its minimum after 1.5s and it is less than 3% for the scale and less than 10% for the relative speed (note that, the larger error on the speed is due to its smaller absolute value).

Impact of gyroscope bias on the performance

To visualize the impact of the gyroscope bias on the performance, we corrupt the gyroscope measurements by an artificial bias (Fig. 8). As seen in Fig. 8, the performance becomes very poor in presence of a bias on the gyroscope and, in practice, the overall method could only be successfully used with a very preciseand expensive -gyroscope.

8 Estimating the Gyroscope Bias (B 1 Ω and B 2 Ω )

In this section, we propose an optimization approach to estimate the gyroscope bias using the closed-form solution.

Let us consider a given experiment, i.e., a set of inertial and camera measurements obtained during a given time interval (t A , t B ). As shown in section 6 (and in the appendix B), these measurements provide all the ingredients to compute the matrix Ξ and the vector b. By solving the linear system in (24) we compute the vector x. Finally, we compute the residual Ξx -b 2 . We can repeat this procedure by changing the measurements provided by the two gyroscopes and by leaving all the other measurements unaltered. In particular, we subtract from the gyroscope measurements a fixed quantity (i.e., which is constant on the considered time interval). In other words, for each t ∈ (t A , t B ), we replace Ω 1 (t) and Ω 2 (t) with Ω1 (t)

Ω 1 (t) -B 1 Ω and Ω2 (t) Ω 2 (t)-B 2
Ω , respectively. Then we compute the new matrix Ξ and the new vector b. We solve the new linear system in [START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF] and we compute the new vector x. Finally, we compute the residual Ξx -b 2 .

In accordance with the above procedure, for a given experiment, we can regard Ξx -b 2 as a function of the two vectors: B 1 Ω and B 2 Ω . We introduce the following function:

Cost(B) = Ξx -b 2 (28)
with:

-B is a vector with six components, which are the components of the bias of the first and the second gyroscope, i.e.,:

B = [B 1 Ω , B 2 Ω ].
-Ξ and b are computed by removing from the measurements provided by the two gyroscopes, the corresponding components of B.

By minimizing this cost function, we recover the gyroscope bias B and the vector x. Note that the minimization is carried out over the six components of B, i.e., the bias of the two gyroscopes. Since this minimization requires an initialization and the cost function is non-convex, the optimization process can be stuck in local minima. However, by running extensive simulations we found that the cost function is convex around the true value of the bias. In addition, even if it is true that the bias can significantly increase with time, it increases quite slowly. By continuously estimating its value, and by initializing the minimization of the cost function with the last estimate of the bias, we always remain in the region where the cost function is convex.

Performance Overall Evaluation

This section analyzes the performance of the closedform solution completed with the bias estimator introduced in section 8. The setup is the one described in section 7.1. Also in this case, the results are averaged on 1000 trials. We consider the same five values of the bias of the gyroscopes considered in Fig. 8. Finally, we set the magnitude of the accelerometer bias equal to zero (Fig. 9) and equal to 0.1ms -2 (Fig. 10). Fig. 9 shows a performance comparable to the one exhibited in Fig. 2. Fig. 10 shows a performance even better than the one exhibited in Fig. 7. This demonstrates that the effect of the bias has been fully compensated.

Conclusion

In this paper, we studied the problem of cooperative visual inertial sensor fusion. Specifically, the case of two agents was investigated. Each agent was equipped with inertial sensors (accelerometer and gyroscope) and with a monocular camera. By using the monocular camera, each agent can observe the other agent. Specifically, the camera only returns the position (up to a scale) of the observed agent in its local frame. No additional camera observations (e.g., of external point features in the environment or of known pattern on the observed agent able to directly provide the relative orientation between

Legend Absolute Scale

Relative Speed Relative Orientation the agents, as in [START_REF] Achtelik | Collaborative stereo[END_REF]) were considered. All the inertial sensors were assumed to be affected by a bias. First, the entire observable state was analytically derived. To this regard, we proved that the entire observable state consists of the following independent physical quantities:

-The position of one of the agents in the local frame of the other agent (this means that the absolute scale is observable). -The relative speed between the two agents expressed in the local frame of one of them. -The three Euler angles that characterize the rotation between the two local frames attached to the two agents. -All the bias that affect the inertial measurements (both the accelerometers and the gyroscopes).

It is interesting to remark that this result holds even in the case when only one of the two agents is equipped with a camera and, very surprisingly, even when this camera is a linear camera, i.e., it only provides the azimuth of the other agent in its local frame. Then, the paper provided a closed-form solution, able to determine the observable state by only using vi-sual and inertial measurements delivered in a short time interval (4 seconds). This solution extended the solution in [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF][START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] to the cooperative case. It is remarkable that it is possible to retrieve the absolute scale even when no point features are available in the environment.

Following the analysis conducted in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF], the paper focused on investigating all the limitations that characterize this solution when used in a real scenario. Specifically, the impact of the presence of the bias on the performance of this closed-form solution was investigated. As in the case of a single agent, this performance is significantly sensitive to the presence of a bias on the gyroscope, while the presence of a bias on the accelerometer is less important. A simple and effective method to obtain the gyroscope bias was proposed. Extensive simulations clearly showed that the proposed method is successful. It is fascinating that it is possible to automatically retrieve the absolute scale and simultaneously calibrate the gyroscopes not only without any prior knowledge (as in [START_REF] Kaiser | Simultaneous State Initialization and Gyroscope Bias Calibration in Visual Inertial Aided Navigation[END_REF] for a single agent), but also without external point features in the environment.

Future works will be focused on additional theoretical investigation. This will include the study of the case of more than two agents. This study requires to address several important issues. In the case of two agents, to obtain the linear system, we had to consider a state whose components are not independent (e.g., all the nine entries of the rotation matrix are included in the state). In the case of more than two agents, obtaining a linear system by minimizing the redundancy in the state, is a first issue to be investigated. Note that, as mentioned at the end of section 6, our objective to be achieved firstly in the case of two agents, is to consider a state whose components are independent and obtaining the equation system that characterizes the problem. This equation system will be a Polynomial Equation System (PES), instead of a linear system. So far, we already found a partial solution to this problem. The PES consists of three polynomial equations in three unknowns and several linear equations (this provides up to eight solutions in the minimal case). The analysis of this PES, that fully characterizes the problem, provides all the theoretical features of the problem. This analysis is currently under our investigation and will be the extension of the analysis provided in [START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] for the case of a single agent (in that case the PES consists of a single polynomial equation of second degree and several linear equations). In the case of more than two agents, we expect that the PES becomes much more complex and this issue certainly deserves to be investigated. From a practical point view, there are many issues to be considered in the case of more than two agents. The visual constraint due to the limited camera field of view becomes more important. In particular, the new issue to be investigated is how the performance changes by varying the number of agents that can be seen from each agent. In addition, the cameras synchronization is harder to be realized in the case of many agents. Finally, the problem of communication delays and how robust is the solution vs communication troubles becomes certainly more relevant.

A Observability with bias

We analytically obtain the observability properties of the system defined by the state in [START_REF] Jones | Visual-inertial navigation, mapping and localization: A scalable real-time causal approach[END_REF], the dynamics in [START_REF] Kato | Identifying and Localizing Robots in a Multi-Robot System Environment[END_REF] (where the last equation has been replaced by Ḃ1 Ω = Ḃ1 A = Ḃ2 Ω = Ḃ2 A = 0) and the three observations in [START_REF] Kia | Cooperative localization for mobile agents: a recursive decentralized algorithm based on Kalman filter decoupling[END_REF] and [START_REF] Kim | Multiple relative pose graphs for robust cooperative mapping[END_REF]. Since both the dynamics and the observations are nonlinear with respect to the state, we use the observability rank condition in [START_REF] Hermann | Nonlinear Controllability and Observability[END_REF]. The dynamics are affine in the inputs, i.e., they have the expression given in [START_REF] Hesch | Consistency analysis and improvement of vision-aided inertial navigation[END_REF]. Specifically, we set:

u 1 , u 2 , u 3 the three components of A 1 ; u 4 , u 5 , u 6 the three components of Ω 1 ; u 7 , u 8 , u 9 the three components of A 2 ; u 10 , u 11 , u 12 the three components of Ω 2 .

Then, by comparing [START_REF] Kato | Identifying and Localizing Robots in a Multi-Robot System Environment[END_REF] with [START_REF] Hesch | Consistency analysis and improvement of vision-aided inertial navigation[END_REF] it is immediate to obtain the analytic expression of all the vector fields f

0 , f 1 , • • • , f 12 ;
for instance, we have:

f 4 = [0, -Rz, Ry, 0, -Vz, Vy, qx/2, -qt/2, qz/2, -qy/2, 0 12 ] T f 7 = [0, 0, 0, q 2 t + q 2
x -q 2 y -q 2 z , 2qtqz + 2qxqy, 2qxqz -2qtqy, 0, 0, 0, 0, 0 12 ] T For systems with the dynamics given in (9) the application of the observability rank condition can be automatically done by a recursive algorithm (Algorithm 1). In particular, this algorithm automatically returns the observable codistribution by computing the Lie derivatives of all the system outputs along all the vector fields that characterize the dynamics (see Chapter 1 of [START_REF] Isidori | Nonlinear Control Systems[END_REF]). For the specific case, we obtain that the algorithm converges at the fourth step, i.e., the observable codistribution is the span of the differentials of the previous Lie derivatives up to third order. In particular, its dimension is 22 meaning that all the state components are observable.

A choice of 22 Lie derivatives is:

L 0 hu, L 0 hconst, L 1 f0 hu, L 1 f4 hu, L 2 f0f0 hu, L 2 f0f1 hu, L 2 f0f7 hu, L 2 f0f8 hu, L 2 f0f4 hu, L 2 f0f5 , L 2 f0f6 hu, L 3 f0f0f0 hu, L 3 f0f0f1 hu, L 3 f0f0f2 hu, L 3 f0f0f7 hu, L 3 f0f0f4 hu, L 3 f0f0f5 hu, L 3 f0f0f10 hu, L 3 f0f7f0 hu, L 3 f0f8f0 hu, L 3 f0f0f0 hv, L 3 f0f0f5 hv .
Note that the choice of these 22 independent Lie derivatives is not unique. In particular, it is possible to avoid the Lie derivatives of the functions hv. Specifically, in the previous choice, only the last two Lie derivatives are Lie derivatives of the function hv. It is possible to avoid these two functions. On the other hand, in this case we need to include fourth order Lie derivatives of hu. For instance, we can replace the last two functions with L 4 f0f0f0f0 hu, L 4 f0f0f0f5 hu. This means that we obtain the same observability properties when the first agent is equipped with a linear camera able to only provide the azimuth of the second agent in its local frame. Finally, as in the unbiased case, a necessary condition to have 22 independent Lie derivatives is that at least one of them must be computed along a direction that corresponds to one of the axes of at least one of the two accelerometers (note that in the above selection we have Lie derivatives computed along f 1 , f 2 , f 7 and f 8 ). This means that a necessary condition for the observability of the absolute scale is that the relative acceleration between the two MAVs does not vanish.

Legend Absolute Scale

Relative Speed Relative Orientation Fig. 10 As in Fig. 9 but the magnitude of the accelerometer bias is set to 0.1ms -2 for both the MAVs.

B Analytic derivation of the closed-form solution

In this appendix we provide the analytic steps to obtain the linear system given in section 6 (equation ( 24)). For the brevity sake, we only consider the case of a single camera. Specifically, we assume that only the first vehicle is equipped with a camera. The derivation in the case of two synchronized cameras follows the same schema and is available in [START_REF] Martinelli | Closed-form Solution to Cooperative Visual-Inertial Structure from Motion[END_REF]. We start our derivation by introducing a new local frame for each vehicle. Each new frame is defined as follows. It shares the same origin with the original local frame. Additionally, it does not rotate and its orientation coincides with the one of the original frame at the time t A . From now on, we will refer to this frame as to the new frame. Additionally, we will refer to the original local frame, namely the one defined at the beginning of section 2, as to the original frame. Let us introduce the following notation:

-V 1 and V 2 denote the first and the second vehicle; ξ is the position of V 2 in the new local frame of V 1 ; η is the relative velocity of V 2 with respect to V 1 , expressed in the new local frame of V 1 ;

By construction we have:

ξ A ≡ ξ(t A ) = R A η A ≡ η(t A ) = V A (29)
From [START_REF] Isidori | Nonlinear Control Systems[END_REF] we have the following dynamics in the new coordinates:

   ξ = η η = O A A 2 -A 1 ȮA = 0 (30)
where:

-A 1 is the acceleration (gravitational and inertial) of V 1 expressed in the first new local frame (i.e., A 1 = M 1 A 1 ); -similarly, A 2 = M 2 A 2 .

Let us introduce the following notation:

w 1 , w 2 and w 3 are the three columns of the matrix O A , i.e., O A = w 1 w 2 w 3 ; α 1 (t) = [α 1

x (t), α

η(t) = η A + w 1 α 2
x (t) + w 2 α 2 y (t) + w 3 α 2 z (t) -α 1 (t) [START_REF] Martinelli | Vision and imu data fusion: Closed-form solutions for attitude, speed, absolute scale, and bias determination[END_REF] and by substituting in the first equation in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] and integrating again, we obtain: ξ(t) = ξ A +η A (t-t A )+w 1 β 2 x (t)+w 2 β 2 y (t)+w 3 β 2 z (t)-β 1 (t) [START_REF] Martinelli | Closed-form solution of visual-inertial structure from motion[END_REF] Note that this equation provides ξ(t) as a linear expression of 15 unknowns, which are the components of the 5 vectors: ξ A , η A , w 1 , w 2 and w 3 . In the following, we build a linear system in these unknowns together with the unknown distances when the camera performs the measurements. The camera (on V 1 ) provides the vector R(t) = M 1 (t)ξ(t), up to a scale. We denote by λ(t) this scale (this is the distance between V 1 and V 2 at the time t). We have ξ(t) = λ(t)µ(t), where µ(t) is the unit vector with the same direction of ξ(t). Note that our sensors (specifically, the camera together with the gyroscope on V 1 ) provide precisely the unit vector µ(t): the camera provides the unit vector along R(t); then, to obtain µ(t) it suffices to pre multiply this unit vector by [M 1 (t)] T .

We remind the reader that the camera performs n observations at the times t j , (j = 1, • • • , n), with t 1 = t A and tn = t B . For notation brevity, for a given time dependent quantity (e.g., λ(t)), we will denote its value a the time t j by the subscript j (e.g., λ j = λ(t j )). In this notation, equation (32) becomes:

λ j µ j = ξ A + η A (t j -t A ) + w 1 β 2 xj + w 2 β 2 yj + w 3 β 2 zj -β 1 j (33)
This is a linear equation in 15 + n unknowns. The unknowns are:

-The distances λ 1 , • • • , λn.

-The three components of ξ A .

-The three components of η A .

-The components of the vectors w 1 , w 2 and w 3 , i.e., the nine entries of the matrix O A .

Note that equation ( 33) is a vector equations, providing 3 scalar equations. Since this holds for each j = 1, • • • , n, we obtain a linear system of 3n equations in 15 + n unknowns. This is precisely the linear system given in [START_REF] Li | High-precision, consistent EKF based visual-inertial odometry[END_REF] with the vector x given in [START_REF] Li | Cooperative multi-vehicle localization using split covariance intersection filter[END_REF], the matrix A given in [START_REF] Luft | Recursive decentralized collaborative localization for sparsely communicating robots[END_REF] and the vector b given in [START_REF] Liu | Spline-Based Initialization of Monocular Visual-Inertial State Estimators at High Altitude[END_REF].

Fig. 1

 1 Fig.1The global frame and the two local frames (attached to the first and the second aerial vehicle, respectively). r 1 and r 2 are their position, expressed in the global frame. R is the relative position of the second vehicle with respect to the first vehicle, expressed in the local frame of the first vehicle.

Fig. 2

 2 Fig.2Relative error of the closed-form solution in determining the absolute scale (solid blue), the relative speed (dotted red) and the relative orientation (dashed black). The two agents observe one each other over a variable duration of integration. σ Accel = 0.03 ms -2 and σ Gyro = 0.1 deg s -1 . All the values are averaged on 1000 trials.

Fig. 3

 3 Fig. 3 As in Fig. 2 but for a variable noise on the inertial measurements (σ Accel = (s • 0.03) ms -2 , σ Gyro = (s • 0.1) deg s -1 and σ Cam = (s • .5) deg). The two agents observe one each other over 3 seconds.

Fig. 5

 5 Fig.5As in Fig.2but for a variable position of the origin of the camera frame in the IMU frame (of both MAVs). The two agents observe one each other over 3 seconds.

Fig.

  Fig.6As in Fig.2but for a variable synchronization error (∆t, in seconds). The two agents observe one each other over 3 seconds.

Fig. 7

 7 Fig. 7 Impact of the accelerometer bias on the performance of the closed-form solution. The two MAVs observe one each other over a variable duration of integration.

Fig. 8

 8 Fig. 8 Impact of the gyroscope bias on the performance of the closed-form solution. The two MAVs observe one each other over a variable duration of integration.

Fig. 9

 9 Fig. 9 Impact of the gyroscope bias on the performance of the closed-form solution completed with the bias estimator. The accelerometers are unbiased. The two MAVs observe one each other over a variable duration of integration.

  Note that the quantities β 1 (t) and β 2 (t) defined in section 6 areβ 1 (t) = t t A α 1 (τ )dτ and β 2 (t) = t t A α 2 (τ )dτ .Let us 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] between t A and a given t ∈ [t A , t B ]. We obtain:

	1 y (t), α 1 z (t)] T = t t A x (t), α 2 -α 2 (t) = [α 2 y (t), α 2 z (t)] T = t t A	A 1 (τ )dτ ; A 2 (τ )dτ .

A quaternion q = qt +qxi+qyj +qzk is a unit quaternion if the product with its conjugate is 1, i.e.: qq * = q * q = (qt +qxi+ qyj +qzk)(qt -qxi-qyj -qzk) = (qt)

+(qx) 2 +(qy) 2 +(qz) 2 = 1

The reader unfamiliar with the concept of codistribution, as it is used in[START_REF] Isidori | Nonlinear Control Systems[END_REF], should not be afraid by the term distribution and the term codistribution. Very simply speaking (and this is enough to understand the theory of nonlinear observability) they are both vector spaces. Specifically, a distribution is the span of a set of column-vector functions. A codistribution is the span of a set of line-vector functions. Hence, both a distribution and a codistribution can be regarded as vector spaces that change by moving on the space of the states (X), namely, vector spaces that depend on X.

Acknowledgment

This work was supported by the French National Research Agency ANR 2014 through the project VIMAD.

Agostino Martinelli received the M.Sc. degree in theoretical physics from the University of Rome Tor Vergata, Rome, Italy, in 1994 and the Ph.D. degree in astrophysics from the University of Rome La Sapienza, in 1999. While working toward the Ph.D. degree, he spent one year at the University of Wales, Cardiff, U.K., and one year with the Scuola Internazionale Superiore di Studi Avanzati, Trieste, Italy. His research focused on the chemical and dynamical evolution in elliptical galaxies, in quasars, and in the intergalactic medium. After receiving the Ph.D. degree, his interests moved to the problem of autonomous navigation. He was with the University of Rome Tor Vergata for two years, and, in 2002, he moved to the Autonomous Systems Laboratory, Ecole Polytechnique Federale de Lausanne, Switzerland, as a Senior Researcher. Since September 2006, he has been a Researcher with the Institut National de Recherche en Informatique et en Automatique (INRIA), Rhone Alpes, Grenoble, France. His current research focuses on three main topics: (i) Visual-Inertial Structure from Motion, (ii) Non linear observability, (iii) Overdamped Brownian motion and the Fokker-Plank equation without detailed balance.