Agostino Martinelli

Roland Siegwart

Vision and IMU Data Fusion: Closed-Form Determination of the Absolute Scale, Speed and Attitude

This chapter describes an algorithm for determining the speed and the attitude of a sensor assembling constituted by a monocular camera and inertial sensors (three orthogonal accelerometers and three orthogonal gyroscopes). The system moves in a 3D unknown environment. The algorithm inputs are the visual and inertial measurements during a very short time interval. The outputs are: the speed and attitude, the absolute scale and the bias affecting the inertial measurements. The determination of these outputs is obtained by a simple closed form solution which analytically expresses the previous physical quantities in terms of the sensor measurements. This closed form determination allows performing the overall estimation in a very short time interval and without the need of any initialization or prior knowledge. This is a key advantage since allows eliminating the drift on the absolute scale and on the orientation. The performance of the proposed algorithm is evaluated with real experiments.

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. In addition, these sensors have very interesting complementarities and together provide rich information to build a system capable of vision-aided inertial navigation and mapping and a great effort has been done very recently in this direction (e.g. [START_REF] Ahrens | Vision-based guidance and control of a hovering vehicle in unknown, gps-denied environments[END_REF], [START_REF] Blosch | Vision Based MAV Navigation in Unknown and Unstructured Environments[END_REF]). A special issue of the International Journal of Robotics Research has recently been devoted to the integration of vision and inertial sensors [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. 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 with an Extended Kalman Filter (EKF ) and with 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 remarked 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. Regarding the robot attitude, in [START_REF] Bryson | Building a Robust Implementation of Bearing-only Inertial SLAM for a UAV[END_REF] it has been noted that roll and pitch angles remain more consistent than the heading.

A fundamental issue to address when fusing vision and inertial measurements, is to understand which are the observable modes, i.e. the physical quantities that the information contained in the sensor data allows uniquely determining. The next issue to address is to find a reliable and efficient method to determine all the previous physical quantities.

The following simple 1 -D example clearly shows that it is reasonable to expect that the absolute scale is an observable mode and can be obtained by a closed-form solution. 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 robot 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. When the initial speed v A is unknown, all the previous segment lengths can be obtained in terms of v A . In other words, it is possible to 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 robot navigates from A to B. 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.

By repeating the same computation with the bearing measurements in

In [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF] these restrictions were relaxed. A vehicle equipped with IMU and bearing sensors was considered. The motion of the vehicle was not constrained. However, only the case of one single feature was considered. In addition, the inertial measurements were unbiased. This chapter extends the results obtained in [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF] by also considering the case of multiple features. Additionally, also the case when the accelerometers provide biased measurements will be considered.

The chapter is organized as follows. Section II provides a mathematical description of the system. Sections III and IV provide conditions for the state observability. Then, section V provides the analytical derivation of the closed-form solution to determine the speed and attitude.

Section VI highlights the main steps of the proposed algorithm. The performance of the algorithm is evaluated in section VII. Conclusions are provided in section VIII.

II. THE CONSIDERED SYSTEM

The system is a sensor assembling constituted by a monocular camera and IM U sensors.

The IMU consists of three orthogonal accelerometers and three orthogonal gyroscopes. The transformations among the camera frame and the IMU frames are known (the local frame is the camera frame). In the following, the word vehicle will be used 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, 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.

In the following, upper-case letters will indicate the vectors when expressed in the local frame and lower-case letters when they are expressed in the global frame. Hence, regarding the gravity:

a g = [0, 0, -g] T , being g 9.8 ms -2 .
The camera is observing a point feature during a given time interval. The global frame will be attached to this feature. The vehicle and the feature are displayed in fig 2. Finally, a quaternion will be adopted 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].

The 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 , the imaginary quaternion ŵ ≡ 0 + iw x + jw y + kw z will be associated with it. The dynamics of the state [r, v, q] T are:

           ṙ = v v = q Âv q * = q Âq * + âg q = 1 2 q Ω (1)
being q * the conjugate of q, q * = q t -iq x -jq y -kq z . The camera observations can be expressed in terms of the same state ([r, v, q] T ). The camera provides the direction of the feature in the local frame. In other words, it provides the unit vector F |F | (see fig. 2). Hence, the camera provides the two ratios y 1 = Fx Fz and y 2 = Fy Fz , being F = [F x , F y , F z ] T . 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 (2)
where the pedices x, y and z indicate respectively the i, j and k component of the corresponding quaternion. Finally, the constraint q * q = 1 can be dealt as a further observation (system output):

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

A. The Case with Multiple Features

In the case when the camera observes N f features, simultaneously, the global frame will be attached to one of the features. d i denotes the 3D vector which contains the cartesian coordinates of the i th feature (i = 0, 1, ..., N f -1). 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 (1) together with the equations:

ḋi = [0 0 0] T i = 1, ..., N f -1 (4)
The position F i of the i th feature in the local frame is obtained by the quaternion product

F i = 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 (5)
which coincides with the observation in (2) 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 ( 1) and ( 4)

and the observations are given in ( 5) and (3).

B. The Case with Bias

A bias denotes and Ω bias denote 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 (1) 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 (6)
Note that these equations only hold for short time intervals. In the following, these equations will be used only when this hypothesis is satisfied (in particular, during time intervals allowing the camera to perform at most ten consecutive observations).

III. OBSERVABILITY PROPERTIES

We investigate the observability properties of the system whose dynamics are given in (1) and whose observations are given in ( 2) and (3). For the sake of clarity, we discuss both the case without gravity (III-A) and with gravity (III-B).

A. The Case without Gravity

Let us set g = 0 in [START_REF] Ahrens | Vision-based guidance and control of a hovering vehicle in unknown, gps-denied environments[END_REF]. By directly computing the Lie derivatives and their gradients, it is possible to detect three independent symmetries for the resulting system (see [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF]). They are:

w 1 s = 0 -r z r y 0 -v z v y - q x 2 q t 2 - q z 2 q y 2 T ( 7
)
w 2 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 3 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 of continuous symmetry introduced in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF], 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 3 s
On the other hand, without computing the Lie derivatives, we could not conclude that the previous ones are all the symmetries for the considered system.

In [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] we proved that for every symmetry there is an associated partial differential equation and 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 ( 7) but can also be proved by remarking that the scale factor 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 ( 3); it can be in any case verified that it satisfies the three partial differential equations).

B. The Case with Gravity

We investigate the observability properties when g = 0. The presence of the gravity breaks two of the previous three symmetries. In other words, the system remains invariant only with respect to rotations about the vertical axis. This means that w 1 s and w 2 s are no longer symmetries for the new system. By directly computing the Lie derivatives, we were able to find nine independent Lie derivatives. Hence, the system has 10 -9 = 1 symmetry which is w s 3 .

The partial differential equation associated with w s 3 is:

-2r y ∂Λ ∂r x + 2r x ∂Λ ∂r y -2v y ∂Λ ∂v x + 2v x ∂Λ ∂v y + (8) -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 (9)
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] Goldstein | Classical Mechanics[END_REF].

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 ( 1) and ( 4) and the observations are given in ( 5) and (3).

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 the previous subsections can be summarized with the following property:

Property 1 Let us consider the system defined by ( 1), ( 3), ( 4) and ( 5). 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 bias affecting the accelerometers and the gyros are observable. The system we are considering is defined by the state: [r v q b A b Ω ] T , whose dimension is 16. This state satisfies the dynamics in [START_REF] Dias | 2nd Workshop on Integration of Vision and Inertial Sensors[END_REF]. Finally, this system is characterized by the observations given in ( 2) and (3).

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 III-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] Goldstein | Classical Mechanics[END_REF], where, now, Λ also depends on the components of b A and b Ω . 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 As previously mentioned, we know that we cannot have more than 15 independent Lie derivatives (otherwise, the yaw angle would be observable). 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 (8). They are: the 9 solutions provided in III-B and the six components of the two vectors b A and b Ω (note that these components are trivial solutions of ( 8)).

E. Unknown Gravity

The results provided in the previous sections are obtained by assuming that the magnitude of the gravity (g) is a priori known. In this section we want to investigate if the information contained in the sensor data allows us to also estimate g. This investigation could seem useless since in most of cases the value g is known with good accuracy. On the other hand, this investigation allows us to derive several properties of practical importance.

We will show that g is among the observable modes even in the worst case when the inertial sensors are affected by bias and when only a single feature is available. We will proceed as in section III-D.

The system we are considering is defined by the state: [r v q b A b Ω g] T , whose dimension is 17. This state satisfies the dynamics in [START_REF] Dias | 2nd Workshop on Integration of Vision and Inertial Sensors[END_REF] with the additional equation ġ = 0. Finally, this system is characterized by the observations given in (2) and (3).

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 III-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] Goldstein | Classical Mechanics[END_REF], where Λ also depends on the components of b A , b Ω and on g. 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 16 independent Lie derivatives. By a direct computation, performed by using the symbolic Matlab computational tool, we were able to find the following

IV. NECESSARY CONDITIONS FOR OBSERVABILITY

The observability analysis performed so far takes into account all the degrees of freedom allowed by the dynamics in [START_REF] Ahrens | Vision-based guidance and control of a hovering vehicle in unknown, gps-denied environments[END_REF]. In other words, the observability of the modes previously derived, could require the vehicle to move along all these degrees of freedom. It is interesting to understand what happens when only special trajectories are considered. Mathematically, this can be done by introducing in (1) the constraints characterizing the trajectory we want to consider.

Then, it suffices to apply the method described in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] to the system characterized by the new dynamics and the same observations ( 2) and (3).

By applying this technique we proved in [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] the two following properties:

Property 2
The absolute scale factor is not observable when the vehicle moves at constant speed.

Property 3 When the vehicle moves at constant acceleration all the modes derived in section III are observable, with the exception of the magnitude of the gravitational acceleration (g).

A fundamental consequence of the previous two properties is:

Theorem 1 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 is unknown, the minimum number of camera images becomes four.

Proof: The proof of this theorem is provided in [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF]. In particular, it is shown that, if the observability of a given physical quantity requires to have a not constant speed, this observability also requires at least three camera observations. Similarly, it is shown that, if the observability of a given physical quantity requires to have a not constant acceleration, this observability also requires at least four camera observations.

In most of cases, the value g is known with good accuracy. Hence, considering the case of unknown magnitude of gravitational acceleration, could seem useless. On the other hand, considering this case has a very practical importance (see sections V and VI).

V. CLOSED-FORM SOLUTIONS

This section provides closed form solutions which directly express the observable modes in terms of the sensor measurements collected during a short time interval. It starts by dealing with the case without bias.

A. The case without Bias

In the local frame, the dynamics are:

      Ḟ i = M F i -V V = M V + A + A g q = mq i = 0, 1, ..., N f -1 (10)
where F i is the position of the i th feature in the local frame (i = 0, 1, ..., N f -1), V is the vehicle speed in the same frame, A g is the gravitational acceleration in the local frame, i.e. Âg = q * âg q, and 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 . Finally:

m ≡ 1 2        0 -Ω x -Ω y -Ω z Ω x 0 Ω z -Ω y Ω y -Ω z 0 Ω x Ω z Ω y -Ω x 0        , M ≡      0 Ω z -Ω y -Ω z 0 Ω x Ω y -Ω x 0     
The validity of ( 10) can be checked by using F = -q * rq, V = q * vq and by computing their time derivatives with (1). In the local frame, the observation in (2) for the i th feature is:

h cam = [y 1 , y 2 ] T = F i x F i z , F i y F i z T (11)
Because of the gravity, the first two equations in (10) cannot be separated from the equations describing the dynamics of the quaternion. χ g will denote the gravity vector in the local frame at a given time T 0 . In other words, χ g ≡ A g (T 0 ). Note that, determining χ g allows determining the roll and pitch angles (R 0 and P 0 ). Indeed, from the definition of the roll and pitch angles it is possible to obtain:

χ g = g[sin P 0 , -sin R 0 cos P 0 , -cos R 0 cos P 0 ] T (12)
F i 0 ≡ F i (T 0 ) will denote the position of the i th feature (i = 0, 1, ..., N f -1) at T 0 . Similarly, V 0 ≡ V (T 0 ) will denote the vehicle speed at T 0 .

In the following, a closed form expression of the vectors F 0 0 , F 1 0 , ..., F N f -1 0

, V 0 and χ g in terms of the sensor measurements in the time-interval [T 0 , T 0 + T ] will be provided.

To derive this closed form expression it is useful to first consider the special case where the vehicle does not rotate during the interval [T 0 , T 0 + T ]. In this case, the first two equations in (10) become:

  Ḟ i = -V V = A + χ g i = 0, 1, ..., N f -1 (13)
It is immediate to integrate the previous equations and obtain the position of the i th feature in the local frame:

F i (t) = F i 0 -∆tV 0 - ∆t 2 2 χ g - t T 0 t T 0 A(τ )dτ dt (14)
where A(τ ) are provided by the accelerometers and ∆t ≡ t -T 0 .

Ξ(t) will denote the matrix which characterizes the rotation occurred during the interval [T 0 , t].

The equations in [START_REF] Martinelli | State Estimation Based on the Concept of Continuous Symmetry and Observability Analysis: the Case of Calibration[END_REF] correspond to the case when Ξ(t) is the identity 3 × 3 matrix for any time t ∈ [T 0 , T 0 + T ]. In the general case, i.e. when the vehicle is not constrained to move with a fixed orientation, Ξ(t) can be evaluated by using the data from the gyroscopes during this time interval (see appendix A). Hence, it is possible to obtain the extension of ( 14) to a generic motion:

F i (t) = Ξ(t) F i 0 -∆tV 0 - ∆t 2 2 χ g - t T 0 t T 0 Ξ -1 (τ )A(τ )dτ dt , i = 0, 1, ..., N f -1 (15)
In [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] the same result has been obtained by directly integrating the equations in [START_REF]Quaternions and rotation Sequences: a Primer with Applications to Orbits, Aerospace, and Virtual Reality[END_REF].

The components of F i (t), i.e. F i x (t; F 0 0 , F 1 0 , ...,

F N f -1 0 , V 0 , χ g ), F i y (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) and F i z (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) are linear in the unknowns F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g .
By using [START_REF] John | Partial Differential Equations[END_REF] the following linear equations are obtained:

F i x (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) = y 1 (t) F i z (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) (16) F i y (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) = y 2 (t) F i z (t; F 0 0 , F 1 0 , ..., F N f -1 0 , V 0 , χ g ) i = 0, 1, ..., N f -1
In particular, each camera observation occurred at the time t ∈ [T 0 , T 0 + T ] provides 2N f linear equations in the 3N f + 6 unknowns (which are the components of F i 0 (i = 0, 1, ..., N f -1), V 0 and χ g ).

When the camera performs observations from n obs distinct poses the number of equations provided by ( 16) is 2n obs N f while the number of unknowns is 3N f + 6. In order to determine the unknowns, it is fundamental to know whether these equations are independent or not. To this regard, according to theorem 1, we know that the number of independent equations is always smaller than the number of unknowns for n obs ≤ 3. On the other hand, when n obs = 3 the knowledge of the magnitude of the gravity makes possible the determination of the modes. 1) n obs ≥ 4: In this case the equations in ( 16) are in general independent. On the other hand, when n obs = 4 and N f = 1, the number of equations is 8, which is less than the number of unknowns 9. In section VI it is shown that, by using the knowledge of the gravity (i.e. the magnitude of the vector χ g ), it is possible to determine the unknowns by solving a second order polynomial equation. Hence, in this case, two solutions are determined. When n obs ≥ 5 and/or N f ≥ 2 the determination can be done by the computation of a pseudoinverse. Hence, a single solution can be obtained. Then, the knowledge of the magnitude of the gravitational acceleration, can be used to improve the precision (see section VI).

2) n obs = 3: When N f = 1 the number of equations is 6 and the number of unknowns is 9.

Hence the estimation cannot be performed. When N f ≥ 2 the number of equations is larger or equal to the number of unknowns. On the other hand, according to theorem 1, the vector χ g cannot be determined, since its norm is not observable. In other words, the equations in [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] are not independent. As in the case n obs = 4, N f = 1, it is possible to determine the unknowns by solving a second order polynomial equation. Hence, also in this case, two solutions are determined (see section VI and [START_REF] Martinelli | Vision and IMU Data Fusion: Closed-Form Solutions for Attitude, Speed, Absolute Scale and Bias Determination[END_REF] for further details).

Note that the previous remarks hold in general. There are special situations, whose probability of occurrence is zero, where the determination cannot be carry out. For instance, in the case n obs = 3, N f = 2, if one of the three camera poses is aligned along with the two features, the determination cannot be performed. Another special case is when the three camera poses and the two features belong to the same plane.

B. The case with Bias

The closed-form solution will be derived only when the accelerometers are affected by a bias, i.e. in the case A bias = [0 0 0] T and Ω bias = [0 0 0] T .

The expression in [START_REF] Martinelli | Closed-Form Solution for Attitude and Speed Determination by Fusing Monocular Vision and Inertial Sensor Measurements[END_REF] can be easily extended to deal with this case by the substitution:

A(τ ) → A(τ ) + A bias : F i (t) = Ξ(t) F i 0 -∆tV 0 - ∆t 2 2 χ g - t T 0 t T 0 Ξ -1 (τ )dτ dt A bias - t T 0 t T 0 Ξ -1 (τ )A(τ )dτ dt (17)
i = 0, 1, ..., N f -1

By proceeding as in the case without bias the analogous of equations ( 16) is obtained. The new equations also depend on the vector A bias .

VI. THE ALGORITHM

This section describes the algorithm which allows determining the speed, the attitude and the absolute scale starting from the inertial and visual data collected in a very short time interval.

The extension to also determine the bias is straightforward.

As stated at the beginning of the previous section, the local frame is the camera frame.

Hence, it is necessary to determine the expression of the acceleration and the angular speed of this frame starting from the acceleration and the angular speed provided by the IMU and from the knowledge of the transformation between the IMU frame and the camera frame (which is assumed a priori known).

The following notation will be adopted:

• C is the matrix which transforms vectors in the IMU frame in vectors expressed in the camera frame;

• D is the vector describing the position of the origin of the camera frame in the IMU frame;

• Ω IM U and A IM U are the angular speed and the acceleration in the IMU frame (i.e. provided by the inertial sensors).

The expressions of the angular speed and the acceleration in the camera frame are:

Ω = CΩ IM U , A = C A IM U + ΩIMU ∧ D + Ω IM U ∧ (Ω IM U ∧ D) (18)
The previous expressions must be used to obtain all the inertial measurements in the camera frame.

The second step consists in computing the matrix Ξ at each time step when the inertial data are delivered (see appendix A). This allows computing the term -t T 0 t T 0 Ξ -1 (τ )A(τ )dτ dt for all the time t when a camera image is available.

The linear system in ( 16) will be denoted with:

Γx = β (19)
where the vector x contains all the unknowns, i.e. x ≡ [F 0 0 , F 1 0 , ...,

F N f -1 0 , V 0 , χ g ] T . Γ and
β are respectively a (2n obs N f × 3N f + 6) matrix and a (2n obs N f × 1) vector and are obtained as follows. For a camera observation occurred at time t, each feature contributes with two rows to the matrix Γ and with two entries to the vector β.

For the j th feature observed at time t the three rows of the matrix Ξ(t) will be denoted with ξ 1 (t), ξ 2 (t) and ξ 3 (t). The two rows of the matrix Γ are, respectively:

0 3j-3 , ξ 1 (t) -y 1 (t)ξ 3 (t), 0 3N f -3j , -∆t (ξ 1 (t) -y 1 (t)ξ 3 (t)) , - ∆t 2 2 (ξ 1 (t) -y 1 (t)ξ 3 (t)) 0 3j-3 , ξ 2 (t) -y 2 (t)ξ 3 (t), 0 3N f -3j , -∆t (ξ 2 (t) -y 2 (t)ξ 3 (t)) , - ∆t 2 2 (ξ 2 (t) -y 2 (t)ξ 3 (t))
where 0 n denotes the row-vector whose dimension is n and whose entries are all zeros. The corresponding two entries in the vector β are, respectively:

[ξ 1 (t) -y 1 (t)ξ 3 (t)] t T 0 t T 0 Ξ -1 (τ )A(τ )dτ dt , [ξ 2 (t) -y 2 (t)ξ 3 (t)] t T 0 t T 0 Ξ -1 (τ )A(τ )dτ dt
As stated in the previous section, it is possible in general to compute the pseudoinverse (or the inverse) of the matrix Γ in the following cases:

1) when n obs ≥ 4 and N f ≥ 2;

2) when n obs ≥ 5 and N f = 1.

When the rank of Γ is one less than the number of its columns, the nullspace of Γ has dimension one. As discussed at the end of the previous section, this is in general the case when n obs = 3, N f ≥ 2 (because of the theorem 1) or when n obs = 4, N f = 1. In this case, the system in (19) has an infinite number of solutions. By denoting with ν the unit vector belonging to the nullspace of Γ, with x p one among the solutions of (19), any solution of ( 19) is

x = x p + λν
where λ is a real number. On the other hand, by knowing the magnitude of the gravitational acceleration, it is possible to determine two values of λ. This is obtained by enforcing the constraint that the vector s λ constituted by the last three entries of the solution x p + λν is a vector with norm equal to g. In other words:

|s λ | 2 = g 2 (20)
which is a second order polynomial equation in λ. Hence, in this case two solutions are determined.

Finally, when Γ is full rank, the knowledge of the magnitude of the gravitational acceleration can be exploited by minimizing the cost function:

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

The main steps of the algorithm are displayed in the algorithm 1.

Algorithm 1 (Returns features's positions, speed and attitude)

Inputs: A IM U (t), Ω IM U (t), y i 1 (t), y i 2 (t), (i = 0, 1, ..., N f -1), t ∈ [T 0 , T 0 + T ] Outputs: F i 0 , V 0 , χ g , (i = 0, 1, ..., N f -1)
Compute A and Ω by using [START_REF] Veth | Fusing low-cost image and inertial sensors for passive navigation[END_REF] Build the matrix Γ and the vector β in (19)

Compute the rank (r) of Γ if r = 3N f + 6 then

VII. PERFORMANCE EVALUATION

This section shows the results obtained by using the algorithm with a real data set. 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 USBcamera 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. Let consider a vehicle and a frame attached to this vehicle. When the vehicle moves during the infinitesimal interval [t j , t j + δt], the rotation matrix which transforms vectors in the reference before this motion and the reference after this motion is: I 3 +M j δt, where I 3 is the 3×3 identity matrix and M j is the skew-symmetric defined in section V at the time t j . Now, let us suppose that the vehicle moves during the interval of time [t i , t f ]. In order to compute the rotation matrix which transforms vectors in the reference before this motion and the reference after this motion, the path is divided in many (N ) steps. For each step, the expression of the rotation matrix is the one previously provided. Then, it suffices to compute the product of all these matrices, namely:

Ξ = N k=1 (I 3 + M k δt k ) (22)
where t 1 = t i and t N = t f .

  A and C, it is obtained 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 the value of v A is obtained. Hence, the value of all the segment lengths in fig 1 is obtained in terms of the measurements performed by the accelerometer and the bearing sensor.

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 , respectively from the points A, B and C.

Fig. 2 .

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

x

  in = pinv(Γ)β minimize c(x) in (21) with initialization x in else if r = 3N f + 5 then Determine two solutions by (20)

Figure 3 a

 3 Figure 3 a shows the trajectory (ground truth) during the time interval [200, 240]s.

Figures 3 bFig. 3 .

 33 Figures 3 b and 4 show the results regarding the estimated speed, roll and pitch angles, respectively. In all these figures, the blue dots are the ground truth while the red disks are the estimated values.

Fig. 4 .

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

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 , 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 , L 3 f 0 , f 0 , f 6 y 2 .

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 , 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 0 y 1 , 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 3 , f 0 y 1 , L 3 f 0 , f 0 , f 6 y 2 .As previously mentioned, we know that we cannot have more than 16 independent Lie derivatives (otherwise, the yaw angle would be observable). The fact that we have 16independent Lie derivatives means that there are no additional symmetries and, the independent observable modes, are the independent solutions of[START_REF] Goldstein | Classical Mechanics[END_REF]. They are: the 15 solutions provided in


Patrick O Kamgueu

Emmanuel Nataf

Thomas Djotio

Olivier Festor

Energy-based routing metric for RPL

Keywords: routing, energy efficiency, RPL, Wireless Sensor Network routage, efficacité en energie, RPL, Réseau de capteurs sans fil

Saving power while ensuring acceptable service levels is a major concern in wireless sensor networks, since nodes are usually deployed and not replaced in case of breakdown. Several efforts have recently led to the standardization of a routing protocol for low power and lossy network. The standard provides various metrics, which can be used to guide the routing. Most protocol implementations use expected transmission count as the routing metric, thus focus on the link reliability. To our knowledge, there is no protocol implementation that uses the nodes remaining energy for next hop selection. This document discusses about the usage of the latter as the routing metric for RPL, the new standard for routing for Low power and Lossy Network (LLN). We design an objective function for that metric and compared experiments result with the most popular expected transmission count scheme.

Introduction

Wireless Sensor Network (WSN) consist of up to hundreds or thousands of nodes scattered in an environment of interest, where nodes and their interconnect are constrained. Usually, each node sense its vicinity and forward measured parameters at a central point: the sink, through its neighbour via multi-hop radio communication. Nodes discover their neighbours, self-organize to build a topology and route sensed data. To deals with challenge presented by LLN, IETF Roll Working Group have recently published sevral standards related to RPL [START_REF] Winter | RPL: IPv6 Routing Protocol for Low-Power and Lossy Networks[END_REF] [START_REF] Jp | Routing Metrics Used for Path Calculation in Low-Power and Lossy Networks[END_REF] [19] [START_REF] Gnawali | The Minimum Rank with Hysteresis Objective Function[END_REF].

RPL organizes networks as one or more Directly Acyclic Graph (DAG), each one rooted at a single point: the DAG root. Topology construction begins at this point, which periodically sends Destination Oriented DAG Information Object (DIO) via link local multicast. DIO carries necessary informations to build the topology, including root unique identifier, routing metrics, originating router's depth also called rank, and other network parameters. Nodes in the vicinity receiving DIOs, join DAG by selecting their parents (one or more) as next hop upwards to the sink. Parent selection process is governed by an Objective Function (OF), which uses routing metrics to select node's preferred parent among neighbours. Once node has made preferred parent selection, it determines its relative position to the DAG root based on the latter's rank, then node can originate its proper DIO messages. Different criteria also called routing metrics [START_REF] Jp | Routing Metrics Used for Path Calculation in Low-Power and Lossy Networks[END_REF] are defined to capture link or node characteristics on the path for parent selection. They could be node attribute: hop count, node residual energy, or link attribute : throughput, latency, link quality level or expected transmission count (ETX). The latter is widely used in wireless sensor networks [START_REF] Couto | A high-throughput path metric for multi-hop wireless routing[END_REF], moreover there are several RPL ETX-based implementations and IETF Roll Working Group has a long experience of routing [START_REF] Hui | Ip is dead, long live ip for wireless sensor networks[END_REF] with this metric. The hop count and ETX are the only metrics for which standards related to their usage in RPL are published [START_REF] Thubert | Objective Function Zero for the Routing Protocol for Low-Power and Lossy Networks (RPL)[END_REF] [START_REF] Gnawali | The Minimum Rank with Hysteresis Objective Function[END_REF]. Standards do not state how the other link/node attributes are transformed into path cost, nor how these costs are translated into DAG rank.

In this paper, using an online real time battery level estimation model, we design an OF for RPL that used node remaining energy as metric. The proposed OF is compared against the popular that rely on ETX.

The remainder of the document is organised as follows. In the next section we describe energy-based OF characteristics in terms of node battery level estimation, path cost and node rank computation. Section 3 presents protocols requirements and sufficient conditions for the routing metric to ensure proper operations, followed by some related work on energy aware routing in section 4. Implementation parameters, simulations and results are discussed in section 5. Section 6 concludes our work and discusses future directions.

Energy-based Objective Function

Path cost computation

The path cost or weight is a scalar value representing link or node characteristics along end-toend path, for which the latter expresses some quality level. The calculation of this value depends on metric chosen by the network operator and must be the same on all nodes. Several choices are possible but they are not all reasonable. For example, following ETX approach, the path cost is computed as a sum of expected reliability on traversed links. For energy-based metric, we compute path cost as the minimum node energy level, that in our opinion, better captures the energy-based path weight than the sum of all nodes' energy along the path. According to the selected metric, optimal path from a given node is the one that maximizes or minimizes the path weight to the sink. After receiving a DIO from a neighbour, any non-root nodes computes the path cost through this neighbour:

• If routing metric is a node attribute, the path cost through that neighbour is the one indicated in the metric container option of the sender's DIO.

• Instead, if it is a link attribute, the path cost through that neighbour is computed from link metric on the interface to reach this neighbour and the path cost in the sender's DIO.

After a node has calculated path cost for all its neighbours and chose best parent in regard of the relation order for the selected metric, node updates its metric container (by computing it path through that parent) and starts to send its own DIOs.

Node' battery level estimation

To predict the lifetime of the node, we use a well-known battery model found in [START_REF] Rakhmatov | Energy management for battery-powered embedded systems[END_REF]. It uses the current consumption during each node state and its duration to estimate the battery remaining energy. The model is very accurate and cannot be implemented on real sensor nodes due to its complex computations and the memory size requirements. Rahmé et al. [START_REF] Rahmé | Energy management for battery-powered embedded systems[END_REF] have approximated the latter by simple computations on low memory to fit into sensor nodes, while maintaining the original model accuracy. Based on these approximations, we implemented this model on real sensor nodes, with the possibility to predict their lifetime online. This work can be found in a separate document [START_REF] Nataf | Online estimation of battery lifetime for wireless sensors network[END_REF]. Following RPL metrics recommendations [START_REF] Jp | Routing Metrics Used for Path Calculation in Low-Power and Lossy Networks[END_REF], node residual energy is estimated on a scale of 255 (full) to 0 (empty). A key asset of the used model is that it take into account not only the rate capacity, but also the recovery effect that occurs during idle time.

Note that this state may represent more than 90% of the node lifetime.

Energy based Path cost

We define the path cost P W i from a node i to the sink as the minimum value between the preferred parent path cost and its own energy. The sink node set the value as M AX energy . A node selects the neighbour that advertises the greatest path cost value as parent. More formally:

P W i = min[max j∈Ni (P W j ), E i ] (1)
Where N i is the set of node i's neighbours toward the sink, and E i represents the energy of node i. For a given path, this value is also the same as the minimum node's energy level encountered on that path, since this energy is critical for the route lifetime.

Network topology shown in figure 1 depicts the proposed energy-based path cost. Node 1 is the sink and is main powered, other nodes at a given time are supposed to have residual energy as shown. To describe the topological structure of the network, we consider node 6 that selects 3 as next hop. Dashed lines and arrows represent neighbour reachability. Node 6 receives neighbour's 3, 4, 5 and 8 DIOs, containing respectively values P W 3 = 220, P W 4 = 215, P W 5 = 217, and P W 8 = 240. So 6 selects 3 which indicate higher path cost, as best parent. Following the procedure described above, similarly other nodes select their next hop to the sink. At another time, assume that node 3, the most solicited waste more energy compared to others, so that it decreases by 20 units, while 5 units for others. Figure 2 illustrates how network topology reacts according to the new path cost computed by each nodes:

1. Node 3 is no longer the best parent to the sink for 6, which finds a more interesting path through 4 (P W 3 = 200 while P W 4 = 210).

2. Similarly, node 5 follows 6 that offers a better path than 3 (P W 3 = 200 while P W 6 = 205).

Inria

Energy based DAG Rank

To avoid cycle in the network, every node uses a scalar values : the rank, to record its relative position to other nodes with regard to DODAG root. RPL does not states how rank values are computed at each nodes, but the values must implement generic properties regardless the objective function in uses. The rank is not the path cost, although its value is derived from the latter. Rank value must monotonically decrease as we move upwards to the sink, but it does not necessarily change as fast as some link or node metrics would. For the latter reason rank values is thought as a fixed-point number where the position of the radix point between the integer part and the fractional part is determined by the MinHopRankIncrease parameter (provided by the DODAG root). When rank is compared for parent relationships or loop detection purpose only the integer part is used, but OF computes entire fixed-point value (16-bit). Once a node (say N) has chosen its preferred parent (P), node computes its own rank from preferred parent's rank as defined in [START_REF] Couto | A high-throughput path metric for multi-hop wireless routing[END_REF] where step = M AX energy -N ode energy .

Rank(N ) = Rank(P ) + Rank increase where

Rank increase = step + M inHopRankIncrease (2)
This formula ensures the monotonic property of the rank which increases by at least one point (MinHopRankIncrease) between node and its preferred parent, when child node has a full battery level. The increment is even greater as node consumes its battery, because of penalty of step which feeds the fractional part of fix-point rank value. By cumulative effect of penalties in the node's parentage, node's rank can grow to more than one point (see Table 1 -rank increase between node 5 and 7). Figure 2 illustrates how rank is derived from path cost, and table 1 emphasizes on rank computation on path 1-4-6-5-7-9. Root rank is set to the same value as MinHopRankIncrease (256 in this example).

Protocol requirements and routing metric properties

The diversity of wireless sensor network requirements motivates the design of various routing metrics, to capture different aspects of wireless communication. For that reason, Yang et al. [START_REF] Yang | Design guidelines for routing in wireless sensor network[END_REF], provide a systematic analysis of the relationship between routing metrics and routing protocols. Taking into account the theoretical algebra framework of Sobrinho [START_REF] Sobrinho | Network routing with path vector protocols: Theory and applications[END_REF], they identify the basic properties that a routing metric must have in order to properly operate with different proactive or reactive routing protocols in wireless networks. Metrics that do not meet these requirements may lead to routing loops and suboptimal paths. The following requirements ensure proper operations for routing protocol:

• consistency: routing protocol is said consistent if packet forwarding decisions for all nodes along a given path is consistent with each node in that path. In this sense, if node n 1 decides that the traffic to n k should follow the path p(n

1 , n k ) = n 1 , n 2 , • • • , n k ,
other nodes along this path should make the same decision, i.e n 2 should choose path p(n 2 , n k ) = n 2 , n 3 , • • • , n k to forward traffic to n k , and the same for

n 3 , • • • , n k-1 .
• optimality: It is a generic requirement for routing protocol, such as it always forward packets along the lightest path (in regard of weight of metric) between every pair of nodes. Lack of this requirement may be caused by inconsistency routing.

• loop-freeness: it is the most relevant requirement for a routing protocol, routing loops occurs when a packet is forwarded between a set of nodes without ever reach its final destination.

As established by [START_REF] Sobrinho | Network routing with path vector protocols: Theory and applications[END_REF], necessary and sufficient conditions for a routing metric to achieve these requirements are to satisfy properties of isotonicity and monotonicity. To highlight this, a routing metric can be represented as an algebra on top of a quadruplet (Σ, ⊕, w, ), where Σ is the set of all path, ⊕ the path concatenation operation, w a function that maps a path into a cost and is an order relation.

Inria

Isotonicity

The isotonic property mean that the order relation between two paths is preserved if they are both prefixed or appended by a common third path. More formally, the algebraic structure

(Σ, ⊕, w, ) is said isotonic if ∀a, b, c ∈ Σ, w (

Monotonicity

The isotonic property mean that the path cost will not decrease when prefixed or appended by another path. More formally, (Σ, ⊕, w, ) is said monotonic if w(a) w(c ⊕ a) and w(a) w(a ⊕ c) holds ∀a, c ∈ Σ.

As shown below, the proposed minimum energy-cost metric for RPL is proved both isotonic and monotonic. Thus in accordance with [START_REF] Yang | Design guidelines for routing in wireless sensor network[END_REF], we conclude that the routing metric satisfy consistency, optimality and loop-freeness requirements. The algebraic structure build for the proposed metric is (Σ, ⊕, min, ≥), where min(p) is the minimum energy-based cost along the path p and the order relation for that routing metric is ≥. Since ⊕, the path concatenation operator is commutative for min function, we can limit the demonstration only to the left-isotonicity and left-monotonicity (i.e for prefixed paths) of the algebraic structure. Indeed: Isotonicity: Let us consider two given paths a and b so that min(a) ≥ min(b), we want to show that this implies min(c ⊕ a) ≥ min(c ⊕ b). For the former inequality three cases could occur for a given prefixed c path.

Related Wok on Energy-aware Routing

One major goal in Wireless Sensor Network is conserving the sensor nodes energy and thus maximizing the network lifetime. Several protocol have been proposed in the literature taking into account various parameters to improve the energy efficiency in the network. Some work have focussed on energy efficient techniques including data aggregation, network clustering [START_REF] Heizelmann | Adaptative protocols for information dissemination in wireless sensor networks[END_REF], data centric or event driven [START_REF] Intanagonwiwat | Directed diffusion : A scalable and robust communication paradigm for sensor networks[END_REF]. Among the techniques used to maximize the network lifetime, energy aware routing protocol appears to be suitable for multi-hop wireless sensor networks, since they explicitly take into account node residual energy for route establishment. As energy is depleted, the network may be required to reduce the quality of sent data in order to reduce energy dissipation. The Energy Efficient Shortest Path protocol [START_REF] Shivaprakasha | Energy efficient shortest path routing protocol for wsn[END_REF] follows shortest path algorithm by combining distance (hop count) and node residual energy as cost . This combination uses the energy metric at the denominator of the distance parameter.

Similarly, authors in [START_REF] Mohajerzadeh | An efficient energy aware routing protocol for real time traffic in wsn[END_REF] propose an algorithm which considers both energy and delay metric to find an optimal path with minimum energy consumption and minimum end to end delay for real time traffic in wireless sensor networks. This cost is computed as a linear combination of the transmission delay and node's energy on the path.

Chiang and al proposed Minimum Hop (MH) routing protocol [START_REF] Chiang | A minimum hop routing protocol for home security systems using wsn[END_REF] which organize routing topology based on nodes hop counts and battery power levels. For a given nodes, neighbours are classified into three categories: parent, sibling and child node, on the basis of their vicinity in hop count to the sink. A Parent node has always a hop count one less than the sending node, sibling node the same, whereas the child node is in the transmission range but having a hop count one more than the sending node. MH first try to reach sink by path through a parent node, which guarantee a minimum hop path. In case of more than one parent protocol uses parent with the highest energy level. If there is no parent node available, sending node forward data through the sibling node which has the highest energy.

MH uses a local (parent and sibling) energy view of the sender for the next hop selection, and does not always reflect the real energy distribution of nodes in the path. On contrary, authors in [START_REF] El-Semary | Path energy weight: A global energy-aware routing protocol for wireless sensor network[END_REF] proposed the path energy weight (PEW) protocol that improve MH by using an energyweighted function to indicate how balanced is the energy distribution among all nodes along the path. With the same notation as the equation 1, the path cost is:

P W i = 2 (-Wmax Emax Ei+Wmax) min j∈Pi∪Si (P W j ) (3)
Where P W j is the path with best energy weight function from the set of all parents (P i ) and siblings (S i ) nodes of the sensor i to the sink. W max and E max are experiments dependant parameters which bound P W values. To illustrate MH and PEW operations, we consider the Following this scenario, node E sends data through E → C → A → S, and therefore PEW protocol more extend network lifetime than MH. Note that, on the one hand this path weight takes into account all nodes energy along the path, although greatly disadvantaging lower energy nodes. In our approach, only the lowest nodes constraint the path. On the other hand, the set of the potential next hop P i ∪ S i is only determined by hop count, whereas we use the rank notion to choose the next-hop among the neighbours, while avoiding loops.

Unlike previous protocols which attempt to find a minimum energy cost path, REAR (Reliable Energy Aware Routing) [START_REF] Hassanein | Reliable energy aware routing in wireless sensor network[END_REF] provides an energy-sufficient path instead. The protocol is intended to establish a reliable transmission environment for data packet delivery with low energy cost. The algorithm proceed in four steps: ( 1 When the sink receives an interest, it checks its routing table for a route to the target. If no route the source exits in the table, the sink floods a service-path request through the network until it reaches the source. After the source retrieves the route information from the first received request packet, it will unicast a corresponding service-path reservation request along the retrieves path towards the sink, indicating the amount of energy requested to reserve (E resv ). Every intermediate node on this path will mark part of its energy as reserved for this communication, and that one is no longer available for other communication. After end of this service path discovery and service path reservation, the sink will launch a backup path discovery to the same source. SPD and BPR are carried out in the same manner except that only the intermediate nodes which are not on the service-path, will relay messages to their neighbours. Therefore, a two completely disjoints paths are established and reserved between sink and source for reliability purpose and fast topology reconstruction. Reserved energy release is initiated when the path is broken. The path-reservation process is concerned with energy, different sources might reserve different quantity of energy for their path, depending on an estimation of the quantity of data from that source.

To facilitate the selection of path having high energy level nodes, REAR introduces delay when flooding network during route discovery. Each intermediate node does not broadcast the message to its neighbours upon reception, instead each node delays the message based on its available energy (which is the residual remaining energy excluding all reserved energy). Using this delay scheme, energy-weak nodes are kept out of the potential path, and time-shortest route are promoted. One major challenge with REAR is to determine the accurate value for E resv , since reserved energy along the service and backup path can prevent some nodes to be selected for another Sink -Source route, even though the backup path is not used. Since the wireless sensor network environment is error-prone with a high packet loss rate, the delayed process during route discovery can lead to select less available energy-sufficient path, leading some nodes to depleted more quickly their energy. Moreover, REAR is an on demand, reactive routing protocol, it would not be appropriate for networks where nodes send data very frequently.

Simulations and results

Environment setup

Using Cooja simulator [START_REF] Osterlind | Cross-level sensor network simulation with cooja[END_REF] we evaluated the performance of RPL with both, energy-based and expected transmission count metrics, on the tiny networked sensors operating system Contiki [START_REF] Dunkels | Contiki -a lightweight and flexible operating system for tiny networked sensors[END_REF]. Experiments were carried out using a 300 × 300m 2 2D-grid of 20 sensors network topology. The sink is located at the left upper corner. Each sensor node acts in 120m maximum transmission range, 140m interference range, and periodically sends data to sink using UDP as the transport layer with a Tx/Rx success ratio of 80%. The layer 2 medium access control is ContikiMAC [START_REF] Dunkels | The contikimac radio duty cycling protocol[END_REF] that provides power efficiency by the node keeping their radios turned off for roughly 99% of the time. All nodes have fully charged battery at the beginning of the simulations, with an initial power level set to 880mAh. The hardware characteristics for the simulation computer is 3.2Ghz Dual Core Intel XEON processor board, with 8GiB Memory size, on Ubuntu 11.10 Operating System. Figure 4 presents a snapshot of the network and nodes layout, where the arrows indicate the selected next hop according to the routing metric. For example, concerning the node 7, the green area define its transmission range and the gray circle, the corresponding interference range. Below each circumscribed node is indicated the link quality with the selected node.

Results

Simulations were performed for one month network activities (corresponding to 13 real days on our simulation computer) to demonstrate the increase on network survivability without lack of accuracy. We define the network lifetime as the date on which the first node has completely exhausted its battery [START_REF] Dietrich | On the lifetime of wsn[END_REF]. The energy aware RPL implementation was compared against the ETX implementation. For both, the sink collects data generated at various throughput expressed as the number of application packets per minute (pkts/min), each having 87 bytes of size. Then, we Inria evaluate nodes energy depletion and packet delivery ratio for both scenarios, one at 1pkt/min, the other at 6pkts/min.

Remaining Power Distribution

Energy aware routing aims to use nodes with higher remaining power level, thus these nodes drain their battery more quickly and further become less attractive to relay data. The network should be reorganized to find more interesting nodes for routing and so on, thereby a balancing on all nodes battery levels should occur. This can be seen in figure 5 which presents the proportion of nodes in the network with the corresponding percentage of remaining energy at the end of the simulation. In figure 5a at 1pkt/min, 85% of nodes have their power level between the range 54% to 56%, whereas the ETX-based routing spread the energy distribution unequally among the nodes. At a higher rate (6pkts/min) in figure 5b, this observation is much more pronounced, since the traffic flow is more important and nodes exhaust their battery much faster. At the same time, in both illustrations the ETX-based scheme presents much less-power nodes (around 20%) than the energy aware scheme, the latter delaying the first nodes that will completely exhaust their battery and the possibility to create network holes. This is an important point, because the network integrity can be affected when some nodes are stopped. Figure 6 represents the battery variation for lowest power nodes at a throughput of 6pkts/min in both implementations, using these informations, we estimated by a linear regression when first nodes drain completely their energy. Computations indicate a network lifetime of 35 days for ETX-based RPL, while 40 days for energy aware scheme, thus the increase in network lifetime is around 14%.

Transmission Accuracy

We also evaluated the accuracy of routing to collect the application data. ETX-based routing promotes routes with higher packets delivery ratio, while energy aware routing don't care on that. It is therefore not surprising that the number of received packets with ETX is slightly greater than energy aware scheme as outlined by the figure 7, which depicts the total number of received packets from each node by the sink. The table 2 summarize all received packets at the sink, as well as all sent packets for both rates. Again, ETX-based routing delivery ratio is better than energy aware scheme, but this difference in only around 3%.

Conclusion and future works

In this paper, we presented an instantiation and implementation of the routing protocol for low power and lossy network that uses the node's remaining energy as the main routing metric. The implementation makes use of a well-known battery theoretical model from which we estimate at runtime the node battery lifetime for routing. Experiments reveal that, compared to the popular RPL ETX-based scheme, the proposed implementation increase the network lifetime and distributes energy evenly among nodes without an appreciable lack of the transmission accuracy.

Our future works aims to combinate these both metrics (energy and ETX), in accordance with [START_REF] Zahariadis | Design guidelines for routing metrics composition in lln[END_REF]. We expect to leverage the strengths of each, and obtain a better compromise. Furthermore Inria we seek to provide additional decision criteria in order to better guide the routing decisions in WSN.

Figure 1 :

 1 Figure 1: Example of energy based path cost computation

Figure 2 :

 2 Figure 2: Rank calculation for energy-based OF

  a) w(b) implies both w(c ⊕ a) w(c ⊕ b) and w(a ⊕ c) w(b ⊕ c).

1 .

 1 min(c) ≥ min(a): we have min(c ⊕ a) = min(a) and min(c ⊕ b) = min(b) =⇒ min(c ⊕ a) ≥ min(c ⊕ b).2. min(a) ≥ min(c) ≥ min(b): we have min(c ⊕ a) = min(c) and min(c ⊕ b) = min(b) =⇒ min(c ⊕ a) ≥ min(c ⊕ b). 3. min(b) ≥ min(c): we have min(c ⊕ a) = min(c ⊕ b) = min(c) =⇒ min(c ⊕ a) ≥ min(c ⊕ b). Monotonicity: Now, we seek to show that min(a) ≥ min(c ⊕ a). ∀a, c ∈ Σ. Two cases could occur: 1. min(a) ≥ min(c): we have min(c ⊕ a) = min(c) =⇒ min(a) ≥ min(c ⊕ a). 2. min(c) ≥ min(a): we have min(c ⊕ a) = min(a) =⇒ min(a) ≥ min(c ⊕ a).

Figure 3 :

 3 Figure 3: MH and PEW operations

  ) Path Discovery including Service Path Discovery (SPD) and Backup Path Discovery (BPD); (2) Energy Reservation including Service Path Reservation (SPR) and Backup Path Reservation (BPR); (3) Reliable Transmission (RT) and (4) Reserved Energy Release (RER).

Figure 4 :

 4 Figure 4: A snapshot of the simulated network

Figure 5 :

 5 Figure 5: Nodes Remaining Power Distribution

Figure 6 :

 6 Figure 6: Power depletion for lowest nodes

Figure 7 :

 7 Figure 7: Received packet at the sink node

Table 1 :

 1 Detailed Rank Calculation

	Node ID Fix-point value Fractional Part Interger Rank value
	1	256	0	1
	4	557	45	2
	6	863	95	3
	5	1162	138	4
	7	1568	32	6
	9	1834	42	7
	RR n°8208

Table 2 :

 2 Transmission Accuracy

RR n°8208