a critical look to my presentation and my speech but also gave me valuable ideas and materials! Special thanks also to Florent (a.k.a. Bernard) for all the energy he spent to help me prepare the post-defense cocktail! My deepest thanks also to those friends I shared most of my time with during those three years. You provided a priceless help in times of need. You were always ready to listen to my concerns and give your own vision and advice. I need to mention at least Thomas, Bernard and Kevin there but the list is far to be limited to them.

Finally, I would like to thank my family for encouraging me to keep learning new things and the obstacles they overcame so that I can study in good conditions. And now I would like to express my most sincere thanks to the racoon-fangirl, Clémence (a.k.a. 'Madame Patate') and the Berserker for their moral

First of all, it is customary to thank the directors Nicolas Mansard, Joan Solà and Olivier Stasse and I really think that it is deserved. This work would have been impossible without their advices, support, encouragements, explanations and the opportunity that they gave to me. Besides I learned a lot just talking with them while at the same time having fun.

Thank you Nicolas for your expertise, for your humanity and for trusting me when even I had doubts. Despite your busy schedule you always managed to find some time to not only take part to discussions but also to initiate some and to have an attentive look on the direction taken by this work.

Thank you also Joan for your involvement and the energy you put in this work from the very first day and even before the thesis started! You managed to have a close attention to what I was doing and to actually help me by getting your hands dirty. I think I will remember those intensive debug sessions we had to take care of for a long time! Thanks to Olivier who supervised my work in the first year with enthusiasm for his advice! I think that this supervision team was really interesting since I had the opportunity to have different views of the same problem and different pedagogical approaches that helped me understand things! My deepest thanks also to the reviewers of this thesis, Patrick Rives and Silvère Bonnabel, who took time to read the manuscript and give some valuable feedback. Of course I will not forget to thank also the examinators (David Filliat, Manon Kok, Patrick Wensing and Cyril Roussillon) for taking time to assist to the defense. I would like to thank all my colleagues (not only from LAAS but also from IRI) and co-authors for their collaboration, their patience and the enlightening discussions we have had on many and varied subjects.

Thanks to 'Les barbus' team (that I may also call 'Nounours et C ie ') for making get out of the tiny manor I was living in and giving me the opportunity to visit places around Toulouse! Thanks also to the role play team for the adventures we lived towards gloomy lands with so much fun! Thanks to the tarot team for making everyday only more enjoyable. Many thanks to all these colleagues (not only from the robotics teams) who made the LAAS a friendly place where working is fun.

I also have to thank all the people who helped me to prepare my defense. There I need to mention my directors again but also Céline Pieters who did not only took cos ωt + i sin ωt. Its velocities when passing through 1 and z are in the respective tangent spaces, T S 1 1 and T S 1 z . In the case of T S 1 z , the velocity is ż = z iω = -ω sin ωt + iω cos ωt when expressed in the global coordinates, and z v ∧ = iω when expressed locally. Their relation is given by z v ∧ = z -1 ż = z * ż. In the case of T S 1 1 , this relation is the identity 1 v ∧ = ż = iω. Clearly, the structure of all tangent spaces is iR, which is the Lie algebra. This is also the structure of ż at the identity, and this is why the Lie algebra is defined as the tangent space at the identity. .

List of Figures

Introduction Perception in living beings

Most living beings benefit from their capacity to successfully navigate in space largely thanks to their capacity of localise themselves. This localisation cannot be separated from perception and from the development of sensors with new capabilities. Good examples of the aforementioned sensors are the eyes of animal species which differ from each other (Fig. 1) according to needs in terms of survival and in terms of the environment in which they live (e.g. on the surface / under water/ ...).

However, living beings do not only rely on a single sensor but rather have a multitude of sensors that are used together in a complex way that has not been fully understood yet. A simple way to understand how crucial sensor fusion is in our everyday life is to consider the importance of the inner ear for navigation purposes. The inner ear (see Fig. 2) is not only responsible for sound detection and the balance of our body but also for the perception of the head's angular position and its acceleration. A failure in this sensor can cause the loss of balance and therefore difficulties to navigate in one's environment. Furthermore, the Human proprioception includes the sense of the relative position of one's own parts of the body and strength of effort being employed in movement [Glanze 1994]. Proprioceptors are present in our muscles, tendons and joints to provide us with the sense of pain, hunger or the position of our members and giving rise to the kinesthetic sense. [Bloom 1988] defines kinesthesis as part of the somatosensory system that is conscious bodily perception distributed throughout the whole body. Kinesthesis is the subject of various studies concerning its relation to movements [Young 1945] alone or with other senses [Lovelace 1989[START_REF] Day | Spatial aftereffects within and between kinesthesis and vision[END_REF]. Let us take the example of a cheetah chasing a prey. Its sensors are acquiring much information about not only the world around (structure of the ground, position and velocity of the prey, ...) but also its internal state (position of limbs, forces applied on the body, interaction with the environment, ...). The central nervous system is then responsible for managing all the incoming information and taking a decision. In some cases, the central nervous system may even not take action as it is the case for reflex. This complex sensory system then allows to react to changes in the system, may it be internal changes or related to the environment itself. We can mention the specific case of the unintended (and maybe unexpected) slippage of paws during running. We could say with cautions that the cheetah is then able to dynamically adapt its motion to balance the effect introduced by the slippage.

Perception in robotics

As for human beings, estimating the position of a mobile robot and its configuration is a mandatory step performed through the processing of available information. In- The yellow parts are the three semicircular canals in charge of sensing rotational movements. Otolithic organs are located at the base of the semicircular canal system. They sense the linear accelerations of the head.

List of Figures

formation are provided by various sensors that can be separated into two categories:

proprioceptive sensors measuring values internal to the robot which can be for example the joint angles of the robot, the motor speed, its temperature or its battery voltage level. Examples of such sensors are encoders, gyroscopes, potentiometers, etc.

exteroceptive sensors acquiring measurements from their observation of the outside world. We can cite sonar sensors, ultrasonic distance sensors, cameras or accelerometers due to the measure of the gravity vector as examples. The gathered data must be interpreted to extract a meaningful feature.

The data must then be processed to have an estimation of the state of the robot that would be as close as possible to the reality. However, robots are required to navigate in a changing environment and to interact with it to gain access to some parts inside it or in order to modify it. For this reason, we are likely to ask not only for a precise localisation but we may also need it at a high update rate.

Mobile robots usually have a wide variety of sensors (both exteroceptive and proprioceptive) integrated in their systems. Fig. 4 shows examples of sensors integrated in some mobile robots while Fig. 5 hightlights one possible solution for the design of a humanoid robot. The kinematic chain of the robot is formed of the succession of joints and encoders are used to move each segment. Force and torque sensors are usualy placed at end-effectors while a high-quality inertial measurement unit (IMU) is located in the chest of the robot. Cameras are also placed on the head of the robot. Other designs are possible and other sensors can be placed on the robot. At the end, these sensors will be providing some information that will be used in order to achieve specific tasks such as keeping the robot correctly balanced, reaching a point in space or localising the robot in its environment.

Regarding the localisation problem, each sensor has its own qualities and drawbacks. Localisation techniques such as Global Positioning System (GPS) have been investigated mostly for mobile robot navigation [Ohno 2003[START_REF] Ohno | [END_REF][START_REF] Reina | Adaptive kalman filtering for gps-based mobile robot localization[END_REF]]. GPS provides absolute measurements but is not well suited for indoor localisation. A popular alternative to GPS is the use of odometry which integrates information provided by a given sensor. Some examples are:

• Encoder odometry integrating all small displacements provided by robot encoders to describe the robot's trajectory. For example, in the case of a wheeled robot, the odometry uses the rotations of the wheels to estimate the trajectory of the robot.

• Visual odometry through the use of cameras extracting information from the Roboart II, built by H.R. Everett [START_REF] Everett | Sensors for mobile robots[END_REF]], (d) The Savannah River Site nuclear surveillance robot images (e.g. in [Comport 2010]). Visual odometry methods are still used not only in mobile robotics but also for unmanned aerial navigation, pedestrian navigation and others. For example, this method can be used for visual servoing [START_REF] Tsakiris | [END_REF][START_REF] Chaumette | [END_REF]].

If odometry is a theoretically possible solution, the perfect sensor assumption makes its use unrealistic and results in estimations drifting from the real state. Imperfections and errors due to sensors can also be produced by the data processing step.

Estimators tend to tackle these problems by defining sensor models working under more realistic assumptions used to simplify the problem. Besides, sensor fusion is used to get complementary information enhancing the state estimates. We can distinguish two main categories of estimators: filters and batch optimisation estimators. On the one hand, filtering methods compute the state of the robot using the data on top of the last estimate of the robot. During this estimation, all past poses are marginalised out and the information gained over time is summarised as a probability distribution. The idea of such filters is based on the principle of the Bayes Filter that will be introduced in Section 1.2. Kalman Filters are the most popular solution of this category. The popularity of Kalman Filter based solutions can be explained by both their performances and their simple implementation. On the other hand, methods belonging to the batch optimisation category tend not to marginalise out past poses but rather to keep this information in the system. The estimation does not only concern the last poses but rather all selected poses represented by the so called: "key-frames" along the trajectory. The most naive way to work with these methods is to estimate the states of all the key-frames. The more key-frames are estimated, the more computationally expensive and time consuming the estimation will be. Key-frames not involved in the estimation are rather ignored than marginalised out, implying a different sparsification of the problem when compared to filter based methods.

Thesis overview Rationale

Sensory input and data management form a core problem in robotics since they are at the root of all solutions. The common solution for the design of legged robots is very similar to the one presented in Fig. 5. The encoders are then used to move the robot while the high-quality IMU located in the chest can be used to estimate the pose of the chest and cameras are used to estimate the pose of the robot in its environment. Although we can now find impressive demonstrations of legged robots running, jumping and performing various tasks, we can hardly claim to achieve locomotion tasks or to design perception systems that are as effective as for living beings. Thus let us take again the slippage example while recalling that this phenomenon is mostly unexpected and undesired in robotics. Some strategies need to be used to tackle the slippage problem since it introduces problems in terms of balance, control but also localisation. In 2010, Boston Dynamics released a video showing the Bigdog robot walking in different outdoor scenarii including a frozen floor. However, to tackle efficiently the locomotion problem on slipping floors is still a challenge. In [START_REF] Park | [END_REF]], the authors presented a framework that enables the Cheetah robot to autonomously run over obstacles up to 40 cm in height as illustrated in Fig. 6. We can also notice that the robot jumps again right after the landing phase before reaching the normal running phase. However, how would we manage slippage in this situation taking into account that motion planning methods usually make the no-slippage assumption? This question might be relevant in real case scnearii. Therefore we can wonder whether a different design could help to achieve better performances. IMUs (introduced in Chapter 2) can provide a localised perception that we could benefit of at high rates. Thereby the question that we can raise is whether placing multiple IMUs on the robot and specifically on the end-effectors could be interesting. This localised perception would provide information that could be used along with the other sensors in the robot through sensor fusion methods to have a better estimate of the state of the system. More specifically, this perception could bring some information to be used along with more generalised perception method such as SLAM (Simultaneous Localization And Mapping) methods that will be introduced in Section 1.1. This thesis is an attempt to build the first block that may be used to answer this question.

In order to get an interesting solution, we may need a perception system that can acquire data at a high frequency so that the realtime estimates can be used in a control loop. Since placing multiple IMUs providing high quality measurements may become very costly, we investigate the use of low-cost IMUs. Although we mentioned the performance related issues, evaluating them is application-dependant. However, we tend to require solutions that can run in real-time. Hence, in this thesis, we focus on the use of a single IMU with the following constraints:

• use a low-cost IMU

• data are provided at 1 kHz • the solution should be usable for real-time applications Despite the apparent weaknesses of batch optimisation based methods regarding real-time applications, we tend to think that these solutions are suited to achieve our goals when working in a coherent framework. If this can be achieved, then we would have an optimal estimator with all the advantages when compared to filtering methods but with some disadvantages regarding the time-consuming estimation part. Efforts on formulating the problem and on an efficient use of the information provided by sensors are required to reach our goals.

A more detailed presentation of both filtering and batch optimisation methods is provided in the upcoming chapters. Our contribution is to apply the batch optimisation approach to problems mostly tackled with filtering methods while proposing a method that will allow to efficiently integrate new sensors as well as the information they provide. Hence our approach will be used for pedestrian localisation problems and extended to its use with a humanoid robot (Fig. 3b).

Thesis organisation

Overview

The three contributions presented in this thesis are composed of:

• The development of the quaternion-based preintegration methods and its application to pedestrian tracking along with a factor graph formulation of the problem. This contribution is developed in Chapter 3 and is published as a conference paper [START_REF] Atchuthan | [END_REF]].

• The investigations of similar techniques for use on a humanoid robot.

• A self-calibration procedure extending lessons learned from the preintegration method to its use with other manifolds detailed in Chapter 4 and published with extended materials in [START_REF] Solà | [END_REF]].

Historical narrative

Since we focus primarily on the use of IMUs for pose estimation while formulating the problem with a factor graph, we first need an effective way to use the IMU. This is the purpose of the preintegration method proposed in [Lupton 2009] and applied to IMUs using direct preintegration with Euler angles. The preintegration method was improved in [START_REF] Forster | [END_REF]] with preintegration on the SO(3) manifold. The novelty and interest of the work can be recognized through the different contributions using this preintegration method can it be for navigation [START_REF] Caruso | [END_REF][START_REF] Liu | [END_REF][START_REF] Hartley | [END_REF] or calibration techniques [START_REF] Kim | [END_REF]]. This preintegration method is explained using rotation matrices and cumbersome derivatives regarding the jacobians that could possibly lead to misunderstandings. Thus we first propose a quaternion-based preintegration method due to specific interests regarding the quaternion representation to represent orientations and provide simpler and clear derivatives using the chain rule. We believe that this new and elegant formulation makes the understanding of the preintegration method easier, thus resulting in fewer errors and the possibility to imagine other applications with similar methods. Secondly, we generalise the preintegration method to its use with any kind of manifolds. This work is a step further towards splitting things and acting more rationnaly with the chain rule while giving a clear definition of Jacobians in manifolds as opposed to the algebraic way presented in [START_REF] Forster | [END_REF]].

We illustrate the use of this generalised preintegration method through its use for sensor self-calibration.

Chapter organisation and contributions

This thesis is organised as follows. Chapter 1 introduces the optimal estimation theory by underlining the similarities and differences between the main popular filtering methods and the batch optimisation approach. It is both a tutorial and a state-of-the-art chapter. It relies on [Sola 2016], [Sola 2007] and [Koller 2009] as main sources, yet the content of the chapter is not fully covered by these sources.

Chapter 2 then introduces the IMU (acronym for Inertial Measurement Unit) technology due to its importance in this thesis. Chapter 3 details the IMU preintegration method derived from a proposal of [START_REF] Forster | [END_REF]] and also introduces some implementation on a humanoid robot. The development of the method was conducted in collaboration with Joan Solà and I am the author of its software implementation. This chapter partially relies on a submitted conference paper of which I am the main author.

Chapter 4 extends some solutions driven by remarks upon the preintegration method proposed in Chapter 3 to the usage of any kind of manifold. Joan Solà is the leader of this work in which I contributed in a software implementation and experimentation point of view. This work is published with extended materials in [START_REF] Solà | [END_REF]].

Finally, Chapter 5 introduces the long-term project this work is part of in the GEPETTO team as well as the localisation library (WOLF) in on-going development to which I contributed equally with Joan Solà (lead developer), Joan Vallvé, Angel Santamaria-Navarro, and Jeremy Deray, as well as other secondary developers.

Chapter 1

Estimation as filtering or online batch-optimisation problems

Definition of the problem

Let us suppose that you are walking in a haunted house attraction. Everything goes fine until you enter a new room at t 0 = 0. You can see a door you want to reach on the opposite wall, it is automatically included in your mind map as a point of reference to localise yourself in the room along with other objects you may have noticed. Suddenly, the lights are turned off and you lose all visual information. Because you had a map of the room at t 0 (before the lights are turned off) and did not move, you can assume to know where the door is and you can navigate towards it in the dark. This may seem like a very easy experiment! However, experiments show that walking straight forward to an objective with no visual information is a When one generates a sequence of control designed to make the robot move from a place to another, the estimation of the state of the robot given only the initial position and the control sequence is called odometry.

Going back to our haunted house example, if the lights were still turned on then reaching the door would have been easy for us. This can be explained by the correction of the trajectory given our visual perception of the world. In robotics, this problem is called Simultaneous Localisation And Mapping (SLAM).

This thesis is written in the context of estimation of the state of a mobile robot in an unknown environment. While our contributions mostly address the problem of localising the robot, and not that much of reconstructing the environment, we believe that our work naturally extends to the full localisation and mapping problem. It is important to understand the extended context before digging in our personal contributions.

Motivation and main concepts

The broader view is given by considering the problem of reconstructing the structure of the environment from camera motion. Structure-from-Motion (SfM) problems are dealing with the reconstruction of the three-dimensional (3D) structure of a (stationary) scene from a set of measurements extracted in two-dimensional (2D) images. Those measurements are specific features (point of interests, corners, lines, ...) detected in the images and projected onto the camera frame. The dataassociation step extracts matching features from one image (or frame) to another. These associations are used to estimate the relative camera poses and thus camera motion between related frames. The minimisation of the re-projection error then leads to the reconstruction of the desired 3D structure. SfM is still a research field into which much effort is put. [Longuet-Higgins 1981] describes a simple algorithm to recover the 3D structure of a scene assuming that the association problem is solved, thus describing the problem as finding the relative orientations of the different view points.

SfM can be considered as a monocular visual Simultaneous Localisation And Mapping (SLAM) problem. Yet there is a slight difference between SfM and SLAM problems. While SfM problems are, generally speaking, about the reconstruction of the environment, SLAM is centred around the problem of localisation in the environment. In SLAM, the features extracted from images are stored to create a map of the environment. The map is not only used to infer the current position of the camera but it is also re-estimated due to new information added in the map. After all, the features are extracted from imperfect sensors and uncertainties about the relative positions of the features cannot be neglected. One way to express these uncertainties is to use a probabilistic formulation as it is often done for SLAM.

Since we are mostly concerned about the localisation part of the problem, the probabilistic formulation will be introduced to the reader using a SLAM example hereafter. Parallels to localisation and calibration problems treated in the next chapters are straightforward. State estimators are used in mobile robotics to estimate both intrinsic and extrinsic states. Their importance led to many efforts in the study of their limits and to improve them. We can distinguish two main categories of estimators: filters and batch optimisation methods. A nice review is proposed in [START_REF] Strasdat | [END_REF]] and compares both categories applied to SfM problems. We discuss filtering and optimisation next.

Let's go back to our initial (haunted house) example and note x T t = x t y t θ t the vector containing your pose (2D position and orientation) in the room at time t. Since the visual part and the mapping of the environment have not been used for this thesis, the formulation will take the example of a blind navigation for a better coherence with the rest of the thesis. However, the similarities make the transposition to a SLAM problem straightforward. Thus, the theory presented hereafter is inspired by SLAM courses [START_REF] Stachniss | Robot Mapping[END_REF], Sola 2016].

Sequences of states and measurements

The pose of a robot is denoted by x t where t denotes the time step we are considering and expressing both position and orientation relative to a reference frame. Under the flat ground assumption, the pose is a 3-components vector x t = x t y t θ t T .

If we consider the exploration of a 3D environment, we get to a minimal representation of the pose containing six components. The position is described by p t = x t y t z t T while the orientation is also represented by a 3-components vector in the minimal form. One possible choice is the Euler angles o t = φ t θ t ψ t T . problems

In this work, rotations will be represented by four dimension quaternion vectors introduced with more details in Appendix B. In both 2D and 3D cases, we still can consider the pose vector as a concatenation of both position and orientation vectors

(x t = p T t o T t T ).
The whole trajectory of the robot from time step t 0 = 0 to time step T is the complete set of poses described by

x 0:T = {x 0 , x 1 , ..., x T } (1.1)
While navigating in its environment, the robot may compute its odometry using embedded sensors, thus measuring the motion between time steps. This measure can be considered as an estimate of the trajectory and is usually noted u t and interpreted as the control sequence from time step t -1 up to time step t. Thus the complete sequence of measurements from t 0 to T is given by

u 1:T = {u 1 , u 2 , ..., u T } (1.2)
Under the assumption that the odometer is perfect, the relation between both poses and odometry sequences would be straightforward. Indeed, integrating the successive odometry measurements would describe the overall trajectory of the robot.

x 1 = x 0 ⊕ x 0→1 = x 0 ⊕ u 1 (1.3) x n = x 0 ⊕ n i=1 x i-1→i = x 0 ⊕ N i=1 u i (1.4)

Some noise in the sensors

However, as explained in the Introduction chapter, measurements are imperfect not only due to the sensor but it may also not take into account some specific phenomena that can occur during navigation such as slipping. Fig. 1.1 illustrates the growth of uncertainty using odometry when a mobile robot has to navigate. The difference between both red and blue trajectories can be explained not only by imperfections in sensors but also by unexpected events due to the environment as for example the wheels slipping on the floor. When considering real applications, imperfections in the sensor need to be taken into account. A popular assumption in robotics [START_REF] Thrun | [END_REF]] when designing the sensor model is to consider that it is affected by white noise and thus it can be described by a Gaussian model with zero mean. Although parametric and non-parametric inference techniques were investigated for SLAM applications [START_REF] Sim | [END_REF][START_REF] Eade | [END_REF]], the Gaussian distribution assumption has not only proven to be the most effective but also simplifies a lot the theory behind state estimation while it seems to represent uncertainties in an acceptable way. Thanks to the Gaussian distribution assumption, the probability density of a random variable x follows (A.8).

t t+1 t+2

u t u t+1

x t→t+1 x t+1→t+2

Figure 1.2: Representation of the growth of uncertainties in odometry due to sensor imperfections and the environment itself. The red trajectory is the one desired and thus the one the control sequence should lead to follow. However, due to errors and unexpected events, the real trajectory is described by the blue path. In orange ellipses here are represented the 2D covariances of the estimated states. Without any further information to better estimate the pose of the robot, this covariance only keeps growing with time.

Odometry sequences are not the only information available to most modern robots to estimate their position. We can cite for example the use of tactile sensors giving specific features and information about the environment [Kawasaki 1999[START_REF] Kerpa | [END_REF]], or more generally RFID sensors that may be reading information [START_REF] Agarwal | [END_REF], Codas 2010]. That information could be anything like for example the current position of the robot or the position of the marker itself. Let us denote z t the sensor measurements giving additional information related to the pose of the robot at time step t. We now consider two sources of measurements: u are the odometry measurements coming from the motion model that we know as a prior, z are the sensors in general (actually, u and z can be considered alike; however we find it convenient to distinguish them from the start). Given this information the state estimation problem is to estimate the posterior state given available information.

p(x t |u 1:T , z 1:T )

(1.5)

Refactoring using Markov and Bayes

The application of Bayes' Rule (A.9) then leads to

p(x t |u 1:t , z 1:t ) = ηp(z t |x t , u 1:t , z 1:t-1 )p(x t |u 1:t , z 1:t-1 ) (1.6)
An additional assumption made at this stage of the theory is the Markov assumption according to which the state x t contains all the relevant information up to time t. In other words, given the present state of the system all the future states are independent of the past states. This assumption is the basis of the recursive estimation theory. The problem is initialised. New landmarks are detected and added in the map with their respective covariance due to sensor specifications. 2: Computation of the predicted states for the state composed of both robot's and landmarks' pose. These states are computed using the control sequence between 1 and 2 only thus resulting in an increase of pose uncertainties for the estimated states. 3: Landmarks are detected and a data association method is used to assign the landmarks to those already present in the map. Assuming that the data association is correct for 100 % of the landmarks, the new observation is used to compute the most probable state for all the states. Newly detected landmarks are added to the map. 4: The pose correction is associated to a decrease of uncertainties represented by the ellipses boundaries. also be written as

p(z t |x t , u 1:t , z 1:t-1 ) Markov = p(z t |x t ) (1.
p(x t |u 1:t , z 1:t-1 ) Markov = p(x t |x t-1 , u t )p(x t-1 |u 1:t , z 1:t-1 ) (1.8)
This corresponds to the prediction at step t -1, or motion model. With (1.7) and (1.8) we can rewrite (1.6) as

p(x t |u 1:t , z 1:t ) = ηp(z t |x t )p(x t |x t-1 , u t )p(x t-1 |u 1:t , z 1:t-1 ) (1.9)
Here, u t can be omitted in p(x t-1 |u 1:t , z 1:t-1 ) since it does not take part in estimating the state x t-1 . Thus this equation becomes

p(x t |u 1:t , z 1:t ) = ηp(z t |x t )p(x t |x t-1 , u t )p(x t-1 |u 1:t-1 , z 1:t-1 ) (1.10)
One can note how the Markov assumption led to a recursive formulation for this estimation problem. Indeed, the last part of the right term of (1.9) is exactly the posterior probability expressed at one step in the past. Its expression can be written like (1.9) as

p(x t-1 |u 1:t-1 , z 1:t-1 ) = ηp(z t-1 |x t-1 )p(x t-1 |x t-2 , u t-1 )p(x t-2 |u 1:t-1 , z 1:t-2 ) (1.11)
The recursive formulation in (1.9) can thus be expressed as

p(x t |u 1:t , z 1:t ) = ηp(z t |x t )p(x t |x t-1 , u t )p(x t-1 |u 1:t , z 1:t-1 ) (1.12)
where η is a normalisation factor used to represent the denominator resulting in the application of Bayes' rule. Finally, by considering p(x 0 ) as a prior knowledge on the initial position of the robot and following the same method for all the past posterior probabilities, one can find the generalised recursive formulation for (1.9) which is

p(x t |u 1:t , z 1:t ) = ηp(x 0 ) t i=1 p(z i |x i )p(x i |x i-1 , u i ) (1.13)
To cite [START_REF] Thrun | [END_REF]], "state estimation addresses the problem of estimating quantities from sensor data that are not directly observable, but that can be inferred". Solving the localisation and by extension the SLAM problems leads to finding the most likely (or probable) values that all the estimated states (robot states and features locations for map estimation in SLAM) would take given information provided by the sensors. This is the crucial and complex job of the estimator.

And the map?

The problem described above is a subset of the general SLAM problem formulation. Indeed, SLAM additionally includes the mapping part processed with fea-

Chapter 1. Estimation as filtering or online batch-optimisation problems

tures extracted from exteroceptive sensors. The exact location of these features remains unknown and thus their positions in the environment have to be estimated (see Fig. 1.3). The map must be corrected with new estimations of features' locations. Thus, if we denote by m the map, it enters in the posterior formula as p(x t , m|u 1:T , z 1:T ) (1.14)

Should we filter or optimise?

Estimators are main components of any robotic systems. Their place for the system is just as important as the brain is to any living creature. Estimators are used not only to estimate internal and/or external states but also to realise the multi-sensor fusion. Their importance pushed the research community to put a lot of efforts in designing new estimators, study existing ones, correction some specific parts and trying to cancel limitations to get an estimator that would get better. However, at some point, two communities were formed based on the preferred philosophy that is using filtering methods or sparse optimisation. Both methodologies have been used extensively by SLAM and SfM communities. [START_REF] Azarbayejani | [END_REF]] already uses an Extended Kalman Filter (EKF), that is a recursive estimation method, to not only solve the SfM problem, that is recovering the motion and the structure of the environment, but also to correctly estimate the focal length of the camera from feature correspondences tracked through the image sequence. Although they use EKF, they also put emphasis on the fact that an equivalent solution can be found using other estimators. Closely related to this example are the work of [START_REF] Oliensis | [END_REF], Soatto 1993] that are using EKF as a smoothing filter. In the search of better performances, some work investigated the use of optimisation methods based on the minimisation of a nonlinear function [START_REF] Kumar | [END_REF][START_REF] Spetsakis | A multi-frame approach to visual motion perception[END_REF][START_REF] Weng | [END_REF]. Making a choice between recursive estimation and sparse optimisation methods in the SfM community is still a problem nowadays. [START_REF] Chiuso | [END_REF]] also uses recursive estimation method to improve SfM results due to the sub-minimal model presented in [START_REF] Azarbayejani | [END_REF]. While looking for a real-time application of the SfM problem, they chose the filtering methods because of the need to process data on-line. [START_REF] Triggs | [END_REF]] is giving an overview of different global bundle adjustment algorithms which objective is to solve the "problem of refining a visual reconstruction to produce jointly optimal structure and viewing parameter estimates". The subject is definitely complex due to all the parameters that have to be taken into account during the implementation of such algorithms with the hope to make the estimation as robust as needed and the computation as effective as possible. If we had to summarise in a single sentence, we would say that bundle adjustment aims at estimating the parameters of a large sparse problem by minimising cost functions. Further details about the optimisation-based methods are given in section 1.3. Bundle adjustment methods are often said to be extremely costly regarding the computation time. The reason may be not only because taking advantage of the sparseness of the problem is not straightforward, but also because these methods require the inversion of linear systems whose sizes grow with the number of estimated parameters. Although there exist examples of SfM using bundle adjustment with nonlinear least square minimisation [Szeliski 1994, Shum 1999], using global bundle adjustment still remained a challenge due to the time-consuming estimation. Some authors approached a more efficient way to use bundle adjustment. [START_REF] Steedly | [END_REF]] used an incremental reconstruction method, readjusting only a subset of parameters, or [Mouragnon 2009] using local bundle adjustment each time a new camera pose is added for real-time SfM.

This choice is also present in the SLAM community. [Davison 2003] uses the recursive filtering method and achieve real-time performances with specific strategies concerning the mapping part to take advantage of the sparseness of the problem and a top-down Bayesian estimation approach. More recently, [START_REF] Roussillon | [END_REF]] proposed a generic and practical solution using the EKF for real-time visual SLAM on robots. Other approaches attempting to propose a SLAM method that would also be effective in large-scale navigation have been proposed [START_REF] Eade | [END_REF][START_REF] Montemerlo | [END_REF].

Some authors have also focused on using sparse optimisation methods in SLAM to achieve better accuracy than recursive estimation based ones. Efforts have also be made to reduce the computational cost of bundle adjustment estimation by reducing the complexity of the problem. Indeed [START_REF] Konolige | [END_REF]] introduced the reduction of the parameter space in FrameSLAM through a marginalisation procedure. Other methods are using local maps for estimation purposes [START_REF] Ni | Out-of-core bundle adjustment for large-scale 3d reconstruction[END_REF][START_REF] Ni | Multi-level submap based slam using nested dissection[END_REF].

Finally, making a choice between bundle adjustment based methods or recursive filtering estimation is still today a question of trade-offs. On the one hand, recursive filter based methods are known to be faster to implement and allow realtime applications. But their main drawback as regards sparse optimisation based solutions seems to be the accuracy of the estimation. On the other hand, these last ones have the reputation to be complex to implement and to require higher computation time despite recent advances allowing to considerably reduce this last parameter. Trade-offs are also application dependent. For instance, both SfM and SLAM are subject specific trade-offs. For SfM, the problem may be summarised as to find a balance between an easier relative orientation estimation or an easier correspondence problem between features in the images. With disparate viewpoints between the images, finding the relative orientation is made easy but the features correspondence problem will be more complex. On the other hand, if the viewpoints are close enough then the correspondence problem will be easy but finding the relative orientation will be harder due to approximations and accuracy. [START_REF] Strasdat | [END_REF][START_REF] Strasdat | [END_REF]] first compare filtering and sparse optimisation methods applied to SLAM problems and underline the differences of these two approaches. The representation of both methods as given in Fig. 1.4 is a good start to understand the differences and what it implies to prefer one solution over the other one.

While some filtering methods and sparse optimisation will be presented with more details in the following sections, let us keep in mind some major points:

1. The sparseness of the problem is different in both cases. This detail has consequences on the computation of the joint probabilities and their propagation.

2. Filtering methods use the Markov assumption to marginalise out past poses and to summarise all the information gained over time with a probability distribution whereas sparse optimisation methods are using key-frames to select specific states to be estimated. Non-estimated key-frames are rather ignored than marginalised out, thus keeping the problem sparse and the optimisation relatively efficient.

3. Both methods are using approximations.

4. There exist a wide variety of methods mixing both philosophies. We could cite for example the multi-state Kalman Filter which is an attempt to overcome some limitations of filtering methods with inspiration from sparse optimisation at the cost of more time-consuming estimations. More examples are given in subsection 1.3.4.

In the next two sections, we reformulate the estimation problem as either a filtering or an optimisation problem, by either marginalising over the past states (filtering) or extracting from the Bayesian network the underlying factor graph (optimisation). This will motivate our choice of using an optimisation approach in the rest of the thesis, but also explains how our contributions can in fact be quite directly transferred into a filtering method.

Filtering Approach

Bayes' Filter Algorithm

The Bayes' filter is the most general algorithm to compute posterior probabilities and gives a good overview of most recursive filters. The pseudo-algorithm of Bayes' filter is presented in Algorithm 1. It highlights the two steps that are at the core of the recursive estimation methods : the prediction and correction steps.

The prediction step

The prediction step aims at computing a prior density before using the measurement at time step t and right after using the control measurement u t . This step predicts the state x t on the basis of z 1:t-1 the measurement information up to time t -1 and the latest control measurement u t . p(x t |u 1:t , z 1:t-1 ) = p(x t |x t-1 , u t ) state transition p(x t-1 |u 1:t-1 , z 1:t-1 ) posterior probability at step t-1 dx t-1

(1.15)

The state transition is given by the dynamics of the system. We can also easily give an expression of the posterior probability at step t -1 using (1.8). A way to interpret this prediction step is to say that it computes the most probable evolution for the state of the system given the previously computed posterior probability and the dynamic model of the system.

The correction step

Once the prediction is made, it is time for the system to use the last measurement z t and correct the prediction with the newly provided information. This correction step, also called measurement update, is basically a product of the probability density functions describing the posterior probability computed in the prediction step (1.15) and the one associated with the measurement z t , p(z t , x t ). The computation of the new posterior probability is exactly given by (1.9). We give here the complete form of this equation. Now that the Bayes' Filter is introduced, the next section will present the most popular estimators designed on the principles described above. All of them are built on top of the theory of Bayes' Filters. Variations can be introduced concerning the probabilities that have been described sooner. The reason why assumptions are made concerning the probabilities involved in Bayes' Filter is due to the impossibility to compute exacts beliefs for general problems. Thus assumptions are a way to approximate the posterior probabilities. However, most of the filters are using the Gaussian distribution to design the dynamics and the measurement models, meaning that all probability density functions can be described by (A.8). Thus, the following filters are also known as Gaussian Filters.

p(x

Kalman Filter

The Kalman Filter (KF) [Kalman 1960] is an implementation of Bayes' Filter that can be used to compute the posterior probabilities of continuous states only. In addition to both Markov and Gaussian distribution assumptions, this implementation assumes the dynamic and measurement models of the system to be linear. Mathematical translation of this new assumption gives

x t = f (x t-1 , u t ) = A t x t-1 + B t u t + ε xt (1.17)
where ε xt is the process noise expressing the uncertainty in the state transition through perturbations in the system. This process noise is defined by a Gaussian distribution ( (A.8)). Similarly, the measurement model is given by the function

z t = h(x t ) = C t x t + ε zt (1.18)
where ε zt is the Gaussian measurement noise expressing the noise related to the sensor measurement (due to sensor specifications).

p(ε xt ) ∼ N (ε xt , Q t ) (1.19) p(ε zt ) ∼ N (ε zt , R t ) (1.20) p(x 0 ) = N (x 0 , Σ 0 ) (1.21)
The linear evolution constraint leads to finite dimensions for matrices A t , B t and C t . If we note n, m and p the dimensions of respectively x t , u t and z t , then one can find that those three matrices are respectively of dimension n × n, n × m and p × n. We can also give an explanation of the meaning of the different matrices in this model. A t describes how the state evolves from t -1 to t without controls or noises while B t describes the changes in the control from t -1 to t. Finally, C t can be understood as the description that allows one to map the state x t to an observation z t If we think about it, (1.17) relates to the state transition in (1.15) while (1.18) is related to p(z t |x t ) and we can express both densities as

p(x t |u 1:t-1 , z 1:t-1 ) = 1 det(Q t ) 1 2 (2π) n 2 exp - 1 2 (x t -xt ) T Q -1 t (x t -xt ) (1.22) p(z t , x t ) = 1 det(R t ) 1 2 (2π) n 2 exp - 1 2 (z t -zt ) T R -1 t (z t -zt ) (1.23)
where

xt ∆ = E[x t ] = A t x t-1 + B t u t zt ∆ = E[z t ] = C t x t
the expression of the prior knowledge on the position of the robot is all that remains to be defined to be able to express (1.13). This prior is also described by a Gaussian distribution (p(x 0 ) ∼ N (µ 0 , Σ 0 )) due to the properties of Gaussian densities. Given all these details, we can say that the KF algorithm presented in Alg. 2 aims at computing the mean and the covariance of the states in a closed-loop of predictioncorrection steps every time a new measurement and/or control sequence arrives. The interested reader is given a more detailed view of both prediction and correction steps in the following.

KF prediction step

With the Gaussian distribution assumption for probability densities, the prediction step (1.15) yields to

p(x t |u 1:t , z 1:t-1 ) = η exp -L t dx t-1 (1.24)
Chapter 1. Estimation as filtering or online batch-optimisation problems with

L t = 1 2 ((x t -xt ) T Q -1 t (x t -xt ) + (x t-1 -xt-1 ) T Σ -1 t-1 (x t-1 -xt-1 )) (1.25)
Going back to (1.17), one can show from (1.15) that

xt|t-1 ∆ = E[x t |u 1:t , z 1:t-1 ] = xt (1.26) Σ t|t-1 ∆ = E[(x t -xt|t-1 )(x t -xt|t-1 ) T ] = A t Σ t-1 A T t + B t Q t B T t (1.27)
Although the details are not presented here, the interested reader can find the mathematical derivations leading to equations above in [START_REF] Thrun | [END_REF]]. We will note here that the posterior probability at step t -1 is a prior probability used to predict the future mean and covariances of the state x t . This prior is described by a Gaussian distribution (N (x t-1 , Σ t-1 )) and used in the state transition part. Furthermore, the state transition in itself is affected by a Gaussian noise of covariance R t due to the imperfections in the control sequence so that p(u t ) ∼ N (u t -0; Q). Another expression for R t is B t Q t B T t under the Gaussian distribution assumption.

KF correction step

Now let's consider the correction step which probabilistic expression is given by (1.16) or alternatively by (1.9). Again, similarly to (1.24), the posterior probability p(x t |u 1:t , z 1:t ) can be expressed with an exponential function as p(x t |u 1:t , z 1:t ) = η exp -J t (1.28)

where

J t = 1 2 (z t -C t x t ) T Q -1 t (z t -C t x t ) + (x t -μt ) T Σ-1 t (x t -μt ) (1.29)
One can show that the covariance characterising the Gaussian distribution of the posterior probability in (1.28) is given by

Σ t = C t Q -1 t C T t + Σ-1 t (1.30)
The correction step aims at incorporating measurement information in the system and correct the prediction. Therefore we need to be able to express how much new information is added to the system for the correction step to be complete and compute both mean and covariance of the posterior probability. This is done by using the Kalman Gain at time step t noted K t . This Kalman Gain can be found by minimising the first derivative of J t and by noting that the value of x t will actually be the mean µ t of p(x t ). Thus we can substitute both terms. This method leads 1.2. Filtering Approach 23 Algorithm 2: Kalman Filter(µ t-1 , u t , z t , Σ t-1 )

1: loop 2: {Prediction(µ t-1 , u t , Σ t-1 ):} 3: μt = A t µ t-1 + B t u t 4: Σt = A t Σ t-1 A T t + R t 5:
{Correction(μ t , z t , Σt ):} 6:

K t ∆ = Σt C T t (C t Σt C T t + Q t ) -1
7:

µ t = μt + K t (z t -C t μt ) 8: Σ t = Σt (I -K t C t ) 9:
return µ t , Σ t 10: end loop to the following expression of the corrected mean µ t µ t = μt predicted mean

+ K t (z t -C t μt ) correction (1.31) with K t = Σ t C T t Q -1 t (1.32)
However, expressing K t as a function of Σ t is not convenient. A transformation leads us to another definition that makes the use of the gain possible in practice

K t ∆ = Σt C T t (C t Σt C T t + Q t ) -1 (1.33)
from (1.31) we can easily deduce a new expression of Σ t expressed as a Σt correction operation

Σ t = Σt (I -K t C t ) (1.34)
From (1.31) and (1.34), an intuitive way to understand the role of the Kalman Gain is then to see it as a translation of how much the new measurement will be integrated in the system.

Extended Kalman Filter

The Extended Kalman Filter (EKF) aims at correcting the major drawback of KF that is the assumption of linear dynamic and observation models. Such a hard requirement makes the filter difficult to use in real systems that are subject to noise and nonlinearities. This is done by modelling both observation and dynamic of the Chapter 1. Estimation as filtering or online batch-optimisation problems system with nonlinear functions so that

x t = f (x t-1 , u t ) + ε xt (1.35) z t = h(x t ) + ε zt (1.36) p(ε xt ) ∼ N (ε xt -εxt , Q t ) (1.37) p(ε zt ) ∼ N (ε zt -εzt , R t ) (1.38) p(x 0 ) = N (x 0 -x0 , Σ 0 ) (1.39)
However, due to the introduction of nonlinear functions, the probability densities at stake here cannot be ensured to be Gaussian. Despite this, the EKF algorithm still considers the distributions to be Gaussian by approximations and most of the prediction-correction loop of the KF algorithm is applied.

In order to apply prediction-correction scheme of Bayes' Filter, the Gaussian distribution approximation is realised through a linearisation step on the most recent estimate we have. The best estimate is also the value the variable is the most likely to take, that is the mean of the distribution. Thus, f (.) and g(.) nonlinear functions are approximated locally by a linear function with assumed Gaussian posterior. The linearisation is realised in practice thanks to a first-order Taylor expansion defined as

f (x) = f (x 0 ) + F x (x -x 0 ) (1.40)
where F x is the Jacobian matrix defined for f (.) by

F x (x -x 0 ) ∆ = ∂f ∂x T x 0
(1.41)

EKF prediction step

The state prediction state begins with the linearisation of the state transition function f (.). This linearisation is realised around the most recent best estimate µ t-1 so that

f (x t-1 , u t ) Taylor = f (µ t-1 , u t ) + F x (x t-1 -µ t-1 ) (1.42) Σ t|t-1 = Σt = F x Σ t-1 F x T + Q t (1.43)
As underlined sooner, Q is a Gaussian approximation of the real distribution Q by means of the Taylor expansion.

EKF correction step

The correction step also implies the use of the linearisation procedure of the mea-

Σ t = H x Σt H x T + R (1.45) K t = Σ t H x T Q -1 t (1.46) µ t = μt predicted mean + K t (z t -h(x t )) correction (1.47)
The EKF algorithm is presented hereafter in Alg. 3. As for K t in the KF algorithm (1.33), it is possible by transformation to deduce a new expression of the EKF Kalman Gain

K t ∆ = Σt H T t (H t Σt H T t + Q t ) -1 (1.48)
where

H t ∆ = H x (x t -μt ) (1.49) Algorithm 3: Extended Kalman Filter(µ t-1 , u t , z t , Σ t-1 ) 1: loop 2: {Prediction(µ t-1 , u t , Σ t-1 ):} 3: μt = f (μ t ) + F x (μ t ) 4: Σt = F xt Σ t-1 F x T t + R t 5:
{Correction(μ t , z t , Σt ):} 6:

K t ∆ = Σt H T t (H t Σt H T t + Q t ) -1
7:

µ t = μt + K t (z t -h(μ t ))
8:

Σ t = Σt (I -K t H t ) 9:
return µ t , Σ t 10: end loop Despite the introduction of nonlinear functions in the system's model of the EKF, the linearisation process introduces growing errors through approximations as illustrated in Fig. 1.5. This undesired behaviour led to the development of alternative filters based on the EKF formulation e.g. the Robust Extended Kalman Filter [START_REF] Einicke | [END_REF][START_REF] Kwon | [END_REF], the Unscented Kalman Filter [START_REF][END_REF], Brossard 2017] and the Particle Filter described hereafter.

The particle filter

The Particle Filter [Doucet 2001[START_REF] Doucet | [END_REF]] is another method that is used to bypass the linearisation step of EKF. Besides, this method is designed to work with non-Gaussian distributions. Instead of linearisation, the Particle Filter uses a sequential Monte Carlo method that generates estimates of the state and innovations problems The system is far from linear in the region of linearisation. Linearisation with the Gaussian approximation is not suited for this case. EKF will perform the operation sketched in dashed line to estimate the output while the true density is represented by the solid line. As a result, the estimates are biased and the filter diverges.(Source: [Sola 2007]) instead of deriving the analytic equations as Kalman does. This estimation with a set of particles that are independent random samples described by Dirac functions

δ(x -a) ∆ = lim σ→0 N (x -a, σ 2 )
(1.50)

The system can be described through the following equations

x t = f (x t-1 , u t ) + ε xt (1.51) z t = h(x t ) + ε zt (1.52) p(ε xt ) = any distribution (1.53) p(ε zt ) ∼ N (ε zt -εzt , R t ) (1.54) p(x 0 ) = N i=1 ρ i δ(x 0 -x i 0 ) (1.55)
where the prior p(x 0 ) is described by a set of N particles.

Particle Filters prediction step

Let's consider (1.15), the aforementioned algorithms were predicting the next most likely state value given the assumption of Gaussian distribution for the posterior probabilities. This time, the prior posterior probability is a sum of N independent particles where each of them follows a randomly generated perturbation distribution so that

p(x t |z 0:t-1 ) = ρ i δ(x t -x i ) (1.56)
where ρ i is the weight of each particle. At the end of this prediction stage, we don't have only a single most likely value but a set of N particles. The prediction step consists of a sampling process (first step of any particle filter algorithm). The general particle filter algorithm is creating particles based on a proposal distribution (also called conditional density), which is a distribution that may not be the real probability distribution describing the random value we want to estimate but from which it is possible to sample efficiently (for example, the proposal distribution could be Gaussian). When it comes to localisation using Monte Carlo, this sampling process is done by applying the motion model (1.51). to each particle in the set (1.56) with a proposal distribution for p(ε xt ) so that we get a new set of N particles in which each sample has a state hypothesis, that is a possible prediction.

Particle Filters correction step

One may notice that the only assumption made until there is in the proposal distribution. The correction step aims at correcting this proposal using the measurement z t thanks to which it is possible to compute the importance weight ρ i of each particle. This importance weight takes into account the probability distribution we actually want to approximate and is defined by

ρ [i] t = target distribution proposal distribution ∝ p(x [i] t |u 1:t , z 1:t ) p(x [i] t |x [i] 1:t-1 , u 1:t ) ∝ p(z t |x [i] t ) (1.57) ρ [i] t = ρ(x [i] 0:t ) N j=1 ρ(x (j) 0:t ) (1.58)
where (1.58) gives the normalised version of the weights while (1.57) corrects each weight depending on how different the proposal and the real distributions are. Before running the prediction step for the next step, the particle filter requires to resample the N particles given the current set of samples with replacement. There exists different methods for resampling [START_REF] Douc | [END_REF]] but we will describe here the basic Sequential Monte Carlo (SMC) approach. This resampling step creates a new set that has as many particles as in the previous normalised one. The new particles are distributed according to the posterior probability p(z t |x t ) also meaning that the higher the weight of a particle the more likely it is to draw a particle in the corresponding area, and they all get a new weight 1 N . This operation allows to keep a fixed size of particles and to eliminate the less likely possibilities that explain the trajectory of the robot, thus keeping the computational cost under some limits.

Particle Filter algorithms have been used extensively in robotics. One can find interesting use based on the particle filter in SLAM ( [Grisetti 2007a[START_REF] Montemerlo | [END_REF], Dissanayake 2000]), or to track moving entities [Montemerlo 2002, Schulz 2001, Germa 2010]. Despite the major advantages of these methods, they also have limitations:

Chapter 1. Estimation as filtering or online batch-optimisation problems

Algorithm 4: Particle Filter(Π t-1 , u t , z t )

1: loop 2: {Prediction(Π t-1 , u t ):} 3: clean set: Πt = Π t = ∅ 4:
for j = 1 to N do do 5:

sample:

x [j] t given p(x t | x [j] t-1 , u t ) 6:
compute particle weight: w

[j] t = p(z t | x [j] t ) 7:
add particle to the set:

Πt = Πt + < x [j] t , w [j] t > 8:
end for 9:

{Correction(μ t , z t , Σt ):} 10:

for j = 1 to N do do 11:

correct weight with observation: ρ

[j] t ∝ p(z t |x [j]
t )

12:

draw particle j with probability ∝ ρ

[j] t 13:

end for 14:

Resample Π t with N new particles 15:

return Π t

16: end loop

• There is no assumption made on the distribution of the motion model.

• As a consequence of the previous point, there is no linearisation step.

• The performance depends on how many particles are used. The more particles are used better the accuracy will be.

The filtering methods presented in these sections highlight the recursive concept that is used to reduce the computational cost of these estimation techniques. Despite this can be a major advantage when considering real-time applications, it comes with drawbacks whose specificities will depend on the filter itself.

Online Batch Optimisation Based Approach

The optimal estimation approach relying on sparse optimisation techniques is based on a more general approach for data fusion and estimation. In particular, last decades saw the emergence of graph-based methods especially in the vision community since SLAM and SfM problems can be posed in terms of inference on a graph. Such methods require the use of two main blocs:

• the front-end is the problem building block taking care of all the data processing part. This part of the system is establishing the relationship between the sensor data. It is not only responsible for transforming data so that it can be used in an optimisation process but also takes care of the data association, sensor fusion and building the graph itself. This graph is then translated into a cost-function.

• the back-end part optimises the problem built by the front-end and extremises the cost-function. It is a complex task requiring a good mathematical understanding of various methods to choose the best suited for one application or another. State-of-the art methods often use iterative methods to solve the problem.

This division between front and back ends is indeed a nice property. It separates the expert knowledge about the estimation problem, written as a knowledge graph in the front end, from the agnostic solver, in the back end, which often requires less expert knowledge about the robotic problem, but strong skills in mathematics. Let's first see how this separation is implemented in the literature. We will then continue our tutorial by describing the methods behind both ends.

Back and front-ends in the literature

Every existing SLAM solution comes with its own front-end motivated by choices of features detection, outliers rejection, data association methods, etc. In [Endres 2012] the authors proposed their own front-end due to the novelty of their feature localisation and localisation approach. [START_REF] Goncalves | [END_REF]] introduced a system to select unique and recognisable landmarks for pose estimation. [Olson 2008] designed a specific front-end in which the outliers rejection is based on spectral clustering. [START_REF] Thrun | The graph SLAM algorithm with applications to large-scale mapping of urban structures[END_REF]] relies on factorisation to reduce the dimensionality of the problem before solving. Relying on the information provided by the front-end, the accuracy and the efficiency of the back-end is also crucial. There exists a variety of back-end solutions implementing various strategies for a more efficient and accurate problem solving. Depending on the provided information, the back-end may fall into a local minimum or even diverge during optimisation. Lu and Milios in [Lu 1997] were the first to propose the optimisation-based approach for the SLAM problem. Since this work, new examples of back-ends have been proposed by the SLAM community. [Olson 2006] used a variant of stochastic Gradient Descent (SGD) to propose a back-end while improving the robustness to wrong initial guesses. This work was further improved to propose TORO [Grisetti 2007b] using a tree parameterisation. Examples of popular recent back-ends are also √ SAM [START_REF] Dellaert | [END_REF]],

iSAM [Kaess 2008b], iSAM2 [Kaess 2012] and g2o [START_REF] Kümmerle | [END_REF]].

As we focused on the front-end part, it will be described with more details hereafter. The back-end will also be introduced for sake of completeness.

Graph-Based optimisation

Graphical model is now a common tool used to describe the complex structure of a problem so that one can intuitively understand the relationship between the variables playing a role in the estimation process. This graph is converted into a cost function that will be minimised during the optimisation process. Graphical models are commonly used in smoothing methods, as opposed to filtering estimation methods. In SLAM community, the graphical model abstractly describes the process of Chapter 1. Estimation as filtering or online batch-optimisation problems

x i-1 x i u i z i p (x i | x i-1 , u i ) p (z i | x i ) p (x i-1 , x i | u i , z i ) = p(x i-1 )p (x i | x i-1 , u i ) p (z i | x i )
Figure 1.6: Representation of the DBN focusing on the relationship between state x i (the current state to estimate) and the variables used for this purpose : x i-1 (prior), u i (control sequence) and z i (measurement). As a consequence this DBN is a representation of the joint probability p(x i-1 , x i |u i , z i ). In this case, the prior is supposed to be independent from any other variable. This joint probability is a sub-part of the general equation given by (1.13).

the front-end, but also very literally guides the way it is implemented, in particular defining the way knowledge are stored in memory.

To introduce the graph formulation, we first need to introduce the Bayesian representation also known as Dynamic Bayes Network (DBN) before moving to the Factor Graph representation that we used in this work. For a detailed introduction to probabilistic graphical models, the interested reader can find explanations in [Koller 2009].

The Dynamic Bayes Network (DBN)

A DBN is a directed graph in which the variables are represented by nodes with their conditional dependencies represented by oriented edges. The dependency is represented with an arrow so that A → B means that the variable B depends on A. A graphical representation of (1.13) with a DBN in the case of a robot having only one sensor providing control sequences u and another sensor for measurements z is given in Fig. 1.6.

This representation can easily be adapted to a multi-sensor situation for both control sequences and measurements. To do this we keep in mind that the overall joint probability is obtained by the product of all the conditional dependencies. We can notice here that the prior is an independent variable. Therefore one can construct the DBN of a system carrying J controllers and K measurement sensors (see Fig. 1.7).

The equivalent graph in the case of a longer trajectory is straightforward ). In this case, the prior is supposed to be independent from any other variable.

(see Fig. 1.8). The graph for a multi-sensor case on a complete trajectory can be easily deduced from figures Fig. 1.7 and Fig. 1.8. DBN represents the knowledge relations. For defining the optimisation problem, another graph, the Factor Graph, is extracted from the DBN. We describe it now.

The Factor Graphs

Factor graphs are bipartite and use two different representations to highlight the relationship between variables. Nodes represent state variables that are estimated while factors represent the relation between those states linked by edges. Whether it is in Fig. 1.6, 1.8 or 1.7, the joint probability is expressed as a product of factors of the type p(x i |x i-1 , u i ). This allows the factorisation of all the conditional probabilities that imply two or more specific states. Indeed let us consider the following models that are assuming Gaussian distribution noises: Chapter 1. Estimation as filtering or online batch-optimisation problems

x 0 x 1 u 1 z 1 p(x i=0:N | u 1:N , z 1:N ) = p(x 0 ) N i=1 p (x i | x i-1 , u i ) p(z i | x i ) . . . x i u i z i . . . x N u N z N Figure 1
.8: Representation of the DBN on a full trajectory. Here we made the assumption that each state except the prior has a dependency link with only one control sequence and one measurement. As a consequence this DBN is a representation of the joint probability p(x 0:N |u 1:N , z 1:N ) which is given by the product of every single joint probability as described in Fig. 1.6. The most general graph implying a different number of control sequences and measurements by states is straightforwardly obtained from this figure and Fig. 1.7.

x i = f i (x i-1 , u i-1 ) + ε x i (1.59) ε x i ∼ N (0, Σ xi ) (1.60) z i = h i (x i ) + ε z i (1.61) ε z i ∼ N (0, Σ zi ) (1.62)
This set of equations yields to

e(x i-1 , x i ) = x i -f i (x i-1 , u i-1 ) = ε x i (1.63) e(x i , z i ) = z i -h i (x i ) = ε z i (1.64)
At this point, the Gaussian distribution assumption (see (A.7)) allows one to express the distributions in the general form

p(.|.) ∝ exp(- 1 2 e T Σ -1 e) (1.65)
where e refers to either (1.63) or (1.64) depending on the context. Similarly, Σ refers to either Σ xi or Σ zi . Σ -1 is referred to as the information matrix and φ is called factor. We will use φ to denote a factor so that φ = e T Σ -1 e (1.66)

A factor φ has the property to contain all the conditional dependencies related to two or more specific states by factorising all the factors into a single one as illustrated by Fig. 1.9. In this example we have the following set of probability distributions:

p(x 1 |x 0 , u 1 1 ) ∝ exp(- 1 2 e T 1 Σ x -1 1 e 1 ) p(x 1 |x 0 , u 2 1 ) ∝ exp(- 1 2 e T 2 Σ x -1 2 e 2 ) p(z 1 |x 1 , l 1 ) ∝ exp(- 1 2 e T 3 Σ z -1 e 3 )
The u 1 factor of Fig. 1.9b factorising the first two probabilities so that

p(x 1 |x 0 , u 1 ) = p(x 1 |x 0 , u 1 1 ) p(x 1 |x 0 , u 2 1 ) ∝ exp - 1 2 (e T 1 Σ x -1 1 e 1 + e T 2 Σ x -1 2 e 2 ) = exp(φ 1 + φ 2 )
As one can see, the Factor Graph allows to efficiently represent the relationship between the variables and thus have a better understanding of the problem. If we consider K to be the number of different factors φ in the graph, then the overall joint probability is given by

p(x, z) = K k=1 exp(φ k ) ∝ K k=1 exp(- 1 2 e T k Σ -1 k e k ) (1.67)
In graph-based optimisation, the joint probability described by (1.67) then needs to be maximised which is equivalent to minimising the negative log-likelihood of this same function so that

-log p(x, z) = K k=1 φ k ≈ K k=1 e T k Σ -1 k e k (1.68)
The problem as formulated here can be solved using nonlinear least square optimisation that will be introduced in the section coming hereafter.

Nonlinear least square optimisation

Solving the graph is in practice done by iterative nonlinear optimisation. An equivalence to (1.68) is given by

F(x) = <i,j> ∈ C φ ij ≈ <i,j> ∈ C e(x i , x j ) T Ω ij e(x i , x j ) (1.69)
where C is the set of all pairs of indices < i, j > for which a constraint is defined in the graph and Ω ij is the information matrix so that

Ω ij = Σ -1 ij .
The purpose of the optimisation and thus of solving the problem in a maximum likelihood approach Chapter 1. Estimation as filtering or online batch-optimisation problems

x 0 x 1 u 1 1 z 1 u 2 1 l 1 (a) DBN version x 0 x 1 z 1 l 1 u 1 (b) Factor Graph version
Figure 1.9: Factor graph and DBN representations of the same problem. We take an example derived from SLAM problems. l 1 is a landmark that is measured by a sensor through z 1 and which pose must be estimated. Two control sequences are used to estimate the transition from x i-1 to x i to be used as an example. Left: DBN representation of the corresponding problem. Right: Factor graph representation of the same problem. Factors corresponding to u 1 1 and u 2 1 are encapsulated in a single factor u 1 .

is to find the configuration of states x * such that the negative log-likelihood as expressed by (1.69) is minimised. Another way to say this is that the optimisation process aims at solving the following equation:

x * = argmin x F(x) (1.70)
Keeping the objective in mind, the nonlinear least square methods aim at reducing iteratively the residual defined by the sum of the factors φ k . The minimisation first involves approximating F(x) around an estimate x and to find a step ∆x to approach the optimal configuration x * while minimising the cost function. This step is used to update the estimate so that x ← x + ∆x and approximate F(x) once again to repeat this process until convergence. By noting x n and ∆x n with n ∈ N respectively the state estimate and step at iteration n, this iterative process can be described by

x n = x n-1 + ∆x n , lim n →∞ x n = x * (1.71)
Various iterative methods for nonlinear least square optimisation will be presented below starting with the Gradient Descent method. We will then introduce the Gauss-Newton and Levenberg-Marquardt methods. The introduction to the methods presented hereafter are taken from [Sola 2016].

The Gradient Descent method

The Gradient Descent method, also called steepest descent method in the literature is a minimisation method aiming at updating the state configuration in the direction given by the opposite of the gradient of the cost function ∇F. The first order Taylor expansion of the cost function around the current state estimate x gives

F(x + ∆x) Taylor = F(x) + ∇F ∆x (1.72) with ∇F ∆ = ∂F ∂ x x (1.73)
By definition of the gradient, ∇F gives the direction of the steepest ascend. Hence by going in the opposite direction that one can think of as the downhill direction, the estimation should converge towards the minimum. Therefore the optimum step is computed as

∆x * GD = -α∇F T (1.74)
where the parameter α is used to determine the length of the step in the steepest descent direction which also has to be computed, e.g. by dichotomy. The search directions computed by this method are completely independent from one iteration to another. Fixing a priori α results in poor convergence rate. This behaviour can be fixed by changing the value of the parameter α with line search method. Yet, even with the optimal α (which is typically too costly to compute) the rate of convergence is linear (i.e. super slow). Newton-based methods would be better alternatives in particular with better rates (at least in the convex basin).

The Newton method

In the Newton method, the cost function, also called objective function, is approximated by its second order Taylor expansion around the current estimate x

F(x + ∆x) Taylor = F(x) + ∇F ∆x + 1 2 ∆x T H F ∆x (1.75)
where

H F = ∇ 2 F (1.76)
The function given in (1.75) is a paraboloid from which we want to find the minimum to find the optimum step ∆x * . This search for the minimum implies to differentiate (1.75) and find the step so that

∇F T + H F ∆x * = 0 (1.77)

Chapter 1. Estimation as filtering or online batch-optimisation problems

yielding to a second-order step when H F > 0 defined by

∆x * = -H F -1 ∇F T (1.78)
This method can show quadratic (i.e. fast and effective) convergence when the initial guess is close enough to the solution. Like the gradient descent, Newton method is local, i.e. would be trapped in the first local minimum, however it is also more sensitive than the gradient descent, in particular requesting the cost function to be strictly convex. If not sufficiently convex, some eigen values of H will collapse, leading to improper numerical behaviours. A first situation illustrating this undesired behaviour is the one where the curvature of the cost function is too low, implying that the Hessian H F will get too small and thus its inverse will be bigger than desired. The second situation is involving the cases when H F is negative. This can happen in concave zones and then the step ∆x goes to the direction opposite to the minimum, thus increasing the cost function. To prevent unacceptable effects when some eigen values become non-positive, the hessian would be regularised, i.e. a small positive value is added to the hessian to guarantee its strict positivity. Before discussing regularisation let's first mention why in our case we at least have non-negative values.

The Gauss-Newton method

The Gauss-Newton method can be applied to objective function that square function of the error e(x) so that

F(x) = 1 2 e(x) T Ωe(x) (1.79)
where Ω is a symmetric positive-definite matrix. This method is similar to Newton's method excepted that it does not require the second derivatives to compute the optimum step as we will see now. The gradient vector ∇F and the Hessian matrix H F are defined from (1.79) as

∇F = ∂F ∂x x = e T Ω ∂e ∂x x = ȇT Ω J (1.80) H F = ∂ 2 F ∂x 2 x = ∂e ∂x T Ω ∂e ∂x + e T Ω ∂ 2 e ∂x 2 x = J T Ω J + ȇT Ω H (1.81)
where ȇ, J and H are respectively the column error vector, the jacobian matrix of first derivatives and the Hessian tensor of second derivative around current state estimate x so that ȇ ∆ = e(x) (1.82)

J ∆ = ∂e ∂x x (1.83) H ∆ = ∂ 2 e ∂x 2 x
(1.84) Supposing ȇ being small and J to vary slowly (hence the Hessian is small as well), we can neglect the second order term in (1.81). Thus, we do not need to compute the Hessian tensor H to approximate the Hessian matrix H F as

H F ≈ J T Ω J (1.85)
By replacing those terms in (1.75), we get the following approximation of the cost function defined by (1.79),

F(x + ∆x) ≈ 1 2 ȇT Ω ȇ + ȇT Ω J ∆x + 1 2 ∆x T J T Ω J ∆x (1.86)
And again by substituting the terms in (1.78) by their definition, we get the following expression of the optimum step in the Gauss-Newton method

∆x * GN = -(J T Ω J) -1 (ȇ T Ω J) T = -(J T Ω J) -1 J T Ω ȇ (1.87)
where the matrix

J #Ω ∆ = -(J T Ω J) -1 J T Ω (1.88)
is the left weighted generalised inverse of J, leading to

∆x * GN = J #Ω ȇ (1.89)
This descent step does not imply the computation of second derivatives. Moreover it induces super-linear convergence rate, i.e. nearly as good as plain Newton (and much better than Gradient Descent) when computing the optimum step as defined here.The weighted inverse J #Ω should not be evaluated explicitly. The linear system should be solved using methods designed to reduce the complexity of the problem such as the QR or the Cholesky factorisation methods. These factorisation methods will not be discussed here but the reader will find plenty of resources in the literature ( [START_REF] Golub | [END_REF], Sola 2016[START_REF] Guennebaud | [END_REF]) for details about QR and Cholesky methods.

The Levenberg-Marquardt method

Both Newton and Gauss-Newton methods are valid when the starting point is close enough to the solution, i.e. in the same convex basin of F. If this condition is not Chapter 1. Estimation as filtering or online batch-optimisation problems satisfied then the computed step length ∆x may lead the estimation into a local minimum due to a too low curvature of the approximated paraboloid given by the approximation of the cost function. The idea behind the Levenberg-Marquardt is to act more like the Gradient Descent algorithm when the current state estimate is far from the minimum and take advantage of the Newton-based methods convergence as we approach this minimum. To get to this behaviour, the first idea is to change the optimum step computation as given in (1.78) so that

∆x * L = -α(H F + µI) -1 ∇F T (1.90)
where α can be used to change the step length and µ is the parameter used to give a Gradient Descent like behaviour. Indeed, when the curvature of the paraboloid is too low, the Hessian approximated as H F = J T Ω J is very small and thus if µ is large enough then the Hessian can be neglected and the optimum step is given by the gradient ∇F T . Small values of the parameter µ result in a Gauss-Newton update. µ is usually initialised to be large, resulting in small steps for the first updates in the direction given by the gradient descent algorithm. If at some point the computed optimum step results in a higher cost, then this parameter is increased. Otherwise µ is decreased as the solution improves to approach the behaviour of the Gauss-Newton method and get a faster convergence towards the local minimum [Lourakis 2005]. This optimum step is improved by Marquardt's idea [Marquardt 1963] to replace the identity term I by the diagonal of the approximated Hessian.

∆x * LM = -α(H F + µ diag(H F )) -1 ∇F T (1.91)
so that the damping of the approximated Hessian matrix affects each direction of the state differently depending on the curvature of the cost function along that direction.

Conclusion

In robotics, we are facing the double challenge of facing a complex problem with the objective of running it on real-time. Filtering methods are not only known for being able to produce real-time estimation, they are also easier to implement, well studied in the literature and the estimation one can get may be satisfying depending on the application. However the remaining issue would be to find the best suited approach among all the different filters that now exist as each new filter often corresponds to some particular new hypothesis or approximation, each of them bringing some drawbacks, limitations or condition of use. In contrast, batchoptimisation methods are indisputable for off-line treatments. In visual SLAM, it is now accepted that batch-optimisation only offers advantages with respect to filtering, except for minimal systems [START_REF] Strasdat | [END_REF]. Is this also true when less rich sensors and no map are involved? In [START_REF] Mirzaei | [END_REF]] the authors compared the results of an EKF algorithm for an IMU-Camera calibration problem with a bundle Adjustment that they consider as giving the 'best achievable (offline) estimates.

Their results indicate that the EKF method and the batch least-square estimator are achieving similar results. However, they are not mentioning anything about the computation time. The main distinction between both filtering and online batchoptimisation methods might be the way to treat past data. Filtering methods tend to summarise the information gained with past historic of poses and measurements and propagate the joint probability distributions through time. Batch-optimisation methods discard unused measurements and historic poses to reduce the complexity of the problem and make the optimisation possible while the propagation of the probability distributions through time is made unnecessary.

There is also a broad middle ground between filtering and smoothing methods. However, if one tries to define the best possible filter by modifying the standard approach, one would converge more and more towards online batch-optimisation [START_REF] Strasdat | [END_REF]]. On the one hand, filtering methods have shown promising improvements these last years in terms of estimation accuracy. In [Mourikis 2007] the authors proposed a multi-state Kalman Filter and applied the estimator to a vision-aided inertial navigation problem providing results of nearly equal quality to batch-based methods. [Li 2013] is yet another example of EKF-based algorithm showing similar results than batch-optimisation methods in real-time. On the other hand, batch-optimisation methods have also been able to reach realtime operation [Leutenegger 2015[START_REF] Mourikis | [END_REF], Leutenegger 2015].

In this thesis we will focus on estimating the state of one rigid body under IMU, contact and odometry measurements. Considering only this information, we might have considered filtering approach for its simplicity. However we have worked in the broader context of later enriching the IMU measurements with visual data (yet left as a perspective, but on which we are currently working). We are also willing to contribute to a generic framework of estimation in robotics, by considering a system that does not need to be deeply refactored each time a new sensor is attached to the robot. We do not see that happening with filtering. For all these reasons, our work is bound to the online batch-optimisation approach, where our contributions take place. Let us insist again on the fact that filtering and optimisation are finally two faces of the same coin, as filtering is often a linear numeric solver on top of some clever marginalisation, but with the same underlying knowledge graph. Our contributions are then likely to be nearly directly useful in filtering.

Measurements

The IMU provides a measurement of the proper acceleration as well as the angular velocity of the sensor itself in an inertial frame. Thus the IMU is a combination of at least both 3-axis accelerometer and 3-axis gyroscope. A 3-axis magnetometer measuring the magnetic field intensity along the axis of the IMU is sometimes also included. The acceleration is measured through the displacement of a mass retained by a spring as illustrated in Fig. 2.2a. The magnitude of this acceleration is then proportional to a force via the relation to the elasticity of the spring, itself proportional to the acceleration via the principle of dynamics. A simplified scheme of both gyroscopes and accelerometers in IMU is given in Fig. 2.1. An IMU at rest on the surface of the Earth will measure an upward acceleration of 9.81 m.s -1 due to the gravity force. The angular velocity is obtained by detecting the changes of dynamics of an oscillating or rotating system via the Coriolis forces (Fig. 2.2b).

IMU sensors are very popular in robotics as they have been used for inertialonly navigation [Barshan 1995, Nilsson 2014a], visual-inertial navigation [START_REF] Roussillon | [END_REF][START_REF] Konolige | [END_REF], attitude estimation [START_REF] Hamel | [END_REF]] or even applications using smartphones [Li 2013]. Indeed, by means of time integration of the provided measurements, it is possible to estimate the trajectory of the IMU or to detect events Simplified scheme of gyroscopes and accelerometers in an IMU [START_REF] Tedaldi | IMU calibration without mechanical equipment[END_REF]]. such as slippage in the applications we are concerned with. In our case, we are using IMUs based on MEMS (micro electromechanical systems) technology resulting in micro scale sensors. There has been a rapid growth in the development of such sensors due to their extensive use in machines as the computers and smartphones while the production cost kept decreasing. These sensors have advantages such as compactness and relatively high bandwidth. However, they are also suffering from drawbacks such as non-neglect able biases in their measurements and also their sensitivity to temperature. Several characteristics must be taken into account when choosing an IMU. In addition to its precision, the operating range is an important criterion and can be limited due to physical factors coming from the mechanical elements the IMU is built with and that have their own limitations. Other criteria that can be taken into account are usually the axis misalignments, the biases ranges or the scale factor errors. The calibration process refers to the identification of these parameters. There are commercial IMUs that are factory calibrated. However, this implies that the manufacturer performed data acquisition with IMUs attached to a specific machine and performing motions so that all these errors are made observable to estimate it. Naturally, these processes increase considerably the cost of the IMUs since each sensor is usually sold with its own calibration parameters stored inside a non-volatile memory or in the firmware itself. As a result, the hardware itself is only a fraction of the final cost of these IMUs but the process leading to provide off the shelf accurate measurement is what people are actually buying to these companies.

The sources of errors 2.2.1 Misalignment errors

In an ideal IMU, the 3 axes of the accelerometers are orthogonal and aligned with the 3 axes of the gyroscope. The accelerometer measures the accelerations distinctly along these 3 axes while the gyroscope measures the angular velocity around each of the 3 distinct axes. However, in real MEMS IMUs and due to assembly inaccuracies, the axes may be not only non-orthogonal but also misaligned (see Fig. 2.3). Let us call the ideal orthogonal accelerometer and gyroscope frames respectively A B and W B and the real (non-orthogonal) frames A S and W S . By thinking separately about the accelerometer and the gyroscope, these frames can be described by the sets of axes (x B , y B , z B ) for A B (respectively W B ) and (x S , y S , z S ) for A S (respectively W S ) in Fig. 2.3. Acceleration (respectively angular velocity) measurements are acquired in the non-orthogonal frame A S (respectively W S ). Depending on the amplitude of the misalignments and how far the frame is from an orthogonal one, a correction might be required to express the output measurements in the orthogonal frame. For small angles, this correction is expressed by a transformation matrix M given angles as described in Fig. 2.3. Thus if a S denotes the 3D acceleration measurements expressed in A S , then a B the corresponding measurement expressed in A B is given by The exact same relation can be written for the gyroscope measurements. Finally, misalignments and orthogonality for accelerometers and gyroscopes are different. Thus specific transformation matrices M a and M ω might be required to correct respectively acceleration and rate of turn data.

a B = M a S (2.1) with M =    1 -β yz β zy β xz 1 -β zx -β xy β yx 1    (2.2)
a B = M a a S , ω ω = M ω ω S (2.3)
In the ideal case M a and M ω are the identity matrix.

Scale factor errors

The scale factor error is the relation between the input and the output. In an ideal IMU, we expect the input and the output to be equal. However in a real IMU, the output might be affected by a linear effect resulting in an output that is proportional to the input. The scale factors on each axis of the IMU are independent so that we can define two scaling matrices K a and K ω so that the accelerometer and gyroscope outputs can be described respectively by

a B = K a a S (2.4
)

ω B = K ω ω S (2.5)
where (2.4) and(2.4), we get the following IMU model error

K a =    s a x 0 0 0 s a y 0 0 0 s a z    , K ω =    s ω x 0 0 0 s ω y 0 0 0 s ω z    (2.6) With (2.3),
a B = M a K a a S
(2.7)

ω B = M ω K ω ω S (2.8)
Scale factor effects are most apparent in times of high acceleration and rotation.

Biases

The output measurements of IMUs are affected by biases for both acceleration and rate of turn components. Each axis of the IMU is affected by its own offset that is independent from the other axis. Thus the bias can be modelled with six components or by two vectors a b and ω b defined as

a b =    a b,x a b,y a b,z    , ω b =    ω b,x ω b,y ω b,z   
(2.9)

These biases take part in the sensor error model as additive terms directly on the inputs so that

a S output = a S input + a b (2.10) ω S output = ω S input + ω b (2.11)
The complexity of the bias estimation can come from two major points. First of all, for each power up of the IMU, the initial bias is different. These changes are partially due to modifications of the physical properties of the IMU itself and different initial conditions. A higher bias repeatability tends to allow a better initial tuning of the IMU's parameters thus leading to a faster convergence towards good bias estimates. At the opposite, a higher variability leads to more difficult bias estimations. Furthermore, the biases change over time making their estimation even more important and difficult. The changes in bias can be related to temperature, time or even mechanical stress applied on the system. The evolution of the biases over time can be modelled as a random walk which depends on the quality of the IMU. The higher the quality of the IMU the stabler we can expect the biases to be over time. For a proper use of the sensor, the biases need to be removed from the sensor measurements before the data are used. Using equations (2.7), (2.8)

a B = M a K a a S + a b
(2.12)

ω B = M ω K ω ω S + ω b (2.13)

Noises

Finally, IMUs do not make an exception among sensors as they are also affected by measurement noises. The noise affecting each axis of the IMU is independent from the others thus also resulting in two noise vectors a n and ω n so that

a S output = a S input + a n (2.14) ω S output = ω S input + ω n (2.15)
with

a n =    a n,x a n,y a n,z    , ω n =    ω n,x ω n,y ω n,z    (2.16)
where each component can be described with a Gaussian distribution.

a n ∼ N(0, Σ a,n ) , ω n ∼ N(0, Σ ω,n ) (2.17)
Σ a,n and Σ ω,n are different and depend on the sensor itself. Finally, using (2.12), (2.13) and (2.16) the complete sensor error model becomes

a B = M a K a a S + a b + a n (2.18) ω B = M ω K ω ω S + ω b + ω n (2.19)

Used IMU sensor model

The effects of aforementioned errors are not the same given the experimental conditions. The scale factor error in IMU is usually not a large contributor to the total errors. The contribution of the accelerometer scale-factor error increases with the input acceleration. Furthermore, it causes position and velocity calculation errors through the integration of the errors but also inaccuracies on the estimation of pitch and roll angles. In a similar way, the gyroscope's scale-factor error is larger when the input increases, that is during motions with large angular velocities. The effect one can expect from this source of error is difficulties to track highly dynamic motions. Velocity and position errors due to the accelerometer scale-factor can be expressed as

v error = a sf_error * g * t (2.20) p error = g * t 2 2 * a sf_error (2.21)

Calibration of the sensor 47

where a sf_error and ω sf_error are respectively the accelerometer and gyroscope scalefactor errors while g and t are the gravity and the integration time.

Compared to the other possible sources of errors, both accelerometer and gyroscope biases are significant contributors to position, velocity and orientation errors. This major contribution is due not only to the difficulty to estimate time-varying biases made only more complex through their observability conditions requiring various poses and motion dynamics but also to the integration of these errors for position and velocity estimation. Besides, these errors occur during any motion of the IMU. Thus we decide to neglect other sources of error than biases and noises. The resulting IMU error model is described by

a S output = a S input + a b + a n (2.22) ω S output = ω S input + ω b + ω n (2.23)

Calibration of the sensor

Introduction

The calibration process usually consists in placing the device in different configurations to apply different stimuli on it. These different excitements introduced as input measures to the IMU enhance the observability of the different parameters thus allowing to determine the actual error in each measurement. There are different calibration methods to estimate the parameters of the IMU. The first methods were based on the use of mechanical equipment. Then came methods going towards estimation using sensor fusion instead of a specific costly equipment.

Calibration based on the use of mechanical equipment

Factory calibration processes are still using specific and costly mechanical equipment. Different equipment may be required to estimate the different parameters of the IMU. Thus accelerometer calibration can be performed by rotating the sensor so that each axis is aligned with the known gravity. This process can be done using a dividing-head precision device that is able to rotate precisely. The gyroscope calibration can be performed using a high precision rate table. There exist different types of rate tables according to the number of degrees of freedom they can operate in. The higher the degree of freedom the more complex the generated motions can be and the quicker the calibration process is expected to be. Once all the measurements are made, a batch linear least square or a Kalman Filter can be used to estimate the optimal calibration parameters while minimising the errors.

In [START_REF] Magnussen | [END_REF]] the authors are describing the calibration process using a 6 degree of freedom hexapod for a strapdown integration of the IMU on an Unmanned Aerial Vehicle (UAV). This calibration process is used to estimate the internal parameters of the IMU and the position offset due to the pose of the IMU on the UAV. [START_REF] Skog | [END_REF]] proposes a calibration method that requires no me- chanical equipment for the accelerometer but a rate table for the gyroscope. The proposed method is based on the measurement properties of the IMU resulting in an equality of the norm of the measured output of the accelerometers (respectively of the gyroscopes) clusters and the magnitude of force (the rotational velocity respectively). Further calibration methods using mechanical equipment are described in [Cho 2005[START_REF] Pittelkau | [END_REF], Beravs 2012] with different methods.

Calibration without mechanical equipment

With the increase of potential applications using IMUs, more effort has been made towards easier calibration of inertial measurements units. [START_REF] Fong | [END_REF]] proposed a calibration method for both accelerometer and gyroscope that does not require any mechanical equipment. This method is also used to compensate both axis misalignment and scale factor errors using the Earth's gravity as a stable physical force applied on the IMU and arbitrary motions. To be more specific, the idea behind is to calibrate the gyroscope by comparing the estimated orientation and the output of the accelerometer measurements. A similar idea is used in [START_REF] Cheuk | [END_REF]] to extend the calibration to the magnetometers. The importance of calibration procedures for low-cost IMUs also increased the interests for an easy estimation of IMU's parameters. Thus [Nilsson 2014b] proposed a device to calibrate single-chip IMUs that can be used to correct the misalignment errors. This method is based on static measurements taken while the IMU is placed in different orientations. The measurements are made easy thanks to a printable device that is an icosahedron solid (see Fig. 2.4b) where the IMU can be placed in and available for download.

The recorded data used to estimate the misalignment of the axis, the gain and biases using a maximum likelihood based approach. Unfortunately, the method requires the IMU to be still in the solid designed for a specific array on which it is placed.

Self-calibration

Alternatively, calibration can be performed online, in real-time alongside all other operations of the system, in what is known as self-calibration. For this, the system needs to use additional sensors that, when operating together, render the parameters to calibrate observable.

We will be presenting self-calibration methods with more details in Chapter 4, after the IMU particular case developed in Chapter 3. Indeed, in Chapter 3 we apply these methods to the self-calibration of the IMU in SO(3) in applications of pedestrians and humanoid walking. In Chapter 4, we generalise the methodology for self-calibrating motion sensors in the framework of graph-based optimisation on manifold.

Chapter 3

Graph based IMU preintegration on the S 3 manifold, with application to localisation

Introduction

Context

Indoor localisation is an open challenge in various situations: location-based life improving services, firefighters localisation and navigation, patient tracking motion monitoring, medical observation, accident monitoring, mobility and independence of partially sighted or blind persons, etc. As GPS are not available indoor, and because relying on a network of fixed sensors (cameras, RFID) also raised many open questions, an appealing way to localize a body in space is to use odometry information measured by embedded inertial measurement units (IMUs). In this context, while performing the integration it is mandatory to take into account the IMU biases. As the biases vary with time and physical conditions, it must be estimated on-line while processing the measurements. Furthermore, it is desirable that additional information coming from other sensors can be integrated in the same estimation process. Similarly to the strategies adopted in simultaneous localisation and mapping (SLAM), sparse measurements or additional information (e.g. coming from intermittent absolute localisation, or from a sparse sensor network) would benefit to the localisation process when available. With these requirements coming from the context in mind, we propose to define an estimator based on graphical models, able to accurately and efficiently integrate inertial measurements while estimating the IMU biases. Thanks to the graphical model, the estimator will then be able to fuse additional measurements coming from other sensors or additional prior information provided by the application. The long-term goal is to merge this method with visual SLAM estimates. Indeed, the method that we propose is suitable for merging with any other source of information. While the fusion with visual data is still work in progress, we propose here two simpler -yet usefulexperimental contexts: pedestrian indoor tracking and legged robotics.

Related Work

One of the most common ways to describe the trajectory of a body in space is to use odometry, which can be derived from a multitude of sensors, from encoders to GPS and cameras. The quality of this information is also known to be dependent on the sensor specifications and to accumulate measurement errors. The development of SLAM (Simultaneous Localization And Mapping) is mature enough to resolve this dependency over time with loop closure strategies. However, other strategies are required to comply with the particularities of legged locomotion and physical interactions in unstructured environments. This is particularly necessary in outdoor or industrial applications with cluttered environment. In this case, assuming a flat floor is not always reasonable and being able to observe the foot pose allows to implement advanced reactive locomotion strategies. While this problem is an open challenge in various domains, like indoor pedestrian tracking for life service, we mostly focus here our analysis of the litterature to the robotics domain. In the DRC [Johnson 2017[START_REF][END_REF][START_REF] Karumanchi | [END_REF] most of the teams used laser or RGB-D cameras to build a reconstruction of the floor surface in order to plan the next feet pose. For some teams the level of accuracy was not always sufficient and involved dramatic failures despite intensive training [START_REF] Kaneko | [END_REF]]. On the other hand, the IHMC team [Johnson 2017] reported an important gain in terms of accuracy using their state estimator alone, reaching an impressive 1 cm drift per every three steps for the pelvis horizontal position, and 5 mm per every nine steps for the pelvis vertical position. Using a quite simple state estimator, it is very interesting to note the following reported factors for reaching this level of precision besides bug fixing: the redesign of Atlas, coming with a significant reduction of backlash in the leg joints, improving measurements using kinematics and a walking gait reducing the amount of foot slipping and bouncing. Although the IHMC tested a localization algorithm [Pomerleau 2013], occasional localisation errors were a problem in the overall behavior and SLAM happened to be not necessary in this situation.

For robots where the design choices introduce flexibilities such as HRP-2 [Nakaoka 2007], or backlashes, the system needs a state estimator being able to take into account this uncertainty. Furthermore, the design of a robot gets even more complex with the increase in the number of degrees of freedom (dof). Estimating the state of the robot usually requires the use of successive transformation using the kinematic chain, thus reconstructing the pose of a specific joint from the estimation of another point in the robot such as the free flyer. In a first instance, errors due to encoders measurements can be neglected although they accumulate over time. However, a good pose estimation of the robot in its environment is still needed for effective interactions with the environment, and especially for behavior involving manipulation or multiple contact locomotion. This estimation can then be used to compute the commands that will be sent to actuators. An accurate model of the robot can then be considered to get better pose estimations. However, although the identification of rigid robot's dynamics is well studied, it is still a complex and time-consuming task.

Localisation can be used to perform real-time planning and model predictive control. To achieve this goal, fusion strategies with information coming from different sensors is needed. Fusion strategies have always been used on humanoid robots to improve the pose estimation. [START_REF] Stasse | [END_REF]] is considered as the first implementation of real-time vision-based 3D SLAM applied to a humanoid robot and using both pattern generator and inertial information with loop closing capabilities. In [START_REF] Ahn | [END_REF]] the authors propose a method for vision-based 3D motion estimation using on-board odometry information and inertial sensing. The proposed method is divided into two submodules that are a EKF-based odometry estimation module and a vision-based SLAM method. They claim to improve the pose estimation given by the second method by augmenting it with the odometry obtained by using the forward kinematics model of the robot and information provided by encoders. The IMU is a main component of the proposed method and operated at 100 Hz. Fallon et al. [START_REF][END_REF]] go further by using leg kinematics, inertial measurements and visual perception provided by LIDAR sensor to perform drift-free state estimation applied to ATLAS humanoid robot (Fig. 3a) while localising it in a prior map. The distinct sensing modalities are used with two different state estimators. The first one provides position and velocity estimates with high rate and low latency so that these can be used for control feedback. The second estimator is using vision sensors for long-term localisation while accepting small short-term corrections. The presented method allows them to have the robot walking over uneven terrain without stopping for ten minutes. Graphical methods have been extensively used to implement such fusion strategies [START_REF] Thrun | The graph SLAM algorithm with applications to large-scale mapping of urban structures[END_REF],Kaess 2008a]. They have been used for large modelling estimation problems by means of sparse networks of constraints. In robotics, the problems of visual odometry, and simultaneous localisation and mapping, have reached a high degree of maturity, in great part thanks of the graphical representation. This is so, among other aspects, because of the power of the graphical representation to accurately model complex estimation problems. These often involve dynamics, proprioceptive measures, exteroceptive measures, and self-calibration. The graphical representation also allows for the design of powerful nonlinear estimation solvers, which can be built taking into account the needs for accuracy, robustness and CPU-performance. In order to keep the problem tractable and maintain real-time performance a key point is to avoid the graph to be too large for a given time window. However, trajectory correction using SLAM and more generally visual information implies to revise the control sequences and the planning at a much lower frequency due to sensor imperfections and the need in SLAM for a loop-closure to propagate uncertainty corrections.

Methodology

Graphical methods have been extensively used to implement such fusion strategies [START_REF] Thrun | The graph SLAM algorithm with applications to large-scale mapping of urban structures[END_REF], Kaess 2008a]. They are well-suited to gather information from sensors and draw conclusions. The underlying principle is to consider that despite all the information gathered from the sensors, we still have uncertainties about the true state of the world due to imperfections of the sensors. Several states of the world can thus be considered as probable. Relying on probabilistic formulations is a way to find out the most probable one. Furthermore, graphical representations are able to accurately model complex estimation problems [Koller 2009] in a versatile way.

Graphical models have been used for large modelling estimation problems by means of sparse networks of constraints and particularly in robotics where SLAM and visual odometry problems have reached a high degree of maturity in great part thanks to these tools. The graphical representation also allows for the design of powerful nonlinear estimation solvers, which can be built taking into account the needs for accuracy, robustness and computational performances.

In order to keep the problem tractable and maintain real-time performance, a key point is to prevent the graph from being too large given a time window. IMUs are challenging in this regard, as their high frequency measurement rate application to localisation creates large sets of data. Pre-integration of IMU measurements helps to reduce the size of the underlying graph by squeezing 100 to 1000 measures into a single pre-integrated Bayesian node [Lupton 2009]. It was later suggested to preintegrate data in the SO(3) manifold instead of using Euler angles [Forster 2015]. The IMU measurements can simply be disregarded even when the initial "pre-integration" conditions change while the numerical solver optimised the maximum-likelihood trajectory.

Contributions

We propose an alternative estimation method following a similar methodology. We define a graphical model where pre-integrated data are added at key-frame instants (about 2 Hz), summarising IMU measurements captured at about 1 kHz. The state that we want to estimate considers the position, orientation and linear velocity (dimension 9) of the IMU attached to the foot, along with the time-varying accelerometer and gyroscope biases (dimension 6). We also implement the prior knowledge that the foot lands horizontally, by adding Bayesian nodes when the foot lands and takes off. This prior knowledge makes the considered state observable. We then use a numerical solver to maximise the likelihood of the measurements and prior knowledge along the past trajectory.

The first contribution is to reformulate the pre-integration method introduced in [Forster 2015] using quaternion representation and to give a detailed and simpler algebraic derivations using the chain rule. The result of this preintegration method is a clear definition of Jacobians as well as a better understanding and thus easier integration for future applications. The second contribution is to apply this method for estimating the human foot-pose during walking. To this end, we build a graphical model formulation of the problem by gathering information. Once the formulation is completed, a nonlinear least-squares optimiser (Google Ceres [Agarwal ]) is used to solve the problem and find the most probable solution in the least-square sense.

Graph-based inertial-kinematic odometry

Graph-based iterative optimisation

As explained in 1.3.2, the problem is represented as a graph, where the nodes refer to the variables, and the edges, are called factors that represent the geometrical constraints between variables due to measurements. We define here the graphical model proposed to handle the IMU. The state x is modelled as a multi-variate Gaussian distribution, and in our case it includes poses and velocities (p, q, v) of the body to which the IMU is attached to and IMU biases (a b , ω b ). This state is sampled at selected key-frames along the trajectory (see Fig. 3.2). For each factor, we have to define the corresponding measurement residual, i.e. an error or a residual r as the discrepancy between a measurement z and its expectation given

Graph-based inertial-kinematic odometry 57

Figure 3.2: Detailed factor graph for the initial keyframe and two steps. Circles: state blocks for position (p), orientation quaternion (q), velocity (v), accelerometer bias (a b ), gyrometer bias (ω b ). Orange: initial pose factor. Red: kinematic factor (deduced from additional sensors). Purple: zero-velocity factor. Green: IMU's delta pre-integration factor. Blue: bias drift factor. Cyan: bias absolute factor. the involved state variables,

r(x) = h(x) + v -z, v ∼ N (0, Ω -1 ) (3.1)
being h(x) the sensor measurement model and Ω the information matrix of the measurement Gaussian noise v. One of the difficulties brought by this estimation problem is that the variables defined on manifolds, such as quaternions or rotation matrices. We then must rewrite (3.1) as

r(x) = (h(x) ⊕ v) ⊖ z (3.2)
The ⊕ and ⊖ symbols correspond to the addition and subtraction operators on the manifold that we more completely introduce in C.1.3.

As explained in 1.3.3, the maximum a posteriori estimation is obtained by iteratively minimising the Mahalanobis squared norm of all linearised errors ∆x * = arg min

∆x k r k (x) + J k ∆x 2 Ω -1 k (3.3)
being x the state estimate at the current iteration, and J k the Jacobian of the k-th residual r k (x) (with J k = ∂(h k (x) ⊖ z k )/∂∆x in the case of variables lying on a manifold) and Ω k is the information matrix of the k-th measurement. Here we do not have to enter in the details of the optimisation scheme, that we kept generic (we literally used the optimiser on manifold Ceres without any additional flavour).

We just have to precisely define this function, and its derivatives J.

Where to put Keyframes?

During a biped walk, we take profit of certain situations where precise and reliable assumptions can be made. For example, the foot velocity is null during its support phase. At these selected instants, we create the keyframes that will produce a chain of states. These states are linked by the measurements, forming our factor graph ( Fig. 3.2). Each keyframe f i contains the following state blocks: the position of the foot, velocity and orientation data, plus the IMU accelerometer and gyrometer biases,

f i = p i v i q i a b,i ω b,i ⊤ .
(3.4)

Description of factors

The types of factor considered in our graph are illustrated in Fig. 3.2. Each factor k requires its own information matrix Ω k , and its residual function r k (x). These residual functions are detailed hereafter.

Absolute factors

These include initial position and orientation (orange in the figure), zero velocity (purple), and bias magnitude (cyan). Each residual depends on a single state block, which is compared against a reference z k ,

r k (φ i ) = φ i -z k (3.5)
where φ i is one among {p i , v i , a b,i , ω b,i }. For the quaternion we implement the residual using the operator ⊖ on the sphere of dimension 3 manifold, denoted S 3 (see (C.5) in Appendix C.1 for further details),

r k (q i ) = q i ⊖ z k = Log(z * k ⊗ q i ) (3.6)

Bias drift factors (blue)

These are relative factors that allow the bias estimates to drift with time at a controlled rate. Each bias drift residual depends on two state blocks, namely

r(a b,i , a b,j ) = a b,j -a b,i r(ω b,i , ω b,j ) = ω b,j -ω b,i (3.7)

Complementary factors (red)

These relate position and orientation between two consecutive steps as it can be provided by other sensors than IMU or methods using human walking specificities,

r(p i , q i , p j , q j ) = q * i ⊙ (p j -p i ) -y k Log(z * k ⊗ q * i ⊗ q j ) (3.8)
3.3. IMU pre-integration in S 3 and SO(3)

59

where y k and z k are respectively the relative position and quaternion measurements.

IMU pre-integrated factors (green)

Due to the different rates of IMU data and keyframe creations, hundreds of IMU measurements need to be integrated to generate a motion factor linking two consecutive keyframes. In addition to that, the integration of the motion equations in an absolute reference frame strongly depends on the initial conditions of orientation, velocity and IMU bias. Therefore, the changes in the estimates of these magnitudes (inherent to the iterative nature of the optimisation) affect the whole motion integral. Delta pre-integration theory was developed to avoid the need of re-integrating all IMU data at each iteration [Lupton 2009, Forster 2015]. On the one hand, this theory defines new motion magnitudes called deltas, which are independent of the initial conditions for orientation and velocity, and thus depend only on the IMU data and bias. On the other hand, the effect of the changes in the bias estimates is linearised so that the deltas can be corrected a posteriori, i.e. when computing the residual, using pre-computed Jacobians.

In the coming section, we revise the IMU pre-integration theory, providing three contributions: 1) a segmentation of the computation pipeline (from measurements, to body magnitudes, to the current delta, and to the integrated delta); 2) a physical interpretation of the delta magnitudes; and 3) a simpler yet rigorous algebraic approach, valid for both the S 3 (quaternion) and SO(3) (rotation matrix) manifolds, which takes profit of the pipeline segmentation and the chain rule to compute the otherwise cumbersome Jacobians [Forster 2015]. Some background that are not so common in robotics can be found in appendix Appendix C.1 to help follow the developments of the preintegration equations.

IMU pre-integration in S 3 and SO(3)

State integration in the absolute reference frame

We define the world-referenced states of the IMU by x = (p, v, q) where p stands for the position, v for the velocity and q for the orientation encoded as a quaternion. The time evolution of x is governed by the kinematic equation,

ṗ = v v = g + q ⊙ a q = 1 2 q ⊗ ω (3.9)
where g denotes the gravity vector and we identify b = (a, ω) as the body magnitudes, that is, the magnitudes of acceleration and angular velocity measured by the IMU and expressed in its reference frame. These body magnitudes are obtained at Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation discrete times t j from biased and noisy IMU measurements, i.e., a j a m,j -a b,j -a n ω j ω m,jω b,jω n , (3.10) with • m the measurements, • b the biases, and • n the noises. Assuming constant body magnitudes within the IMU sampling period δt t k -t j , we have the discretetime relation:

p k = p j + v j δt + 1 2 gδt 2 + 1 2 q j ⊙ a j δt 2 v k = v j + gδt + q j ⊙ a j δt q k = q j ⊗ Exp(ω j δt/2)
(3.11)

Delta definitions

Consider a non-rotating reference frame that is free-falling at the acceleration of gravity g, and name it G t . An ideal (unbiased and noiseless) IMU glued to this frame would measure null linear accelerations and angular velocities. Any non-null measurements would be due to a relative motion of the IMU with respect to G t . Thus, we identify the IMU deltas as being physically identifiable as the motion increments with respect to a free-falling frame, that is, a frame that falls with gravity as illustrated in Fig. 3.3. At a given keyframe instant t i , we initialize G i at x i = (p i , v i , q i ). At a later keyframe instant t j (j > i), G j has fallen according to g, and the state of our moving body is now at x j = (p j , v j , q j ). The motion variation, denoted ∆ ij , is defined as the state variation in position, velocity and orientation of our body between G i and G j , that is,

∆p ij = q * i ⊙ p j -p i -v i ∆t ij - 1 2 g∆t 2 ij ∆v ij = q * i ⊙ (v j -v i -g∆t ij ) ∆q ij = q * i ⊗ q j (3.12)
where ∆t ij t jt i is the time duration between the two keyframes. Notice that this definition of ∆ ij is the same as provided in [Lupton 2009, Forster 2015], and we have given it here a clear physical meaning. It is worth to notice that the deltas form a group under the composition law ∆ ik ∆ ij ⊞ ∆ jk ( Fig. 3.4), defined by:

∆p ik = ∆p ij + ∆v ij ∆t jk + ∆q ij ⊙ ∆p jk ∆v ik = ∆v ij + ∆q ij ⊙ ∆v jk ∆q ik = ∆q ij ⊗ ∆q jk (3.13)
with identity ∆ 0 = [(0, 0, 0), (0, 0, 0), (1, 0, 0, 0)], and inverse ∆ ji ∆ -1 ij such that ∆ -1 ⊕ ∆ = ∆ ⊞ ∆ -1 = ∆ 0 . At any time j we can recover the state estimate x j The blue frame G i is the initial frame at time t = i. At this moment, the IMU is moved with an instant velocity v i from the pose given by {p i , R i }. If the rigid body the IMU is attached on does not move with a proper acceleration and rate of turn from time i to j, then the trajectory of the IMU would be given by the free-falling frame and the device would be on the frame G j at time t = j. However if the IMU moves with a proper acceleration and rate of turn, then its pose at time j is given by ∆ ij with respect to the free-falling frame.

IMU pre-integration in

from the state estimate x i and the motion delta ∆ ij :

p j = p i + v i ∆t ij + 1 2 g∆t 2 ij + q i ⊙ ∆p ij v j = v i + g∆t ij + q i ⊙ ∆v ij q j = q i ⊗ ∆q ij (3.14)

Incremental delta pre-integration

Substituting the integration Eq. (3.11) in the delta definitions (3.12), we obtain the incremental delta pre-integration,

∆p ik = ∆p ij + ∆v ij δt + 1 2 ∆q ij ⊙ a j δt 2 ∆v ik = ∆v ij + ∆q ij ⊙ a j δt ∆q ik = ∆q ij ⊗ Exp(ω j δt) (3.15)
with ∆ ii = ∆ 0 . Interestingly, (3.15) is analogous to the motion of a body in an inertial frame under constant acceleration and rotation rate. Notice that by letting Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation the reference frame fall with gravity, we get rid of the dependence on gravity in the integration equations, and only the body magnitudes drive the integral. Indeed, we can define a proper delta δ jk from the current body magnitudes b j = (a j , ω j ) b m,i -b b,j -b n,j at time t j ,

x i x j x k ∆ ij ∆ jk ∆ i k = ∆ i j ⊞ ∆ j k
δp jk = 1 2 a j δt 2 δv jk = a j δt δq jk = Exp(ω j δt) (3.16)
and write the integration (3.15) as the composition

∆ ik = ∆ ij ⊞ δ jk (3.17)
described in (3.13). Typically, we take the biases at the keyframe time t i , that is, b b,j = b b,i . In the following, we will identify ∆ with the pre-integrated delta, and δ with the current delta.

Jacobians

We note all Jacobians with J y x ∂y/∂x and refer the reader to Appendix C.1 for details on the development of all non-trivial Jacobian blocks in this section.

Jacobians of the body magnitudes

from Eq. (3.10) we have:

J b bm = I 6 J b b b = -I 6 J b bn = -I 6 .
(3.18) 3.3. IMU pre-integration in S 3 and SO(3) 63

Jacobians of the current delta

We have from (3.16),

J δ jk b j =    1 2 Iδt 2 0 Iδt 0 0 J r (ω j δt)δt    ∈ R 9×6 (3.19)
where we develop the lower-right block as in Appendix C.1.5.1.

Jacobians of the delta composition

We differentiate the delta composition (3.17) described in (3.13),

J ∆ ik ∆ ij =     I Iδt -∆R ij δp jk × 0 I -∆R ij [δv jk ] × 0 0 δR ⊤ jk     ∈ R 9×9 (3.20a) J ∆ ik δ jk =    ∆R ij 0 0 0 ∆R ij 0 0 0 I    ∈ R 9×9 (3.20b)
where ∆R ij and δR jk are the rotation matrix deltas corresponding to the respective quaternion deltas ∆q ij and δq jk . We develop all the non-trivial blocks as in Section C.1.5.2 and Section C.1.5.3.

Incremental delta covariance integration

Let Q ∆ be the covariance of the pre-integrated delta, and Q n the one of the measurement noise. For convenience, we first compute the covariance of the current delta,

Q δ = J δ bn Q n J δ bn ⊤ , (3.21)
where J δ bn = J δ b • J b bn is the noise Jacobian, obtained with (3.18-3.19) and the chain rule. The delta covariance is then integrated with

Q ∆ ik = J ∆ ik ∆ ij Q ∆ ij J ∆ ik ∆ ij ⊤ + J ∆ ik δ jk Q δ J ∆ ik δ jk ⊤ , (3.22)
using Jacobians (3.20), and starting at Q ∆ ii = 0 9×9 .

Delta correction with new bias

Let ∆ and b b be respectively the pre-integrated delta and the bias values used during pre-integration. Since the bias estimates change at each iteration of the optimiser, we need to update the delta according to the new bias values b b . We do 64 Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation so with the linearised update,

∆ = ∆ + J ∆ b b (b b -b b ) , (3.23)
where J ∆ b b is the pre-integrated bias Jacobian, computed incrementally using the chain rule,

J ∆ ik b b = J ∆ ik ∆ ij J ∆ ij b b -J ∆ ik δ jk J δ jk b b . (3.24) with J δ jk b b = J δ jk b J b b b . This Jacobian starts at J ∆ ii b b = 0 9×9 .

Residuals

The computation of the residuals for the IMU delta factors (see Fig. 3.2, green) requires: the state estimates x i and x j ; the current bias estimates b b,i ; the pre-integrated delta ∆ ij ; the bias used during pre-integration b b,i ; and the preintegrated bias Jacobian J

∆ ij b b . The process is best understood if split into smaller steps: we first compute a corrected delta ∆ ij using (3.23); then we compute a predicted delta ∆ ij using (3.12); and finally we compute the residual with

r(x i , x j , b b,i ) =    ∆p ij -∆p ij ∆v ij -∆v ij Log( ∆q * ij ⊗ ∆q ij )    ∈ R 9 .
(3.25)

Its information matrix is given by Ω = Q -1 ∆ik .

Pedestrian Localisation

Context

Localisation of pedestrian in indoor environment remains an open problem. A cheap and reliable sensor in this context is the inertial measurement units (IMU), carried by the pedestrian while he/she is walking. However, due to the bias of both the accelerometer and the gyroscope, integrating the inertial measurements directly leads to tremendous drifts, as the state of the system (position, orientation, velocity, bias) is not fully observable. We consider the specific case where an IMU is attached to one of the pedestrian's feet. We exploit specific prior knowledge (i.e. the fact that the foot lands at zero velocity during full-contact phase (Fig. 3.5)) in order to make the full state of the IMU observable. The inertial measurements and these prior knowledge are gathered in a graphical model (a factor graph), and are exploited to build a maximum-likelihood estimator. We validate these concepts on several long-range trajectories captured with human subjects from a dataset provided by the literature [START_REF] Angermann | [END_REF]] and compare the results with groundtruth measurements (coming from a motion capture system) and previous results of the state of the art. This study verifies the validity of our method while not relying on any new hardware configuration. Indeed, running tests directly on a humanoid robot may introduce some undesirable effects with respect to the IMU or specificities of generated motions.

Related Work

Pedestrian localisation using a foot-mounted IMU was first introduced in [START_REF] Hutchings | [END_REF]]. Since then, many developments have been made to find alternative ways to accurately localise people. Pedestrian Dead Reckoning (PDR) methods (also known as Personal Navigation Devices), make use of one or more IMU installed on the body of the subject. The main idea of PDR techniques is to integrate inertial measurements with Zero Velocity Update (ZUPT) constraints to reduce errors [Ojeda 2007]. This work is extensively used in IMU-based human localisation works and various fields [Kwanmuang 2015] analyses the gait of a walking person with PDR method to estimate the direction of the shoe, thus the walking direction, and measure stride length. Shoe-mounted IMU is still considered as a possible way to accurately localise persons in an indoor environment due to lower drift errors when compared to bodymounted solutions [Groves 2007] . One way to understand why foot-mounted IMU is preferred to other alternatives may be given by [Groves 2007] comparing bodymounted and foot-mounted based PDR methods. Both systems had similar performances considering the position error, being lower than 10 m for 60 seconds experiments. However, the foot-mounted shown drifts as results were compared to GPS ground truth. We should note that the body-mounted method proved to be usable in not only walking cases but also when the human agent was jogging or running but with a decrease in terms of accuracy.

Various strategies can be considered to improve the localisation results of footmounted IMU navigation. To achieve this goal, one way to consider is the use of one or more IMU and define some special constraints. The main advantage of this choice is that the solution would be easily wearable and thus usable. As shown in [Kourogi 2010] and [START_REF] Panahandeh | [END_REF]], PDR can be used to recognise the action being carried out by the pedestrian through classification methods, but adding this contextual information is also a way to reduce PDR localisation errors ( [Kourogi 2010]). [Wagstaff 2017] is not only using this contextual information thanks to the training of a support vector machine (SVM) classifier using IMU data, but also making efforts on finding optimal zero-velocity detection parameters taking into

Experiments

Two experimental setups are proposed. First, we use the dataset made available in [START_REF] Angermann | [END_REF]] to validate our method on low-drift IMU and compare our results to those attached to the dataset and provided with a state-of-the-art Kalman filter. Our method is applied on several scenarios from this dataset and we present typical results obtained with two representative cases. The first sets of data correspond to a human walking back and forth while the second example is related to a walking pattern describing a eight-shape. Both IMU and motion capture data are provided at 100 Hz.

Then, we will investigate the use of additional sensors to demonstrate the feasibility of fusion strategies using a low-cost IMU running at 1 kHz (Invensense's MPU6050 [InvenSense ]).

Method

Keyframes are created at the beginning and ending of each support phase of the selected foot according to zero-velocity instants detection provided by the dataset (Fig. 3.6). The zero-velocity instants are often called ZUPT standing for 'Zero velocity UPTdate'. Factors active in the graph (see Fig. 3.2) are: initial position, yaw and velocity set to 0, minimal bias drift; and IMU pre-integration. The only constraints applied on the first key-frame are related to the bias magnitude of the IMU and zero velocity constraint factors. All the graph is optimised after each keyframe creation so that these estimates can be used for future estimations with new keyframes. Here key-frames are added at the beginning and the end of each contact phases. Zero velocity constraints are applied as an estimated prior for the optimiser by setting the velocity to 0, meaning that corresponding variables will still be considered as parameters during the optimisation process. In the following experiments, the variance of the velocity is arbitrarily fixed to a non-zero small value. Indeed, if we use a constraint with 0 variance (i.e. the corresponding quantity is not optimised by the solver as a decision variable but is an invariant of the problem) then we must be super confident with this information. Otherwise we would be imposing a constraint inducing a wrong estimate of the trajectory. Finally, we define motion factor. In a first set of results, we used the basic motion prior (minimal velocity and biases variation). In a second time we show the interest of data fusion by emulating a fake sensor that would measure additional data during the flying phase, allowing the creation of an additional key frame between contacts. We use motion capture recording to create this additional key-frame. Such information might be provided using biological evidence on humans (e.g. using the trajectory of the foot during human walking). But it is mostly a proof of concept of similar experience with the humanoid robot, where kinematic information (measured by the encoders) would provide the same information. As in the usual ZUPT-aided inertial navigation, zero-velocity constraints are imposed only on keyframes created during contact phases. Kinematic odometry is also added between all consecutive keyframes for this experiment (see Fig. 3.2). The optimisation part is currently handled by Google Ceres optimiser [Agarwal ] using the sparse structure of the problem.

Results

State estimation using ZUPT only

With only IMU measurements, the system is not provided with enough information to be able to precisely estimate the vertical orientation of the IMU due to its non-observability from the IMU measurements directly. As a consequence, the estimation of the state of the foot is not able to converge to its real value. In other words, the foot trajectory can be recovered up to a rotation around the gravity axis.

Pedestrian Localisation 69

In the following, we manually fixed initial translation and rotation to align the results with the motion-capture reference in order to compare our results against the ground-truth measurements. Our system is able to estimate the state of the system to precision close to the state-of-the-art 9-state Kalman filter [Foxlin 2005] as shown in Fig. 3.8. Furthermore, we can notice that the level of accuracy is similar in both cases. The initial error jump of the kalman estimate is likely to be caused by the initialisation procedure. Indeed, the experiments starts and ends by hitting the floor with the foot the IMU is attached on, thus resulting in high dynamic variations in the IMU and quick change for foot's pose as given by the motion-capture system. This allows to manually shift the data and work with coherent timestamps. However, the data resulting from this synchronisation procedure is not removed for the processing part although it introduces outliers. We performed a very simple outlier rejection by deleting these synchronisation data. Results of bias estimation using another dataset provided by the DLR are given in Fig. 3.9.

Besides, the experiment shows satisfying results in terms of computational times. Indeed, 3.1 seconds are enough to integrate all the data corresponding to a 50 seconds experiment, that is more than 5000 IMU data, to build and to optimise the graph each time a keyframe is created with our framework, with a total of 80 keyframes. It would be hard, if not currently impossible, to get such results using the IMU without a proper pre-integration method. All in all, it takes 98µs in average to read and integrate a single IMU measurement including the computation of jacobians.

State estimation using ZUPT and sensor fusion

The strength of the optimal estimation can also be found in the fusion strategies. Fig. 3.10 shows the reconstructed foot trajectories for the two cases: without and with the flying keyframe information. Adding kinematic information between keyframes enhances the observability of the system and allows to a better estimation. A major advantage of IMU pre-integration theory is the ability to use past pre-integrations corrected with current estimates of keyframes and biases as simply as it takes to integrate a single IMU data. This removes the need of linearisation of intermediate IMU data between previous and current estimates of the state on which the integration must be performed. We also notice that the use of a flying keyframe makes the bias estimation more stable.

Conclusion

We have revised the IMU pre-integration theory [START_REF] Forster | [END_REF]], and proposed an implementation in the quaternions manifold, with simpler derivations than previous works, and with physical interpretations, which we believe go in the direction of improving the clarity of the method. This method has been applied to pedestrian navigation with an IMU attached to the foot and by exploiting available knowledge extracted from the gait phases, such as zero velocity and IMU bias dynamics. As the reference estimated states provided in the dataset and the estimation with our approach. The continuous green line is the ground truth as given by the motion capture system. Bottom: Evolution of Euclidean distance errors between estimates and the ground truth for this 8-shaped walking trajectory.

opposed to usual methods using filtering strategies, we take advantage of the power of nonlinear optimisation techniques based on factor graphs while estimating the pose of the IMU in real-time. Results showed that this estimation method is able to properly estimate the bias, then leading to an accurate odometry where the drift remains reasonable, even after minutes of integration. The method easily extends to additional prior knowledge or additional sensors. Further work is needed to make the system less critical to wrong ZUPT detections. To achieve this goal we can change the ZUPT implementation strategy from a zero prior and arbitrarily low variance to a computation of the variance that depends on the information provided by available sensors.

Experiments on a humanoid robot

Introduction

While the feet of many of our legged robots are equipped with force sensors, we often only have a rough idea of their exact location in space, neither of the dynamic effects acting on them (e.g. flexibility or inertia). Inaccuracy of the foot location, trajectory or friction often results in rapid and unavoidable falls. We consider the problem of accurately estimating the placement of the foot, either during the contact phase (to assert friction and no sliding) or during the flying phase (to compensate for flexibility in the actuation chain). We propose to attach an inertial measurement unit (IMU) directly on the foot in order to reconstruct its position. The main difficulty is then to recover the observability of the IMU location by integrating all previous sensor measurements along with additional constraints coming from the contact and the kinematic chain. A graphical representation approach is used to integrate IMU measurements, kinematic and contact information on a humanoid robot. Contrary to many state estimators recently proposed in the context of legged locomotion, we propose to rely on an information graph representation to handle observability consistency and other constraints. Since an efficient use of the IMU data is of major importance to prevent the graph from growing too much and to use the available information to estimate the biases online, we use the pre-integration methods on manifolds. We validate these concepts by estimating the trajectory of application to localisation the foot of the HRP-2 humanoid robot during various gaits, and show that we are able to accurately reconstruct some subtle effects, such as sliding effects during the contact and flexibility of the kinematic chain.

There are significant differences between this application and the estimation for pedestrian navigation purposes. The first difference lies in the IMU measurements. Indeed, the textile of the shoe the IMU can be attached to can act like a low-pass filter eliminating possible high frequency vibrations. In contrast, the IMU measurements from a sensor attached to the foot of a robot measures the vibrations of the robot. These vibrations are distributed from the different motors to the entire structure of the humanoid robot. These vibrations can turn out to be problematic when considering especially high frequency estimation at up to 1 kHz. Such problems need to be investigated when considering an online experiments working as a standalone application.

Experimental setup

We acquire data on the humanoid robot HRP-2 (Fig. 3.1) during different scenarii with a low-cost IMU placed on its right foot. Before describing the results, we quickly review the properties of the hardware and specify the precise graphical model.

Hardware

The humanoid robot HRP-2

The HRP-2 humanoid robot (Fig. 3.1) is embedded with various sensors including a camera (in the head), an IMU (in the chest) and force and torque sensors in end-effectors. Due to its mechanical design, the robot has flexibilities between each foot and the ankle it is linked to (Fig. 3.11). These flexibilities are used to absorb some impacts during walking phases for example [Kanehira 2002]. The presence of such flexibilities are changing the kinematics of the robot's free flyer defined as the body from which the robot's pose in its environment is described. As a result, the estimation of the foot of the robot using the kinematic chain will not take into account these flexibilities possibly inducing wrong orientation estimates for example.

Besides, humanoid robots are usually considered to be rigid and their interactions with the environment are modelled as rigid contacts. Such a model is convenient on a numerical point of view and enables one to simplify the dynamics so that the control itself is made easier. However, the complete consequences of the introduction of flexibilities on such a complex machine have yet to be understood. It is a possibility that such mechanical specificities, despite the fact that they can be useful, introduce undesired effects. If sensor measurements are one possible source of possible errors, the interactions with the environment can also induce some undesired behaviours such as slippage. Finally, the complexity of the machine itself makes its a control a non-trivial task and a lot of effort is put on improving the robustness and the accuracy of the control to have all the encoders of the robot in a specific configuration. These encoders are used to measure the joint position of each degree of freedom of the robot. Depending on the sensors, the measurements can be either given in an absolute frame or the information can be relative in incremental encoders. The differences have specific implications on the use of the sensors and their accuracy.

Force and torque sensors

The sense of touch is provided to humans through a complex system of nerves under the skin. This information is extremely important in our everyday life to detect contact through forces applied on the skin and thus interact with the environment. The development of a robotic skin is an active field of research but these sensors are not equipped on robots yet. Instead, 6-axis force and torque sensors (FT sensors) are usually embedded in humanoid robots at each contact points that are the feet and the hands. The use of these sensors is important to correct the balance of the robot either during walking or in static phases and more generally to give feedback to the robot about the end effector itself. A 6-axis FT sensor can measure both force and torque along 3 axis. The main technology used in these sensors is based on a deformable material with strain gauges sensitive to deformations. The strain gauges allow to transform the deformation into a variation of electrical resistance. The output of these gauges are only of a few millivolts and the quality of the sensor is strongly impacted by the quality of the conditioning and the amplifier electronics. FT sensors are usually characterised by the range of the measurements they can provide and their resistance to impacts. On the HRP-2 robot, the force sensors at the feet are protected by an elastic element interposed between the sole and the sensors. Finally, FT sensors are also subject to measurement errors. Some of them can also incorporate temperature measurements to compensate for its effect on biases and scaling factors. The relationship between the forces and strain gauges is determined through a calibration processes inducing higher prices.

IMU

We use a MPU6050 6-axis IMU for data acquisition. The IMU is set to provide raw measurements at 1 kHz while mounted on the robot's right foot. IMU data acquisition is triggered by a STM32 microcontroller through I2C connection at 400 kHz. Finally, the microcontroller sends timestamped data to the computer via a USB connection (Fig. 3.12).

In order to investigate a simple and non-invasive estimation method, we simply attach the MPU6050 IMU on the top of the right foot of the robot. Some consequences arise from this choice. First of all, the transformation between the point in the robot considered as the foot end effector in the kinematic chain and the pose of the IMU is unknown. However, we know that this transformation is fixed and we can neglect this point in our experiments since we are more interested in comparing the pose estimations as given by the proposed method and through forward Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation kinematics estimation.

Method

Using the IMU and the force sensor Since further effort would be required to run the estimation method online directly on the robot in an integration point-of-view into the robot's system, the estimation parts are realised offline. Timestamps of IMU and force sensors data are synchronised manually by hitting slightly the foot several times before the experiments. It is then possible to visualise the chocs on the IMU data and in the force and torque measurements to manually shift a set of measurements.

The forward kinematics estimates are provided using Pinnochio [Carpentier 2018] which is a C++ library for efficient rigid multi-body dynamics computations developed by the GEPETTO Team at LAAS-CNRS. The use of a motion capture (MOCAP) system is considered to provide ground truth displacement measurements and from one point to another.

Forward Kinematics on HRP-2 Robot

Since the experiments are conducted offline, we deduce the kinematic constraint to be added in the factor graph from robot odometry. This odometry can be built using forward kinematics. Standard forward kinematic methods propagate the joint angle information through the kinematic chain of the robot from the free flyer to estimate the position of any joint of the robot in space. To this end we use Pinocchio software implementing these methods explained with more details in [Featherstone 2014]. However, in our case, the free-flyer of the robot is not estimated although the robot moves. Hence if we simply use this method through time to reconstruct the position of the foot then the results would be the same as if the robot walked in the air while its base was fixed. As a consequence, we estimate the position of the robot's right foot with respect to to the left foot and versa while fixing the position of the support foot. The modification of the support foot is realised when a foot was in the air then finally hits the ground. The determination of the new support phase is realised through a hysteresis filter based on the values measured by force sensors in both feet.

Key-frame creation policy

The IMU is used along with the force sensors and the foot pose estimation through the kinematic chain to determine instants with zero velocity. This approximation is possible due to the fixed transformation between the IMU and the rigid body (i.e. the foot in our case). Finally, we can investigate several key-frame creation policies depending on the chosen strategy. Fig. 3.13 and Fig. 3.14 show a possible solution given the hypothesis that the slippage occurs during the contact phase when the foot hits the ground. Thus the hypothesis of reliable kinematic reconstruction before hitting the ground can be used to create an odometry constraint during the foot's flying phase.

Scenarii under investigation

We describe hereafter the different scenarii the proposed method is evaluated in.

Walking in place

The first motion investigated is a pattern test that is used for calibration purposes. The input control sequence is here supposed to make the robot walk in place while the position and orientation of the feet are not supposed to change. In practice, the robot is not able to follow precisely the desired trajectory and slight changes in both position and orientation can be observed (Fig. 3.15). Although the cause for this error is not completely identified, we suspect that the inner stabiliser running in the robot indirectly changes the trajectory of the robot. This effect may be intensified by small differences between the kinematic model of the robot and its actual design. Fig. 3.16 and Fig. 3.18 provide estimated states for the IMU with respect to time during the experiment. These figures are to be interpreted along with Fig. 3.15 to understand the results. Indeed, Fig. 3.16 highlights possible drifts for P x and P y but a P z estimate that remains close to the initial pose of the IMU. This behaviour is coherent with the experiment itself as it can be seen in Fig. 3.15. The final pose of the foot is slightly different of the initial one (for both position and orientation). The difference between both poses can be interpreted as a translation in X and Y axis and a rotation around the tilt, i.e. Oz axis. The change in Oz orientation is also observed through the estimation of the IMU states in Fig. 3.18 while both Ox and Oy orientations ultimately remain unchanged when comparing the initial and final states. It is also relevant to consider the change in the estimated orientation as it is particularly visible on the Oy estimate of Fig. 3.18 during full contact phases considering that the foot is supposed to be still. One hypothesis that may explain this change is to consider wrong bias estimates. There are several properties that could strengthen this hypothesis. First of all we should note that the foot gait cycle does not imply any rotation along this Oy axis. This specificity may render the bias estimation hard due to non-excited axis in the IMU and hence no full-observation of the dynamics of the sensor. Another property may be the use of the zero-velocity constraints on the states of the IMU. The variance of this constraint needs to be small enough to actually add some relevant information in the graph and use it to characterise the biases. However, setting the variance wrongly to a too small value may cause undesired behaviours on the system (including the estimation of the biases in our specific case). The zero-velocity constraint as it is usually defined is a constraint on the instantaneous velocity of the IMU at timestamp t. The use of this specific zero-velocity constraint on the state of the IMU is problematic due to mechanical vibrations measured by the IMU and propagated through the structure of the robot itself. These vibrations should not be ignored and are visible in Fig. 3.16.

Walking forward

The second experiment that is considered is a walking forward motion on a plane Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation floor. Again, observations show differences between the desired and actual final positions and thus in the motion generated by HRP-2. Currently used methods tend to use visual perception in order to relocalise the robot in its environment and correct this drift due to the motion [START_REF][END_REF][START_REF] Hartley | [END_REF]]. However, a simpler high rate estimation method would be beneficial to close the control loop of the robot locally. It could also be used to observe motions that cannot be estimated by the camera in the head of the robot or by the kinematic chain relying on the model of the robot and where all intermediate joints between the end-effector and the base of the robot are possible sources of errors.

Given results in Fig. 3.19 and Fig. 3.20, we can note that it is feasible to estimate the motion of the IMU. P x and P y are repectively slightly under 1 and above 0 metre. This is due to rotations of the robot during walking around the yaw axis. However, P z should be closer to 0. Since our motion capture system is not able to correclty track the pose of the foot or of the IMU itself, it is difficult to compare those results to a ground truth. In Fig. 3.20 one can see again that the IMU is affected by vibrations due to the motion (hitting the floor).

From previous experiments (i.e. walking in place and forward), one can already raise some concerns about installing the IMU on the end-effector of the robot and estimate its pose. Although the problems may be due to the use of the zero-velocity constraint, the pose estimation is giving coherent results. It would be interesting to test the same approach used until there on advanced experiments. However, some strategies to bypass the use the zero-velocity constraints need to be investigated.

Due to time constraints and mechanical failure in the HRP-2 robot, these two following experiments (i.e. walking alonf the quarter of a circle and climbing stairs) could not be tested. However, they still remain interesting and raise specific challenges in practice.

Walking along the quarter of a circle

In an attempt to visualise the effect of slipping on the robot's motion, we intended to evaluate the proposed method while the robot walks describing the quarter of a circle. This specific motion induces specific constraints on the robot due to the rotation part in the movement. Preliminary experiments have thus shown important slipping phases. Whereas these phases cannot be observed with methods using odometry and forward kinematics only, the use of IMUs may be an interesting alternative to overcome the estimation problem in this context. This experiment is thus a way to characterise how interesting the proposed solution would be to face problems that are due to the dynamics of the motion and the rigidity of the robot. Furthermore, this experiment is a first step towards the design of a better slippage detection method.

Climbing stairs

Finally, we intended to investigate the foot pose estimation while HRP-2 is set to climb stairs using a handrail. This is a complex motion with slippage phases and possible undesired contacts with the environment. These contacts can be due to not only the initial pose of the robot but also to its ability to follow the desired trajectory (e.g. the foot of the robot hitting the stair). The previous experiment was to test the slippage detection using data from the IMU, force sensors and the kinematic chain. This last scenario is more challenging since it implies not only slippage on the stairs (Fig. 3.1), but also collisions that should be detected by the three main sensors we use.

These last experiments may conduct to further studies amongst which one example is given hereafter. Indeed, the time window corresponding to the slippage is very short and it will be challenging to run the detection in realtime with sensors providing data at very different rates. Thus, it might be necessary to run the detection on past data and review the keyframe creation policy. This study would be a way to isolate the data corresponding to the slippage and still benefit from the velocity estimation and kinematic odometry information in the other parts of the trajectory for a more accurate estimation. Then right foot moves and we can create a key-frame while during the swing phase by using the robot's kinematic chain. Then the foot of the robot hits the ground, slippage may take place, thus, given the information from the sensors, we can decide whether to add a kinematic constraint between key-frames 1 and 2 or not. While the left foot moves, the right foot is supposed not to move. This case depicts the decision not to create a kinematic factor between key-frames 1 and 2. The key-frame created while the foot was in the air does not have any constraint on the velocity. We can notice that P x and P y drift away from 0. The final estimate along P z axis is close to 0, which is coherent taking into account that the robot is walking on a flat floor. The variance of the zero-velocity constraint being fixed manually to a small value, its evolution is not represented here.

Figure 3.17: Position estimation of the foot of the HRP-2 robot while walking in place with kinematic odometry. Although it is difficult to evaluate the estimation of the biases, the stability of both accelerometer and gyrometer biases may tend to give the impression of a reliable estimate. However, this stability is due also to constraints imposed over the evolution of the biases through time. Furthermore, we note that the Oy orientation is slowly drifting during contact phases despite the fact that the foot is supposed to be still. Each diamond on the graphs represents a key-frame while bars under and over provide a way to visualise the 2σ bounds around the estimates. The stability of both accelerometer and gyrometer biases may tend to give the impression of a reliable estimate. However, this stability is due also to constraints imposed over the evolution of the biases through time. Furthermore, we note that the Oy orientation is slowly drifting during contact phases despite the fact that the foot is supposed to be still. We can also clearly see the effect of the vibrations on the IMU on this graph. The estimated value for a by (-0.55m/s 2 ) is also too big. Hence some doubts remain on the estimation of the biases. We have here one possible explanation for the estimation of the biases given the motion described by the robot and the dynamics applied on the IMU.

Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

Introduction

There has been a remarkable effort in the last years in the robotics community to formulate estimation problems properly. This is motivated by an increasing demand for precision, consistency and stability of the solutions. Indeed, a proper modelling of the states and measurements, the functions relating them, and their uncertainties, is crucial to achieve these goals. This has led to designs involving what has been known as 'manifolds', which are no less than the topologic surfaces of the Lie groups where the state representations evolve. Relying on the Lie group theory we are able to construct a rigorous calculus corpus to handle uncertainties, derivatives and integrals with precision and ease. Typically, these works have focused on the wellknown manifolds of rotation SO(3) and rigid motion SE(3). Higher-dimension manifolds such as the IMU states have been treated as composites, though this has not been stated this way explicitly.

When being introduced to Lie groups for the first time, it is important to try to regard them from different points of view. The topological viewpoint, see Fig. 4.1, involves the shape of the manifold and conveys powerful intuitions of its relation to the tangent space and the exponential map. The algebraic viewpoint involves the group operations and their concrete realization, allowing the exploitation of algebraic properties to develop closed-form formulas or to simplify them. The geometrical viewpoint, particularly useful in robotics, associates group elements to the position, velocity, orientation, and/or other modifications of bodies or reference frames. The origin frame may be identified with the group's identity, and any other point on the manifold represents a certain 'local' frame. By resorting to these analogies, many mathematical abstractions of the Lie Theory can be brought closer to intuitive notions in vector spaces, geometry, kinematics and other more classical fields.

In this work, we explore the lessons learned from the development of the IMU preintegration method presented in Section 3.3 and extend some solutions to the usage of any kind of manifold. We purposely take an approach that escapes from the mathematical language of the Group Theory and approaches concepts, terminologies and notations of more widespread use in robotics. The reader interested in more mathematically precise concepts may find this unfortunate, perhaps even inconvenient, but we believe that a good way to familiarise roboticists with such theoretical concepts is through a shared and familiar language. Excellent references one can consult for more detailed explanations are [Chirikjian 2012] and [Eade 2013]. We believe this presentation is novel and interesting. It presents the different Lie Group elements in a way similar to classical linearisation through Jacobians paradigms. This alone we consider a relevant contribution. This work is closely related to Chapter 3 since the bias estimation of the IMU sensor can now be handled as a self-calibration function, even for dynamic parameters. Furthermore, through this generalisation, we present a way to compute Jacobians using the chain rule exposed in 4.2.3.2. It should also be noted that the presented algorithm as well as all the algebra related to deltas remains the same. For new sensors cases, only the The Lie algebra T M E (red plane) is the tangent space to the Lie group's manifold M (here represented as a blue sphere) at the identity E. Through the exponential map, each straight path vt through the origin on the Lie algebra produces a path exp(vt) over the manifold which runs along the respective geodesic. Conversely, each element of the group has an equivalent in the Lie algebra. This relation is so profound that (nearly) all operations in the group, which is curved and nonlinear, have an exact equivalent in the Lie algebra, which is a linear vector space. Though the sphere in R 3 is not a Lie group (we just use it as a representation that can be drawn on paper), that in R 4 is, and describes the group of unit quaternions -see Fig. 4.3. sensor model equations need to be rewritten. Finally, we introduce the idea of composite manifold for convenience since the IMU state is expressed in such manifold, though this has not been stated this way explicitly.

TM E E exp(τ 1 ) exp(τ 2 ) τ 2 τ 1 M vt exp(vt) log(X 3 ) X 3
The main contributor of this work is Joan Solà with the help of Jérémy Deray and I. My personal contribution here lies in the development of the preintegration method explained in Section 3.3 and help provided for the integration of the methods in WOLF C++ library as well as an experimental validation using an IMU.

Manifold tools for robotics 4.2.1 Definition of manifold and some properties

'Manifold' (or 'Lie Group')

In robotics we speak of 'manifold' to mean a Lie group. In mathematics, a group (G, •) is a set, G, with a composition operation, •, that satisfies the four group Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

T M (X ) M M T M (X ) X X
Associativity : (X • Y) • Z = X • (Y • Z) (4.2) Identity E : E • X = X • E = X (4.3) Inverse X -1 : X -1 • X = X • X -1 = E . (4.4) for X , Y, Z ∈ G.
A Lie group is a group that is also a differentiable (i.e. smooth) manifold. The reader should be able to visualise the idea of manifold (Fig. 4.2): it is like a bent, smooth (hyper)-surface within a space of higher dimension, like e.g. the manifold of unit quaternions, which is a spherical 3-manifold in a 4-dimensional space (Fig. 4.3). In robotics, we say that our state vector evolves in this surface, that is, the manifold describes or is defined by the constraints imposed on the state.

The smoothness of the manifold implies the existence of a unique linear space tangent to any point of it: given X a point in a manifold M, we name T M (X ) the vector space tangent to M at X (Fig. 4.2). This is known as the Lie algebra of M. The dimension m of M is the dimension of T M . The elements of T M are non-trivial (skew-symmetric matrices, imaginary numbers, pure quaternions) but the key aspect for us is that they can be expressed as linear combinations of some base elements (called generators). For example, for

[ω] × ∈ so(3) we have [ω] × = 0 -ωz ωy ωz 0 -ωx -ωy ωx 0 = ω x 0 0 0 0 0 -1 0 1 0 +ω y 0 0 1 0 0 0 -1 0 0 +ω z 0 -1 0 1 0 0 0 0 0 ≃ ωx ωy ωz ∈ R 3 .Therefore T M is isomorphic to the Cartesian space R m -one writes T M ≃ R m .
A few examples of manifold are detailed in Table 4.1 on page 114.

The group actions

Importantly, Lie groups come with the power to transform elements of other sets, producing e.g. rotations, translations, scalings, and combinations of them. These are extensively used in robotics, both in 2D and 3D.

θ q S 3 R 3 q =exp(θ) θ = log(q) S 3 R 3 θ q S 3 R 3 θ q = p ⊕ θ θ = q p q p Figure 4.3:
The S 3 manifold is a unit 3-sphere in R 4 (blue) where the unit quaternions q = 1 live. The tangent space T M is isomorphic to the hyperplane R 3 (red grid). The centre and right figures show a side-cut through the plane (dashed) defined by the tangent vector θ (red segment) and the sphere centre. Mappings exp and log (arrows) map elements of R 3 to/from elements of S 3 (blue arc). The ⊕, ⊖ operators express increments between quaternions.

Given a Lie group M and a set V, we note

X • v the action of X ∈ M on v ∈ V, • : M × V → V ; (X , v) → X • v . ( 4.5)
For • to be a group action, it must satisfy the axioms,

Identity :

E • v = v (4.6) Compatibility : (X • Y) • v = X • (Y • v) . (4.7)
Common examples are the groups of rotation matrices SO(n), the group of unit quaternions, and the groups of rigid motion SE(n). Their respective actions on vectors satisfy

SO(n) : rotation matrix R • x Rx SE(n) : Euclidean matrix H • x Rx + t S 1 : unit complex z • x z x S 3 : unit quaternion q • x q x q *
The group composition (4.1) may be viewed as an action of the group on itself, • : M × M → M. Another interesting action is the adjoint action, which we will see in 4.2.2.3.

The tangent spaces and the Lie algebra

Given X (t) a point moving on a Lie group's manifold M, its velocity Ẋ = ∂X /∂t belongs to the space tangent to M at X (Fig. 4.2), which we note T M X . The smoothness of the manifold, i.e., the absence of edges or spikes, implies the existence of a unique tangent space at each point. The structure of such tangent spaces is Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration z . In the case of T S1 z , the velocity is ż = z iω = -ω sin ωt + iω cos ωt when expressed in the global coordinates, and z v ∧ = iω when expressed locally. Their relation is given by z v ∧ = z -1 ż = z * ż. In the case of T S 1 1 , this relation is the identity 1 v ∧ = ż = iω. Clearly, the structure of all tangent spaces is iR, which is the Lie algebra. This is also the structure of ż at the identity, and this is why the Lie algebra is defined as the tangent space at the identity. the same everywhere.

S 1 1 z(t) T S 1 1 = iR T S 1 z z v ∧ = iω ∈ iR ż = z • iω / ∈ iR 1 v ∧ = iω ∈ iR ż = iω ∈ iR ωt

The Lie algebra m

The tangent space at the identity, T M E , is called the Lie algebra of M, and noted m,

Lie algebra :

m T M E . (4.8)

Every Lie group has an associated Lie algebra. We relate the Lie group with its Lie algebra through the following facts [Eade ] (see Fig. 4.1):

• The Lie algebra m is a vector space. 1 As such, its elements can be identified with vectors in R m , whose dimension m is the number of degrees of freedom of M.

• The exponential map (also called retract in the following), exp : m → M, exactly converts elements of the Lie algebra into elements of the group. The log map (also called lift in the following) is the inverse operation.

• Vectors of the tangent space at X can be transformed to the tangent space at the identity E through a linear transform. This transform is called the adjoint.

Lie algebras can be defined locally to a tangent point X , establishing local coordinates for T M X (Fig. 4.4). We shall denote elements of the Lie algebras with a 'hat' decorator, such as v ∧ for velocities or τ ∧ = (vt) ∧ = v ∧ t for general elements. A left superscript may also be added to specify the precise tangent space, e.g.,

X v ∧ ∈ T M X and E v ∧ ∈ T M E .
The structure of the Lie algebra can be found by time-differentiating the group constraint (4.3). For multiplicative groups this yields the new constraint X -1 Ẋ + Ẋ -1 X = 0, which applies to the elements tangent at X (the term Ẋ -1 is the derivative of the inverse). The elements of the Lie algebra are therefore of the form,

2 v ∧ = X -1 Ẋ = -Ẋ -1 X . (4.9)
The Cartesian vector space R m The elements τ ∧ of the Lie algebra have nontrivial structures (skew-symmetric matrices, imaginary numbers, pure quaternions, see Table 4.1) but the key aspect for us is that they can be expressed as linear combinations of some base elements E i , where E i are called the generators of m (they are the derivatives of X around the origin in the i-th direction). It is then handy to manipulate just the coordinates as vectors in R m , which we shall note simply τ . We may pass from m to R m and vice versa through two mutually inverse linear maps or isomorphisms, commonly called hat and vee, (4.11) with e i the vectors of the base of R m (we have e ∧ i = E i ). This means that m is isomorphic to the vector space R m -one writes m ≃ R m , or τ ∧ ≃ τ . Vectors τ ∈ R m are handier for our purposes than their isomorphics τ ∧ ∈ m, since they can be stacked in larger state vectors, and more importantly, manipulated with linear algebra using matrix operators. In this work, we enforce this preference of R m over m, to the point that most of the operators and objects that we define (specifically: the adjoint, the Jacobians, the perturbations and their covariances matrices, as we will see soon) are on R m .

Hat : R m → m ; τ → τ ∧ = m i=1 τ i E i (4.10) Vee : m → R m ; τ ∧ → (τ ∧ ) ∨ = τ = m i=1 τ i e i ,

Manifold maps and operations

We have the following maps relating R m , T M and M,

R m ≃ -⇀ ↽ - ≃ T M (E) retr(•) ----⇀ ↽ ---- lift(•)
M , (4.12)

where ≃ are linear isomorphisms, and lift(•) and retr(•) map the manifold elements to/from the tangent space (see lift and retract below). For simplicity, and since we 94 Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration do not require here the concepts and tools proper of the Lie algebra, we refer to T M as the tangent space but express its elements in its isomorphic Cartesian space R m , so that the maps T M ⇋ M are implemented algebraically as R m ⇋ M.

Lift and retract

Lift and retract map elements X ∈ M with elements τ ∈ T M (E). Retract is the operation of wrapping the tangent space onto the manifold (as when wrapping a ball with a paper), while lift is the unwrapping operation. For X ∈ M and τ ∈ T M (E), we say that lift :

M → T M (E) ; X → τ = lift(X ) (4.13) retr : T M (E) → M ; τ → X = retr(τ ) . (4.14)
In the rigid motion groups that we consider (see Table 4.1 on page 114 and Fig. 4.3), lift() and retr() correspond respectively to the log() and exp() maps.

Plus and minus

Plus and minus allow us to introduce differences between elements of a (nonlinear) manifold, and express them in its (linear) tangent space. Denoted by ⊕ and ⊖, they combine one lift/retract operation with one composition. Because of the noncommutativity, they are defined in right-and left-versions (see Fig. 4.3-right for the right version), right-plus:

X = X ⊕ τ r X • retr(τ r ) ∈ M (4.15) right-minus: τ r = X ⊖ X lift(X -1 • X ) ∈ T M (X ) (4.16) left-plus: X = τ l ⊕ X retr(τ l ) • X ∈ M (4.17) left-minus: τ l = X ⊖X lift( X •X -1 ) ∈ T M (E) (4.18)
Because in (4.15) retr(τ r ) appears at the right-hand side of the composition, τ r belongs to the tangent space at X (see (4.16)): we say that τ r is expressed in the local frame at X . Conversely, in (4.17) retr(τ l ) is on the left and we have τ l ∈ T M (E): we say that τ l is expressed in the global frame. Notice that while left-and right-⊕ are distinguished by the operands order, the ⊖ notation in (4.16) and (4.18) is ambiguous. It is typical to express perturbations locally and therefore we use the right-forms of ⊕ and ⊖ by default. The derivative is simply lim τ →0 σ/τ .

M N T M (f (X )) T N (X ) X f (X ) f : N → M X 7 ! f (X ) τ X ⊕ τ f (X ⊕ τ ) σ = f (X ⊕ τ ) f (X ) σ ⊕ f (•)

Adjoint

The adjoint linearly relates vectors from the tangent space at a point X to vectors of the tangent space at the origin. This may be written with these equivalent forms,

X ⊕ τ = (Adj X τ ) ⊕ X (4.19) X • retr(τ ) = retr(Adj X τ ) • X (4.20) Adj X τ = lift(X • retr(τ ) • X -1 ) (4.21)
Observe that from Eq. (4.15, 4.17, 4.19) we have τ l = Adj X τ r .

Action

We note with X • v the action of elements X ∈ M on elements v of another set V, having

(X • Y) • v = X • (Y • v) . (4.22)
For example, for vector rotation we have (QR)v = Q(Rv).

Derivatives on manifolds

Definition of derivatives of functions

Departing from the standard derivative definition, which applies to vector spaces,

3 f : R n → R m , ∂f (x) ∂x lim h→0 f (x + h) -f (x) h ∈ R m×n ,
3 We use a compact notation in order to handle the n-dimensional infinitesimal h. The notation establishes each column i of the Jacobian matrix of derivatives as Ji = lim h→0

f (x+he i )-f (x) h
, where h is a scalar and ei is the i-th vector of the canonical basis of R n .

Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

we can now use our plus and minus operators to define derivatives of functions acting on manifolds (see Fig. 4.5), 4

f : N → M, ∂f (X ) ∂X lim τ →0 f (X ⊕ τ ) ⊖ f (X ) τ ∈ R m×n . (4.23)
Notice that τ ∈ T N ≃ R n , and the numerator of the derivative belongs to T M ≃ R m . This derivative is then a proper Jacobian matrix R m×n linearly mapping the spaces T N (X ) → T M (f (X )). Remark that whenever the function f passes from one manifold to another, the plus and minus operators must be selected appropriately: plus for the domain N , and minus for the codomain or image M. For small values of τ , the following linear approximation holds,

f (X ⊕ τ ) ---→ τ →0 f (X ) ⊕ ∂f (X ) ∂X τ ∈ M . (4.24)
With ( 4.23), we can easily compute any derivative from the partial derivative blocks of inversion, composition, retraction and action defined hereafter.

The inverse derivative block, i.e. for τ = X -1 , is defined with (4.23) as

J X -1 X ∂X -1 ∂X ∈ R m×m . ( 4.25)
while the composition derivative block is defined as

J (X •Y) X ∂X • Y ∂X ∈ R m×m (4.26) J (X •Y) Y ∂X • Y ∂Y ∈ R m×m . (4.27)
We define the right Jacobian of M as the derivative of retr(), i.e., for τ ∈ T

M (E) ≃ R m , J r (τ ) ∂ retr(τ ) ∂τ ∈ R m×m , ( 4

Manifold tools for robotics 97

lift, i.e. for τ = lift(X ), is defined as

J lift(X ) X lim δτ →0 lift(X ⊕ δτ ) -lift(X ) δτ = lim δτ →0 lift(retr(τ ) retr(δτ )) -τ δτ = lim δτ →0 τ + J -1 r (τ ) δτ -τ δτ = J -1 r (τ ) = J -1 r (lift(X )) . (4.32)
Finally, noting X • v the action of X ∈ M on v ∈ V, we define with the jacobian of the group action (4.23) as

J X •v X ∂X • v ∂X , J X •v v ∂X • v ∂v (4.33)
and all these derivatives can be used with the chain rule detailed hereafter.

Using the chain rule to compute other derivatives

The derivatives such as those defined above fulfil the chain rule according to which, the composition of functions allows to compute derivatives by multiplying derivatives in a certain way.

Lemma 4.2.1. Let's define functions Y = f (X ) and Z = g(Y) so that Z = g(f (X )). These functions have the following derivatives respectively: J Y X , J Z Y and J Z X . Then, according to the chain rule,

J Z X = J Z Y J Y X .
Proof. The validity of this chain rule can be proven in the case J Z X = J Z Y J Y X using (4.24) twice and the identity X ⊕ τ ⊖ X = τ , which leads to The chain rule can be seen as a main tool making the expression of Jacobians much clearer and their computation easier. As an illustrative example, the derivatives of e.g. Z = X • f (Y ⊕ τ ) can be given through simple application of the chain rule as,

J Z X J g(f (X )) X lim τ →0 g(f (X ⊕ τ )) ⊖ g(f (X )) τ [(4.24) on f (X )] = lim τ →0 g(f (X ) ⊕ J Y X τ ) ⊖ g(f (X )) τ [(4.24) on g(Y)] = lim τ →0 g(f (X )) ⊕ J Z Y J Y X τ ⊖ g(f (X )) τ = lim τ →0 lift((g(f (X ))) -1 • g(f (X )) • retr(J Z Y J Y X τ )) τ = lim τ →0 lift(retr(J Z Y J Y X τ )) τ = lim τ →0 J Z Y J Y X τ τ = J Z Y J Y X . ( 4
J Z X = J (X •f (Y⊕τ )) X J Z Y = J (X •f (Y⊕τ )) f (Y⊕τ ) J f (Y⊕τ ) Y⊕τ J Y⊕τ Y J Z τ = J (X •f (Y⊕τ )) f (Y⊕τ ) J f (Y⊕τ ) Y⊕τ J Y⊕τ τ ,
where J f (X ) X must be derived from (4.23), and all other partial derivative blocks are defined in 4.2.3.1. Derivative forms for ⊕ and ⊖ can be easily derived from the partial derivative blocks and are defined respectively as

J X ⊕τ X = J X •(retr(τ )) X (4.35) J X ⊕τ τ = J X •(retr(τ )) retr(τ ) J retr(τ ) τ = J X •(retr(τ )) retr(τ ) J r (τ ) (4.36)
for ⊕ and

J Y⊖X X = J lift(X -1 •Y) (X -1 •Y) J (X -1 •Y) X -1 J X -1 X (4.37) J Y⊖X Y = J lift(X -1 •Y) (X -1 •Y) J (X -1 •Y) Y , ( 4.38)
for ⊖. Once these forms or 'blocks' are found, all other derivatives follow by the chain rule. Furthermore, the Jacobians are also needed to take into account the effect of uncertainties in manifold through covariance propagation.

Uncertainty in manifolds, covariance propagation

Perturbations around a point X ∈ N are defined in the linear space T N ( X ), that is, for a local perturbation τ ,

X = X ⊕ τ , τ = X ⊖ X ∈ T N ( X ) . (4.39)
Covariances matrices can be properly defined on this tangent space at X through the standard expectation operator

E[•], cov(X ) E[(X ⊖ X )(X ⊖ X ) ⊤ ] ∈ R n×n . (4.40)
Notice that since the dimension n of T N matches the degrees of freedom of N , these covariances are well defined.5 Covariance propagation through a function f : N → M; X → f (X ) just requires the manifold Jacobian matrices (4.23) and

X 0 X 3 X 4 δ 3 δ 1 τ 1 τ 2 τ 3 τ 4 δ 2 M X 1 X 2 δ 4 M T M (X 0 ) τ 1 δ 1 Figure 4
.6: Motion integration on a manifold. Each motion data produces a step τ k ∈ T M (X k-1 ), which is retracted to a local motion increment or 'delta' δ k ∈ M, and then composed with

X k-1 to yield X k = X k-1 • retr(τ k ) = X k-1 • δ k ∈ M.
regular linear covariance propagation,

cov(f (X )) ≈ ∂f ∂X cov(X ) ∂f ∂X ⊤ ∈ R m×m . (4.41)
Perturbations can also be expressed in the global reference, that is, in the tangent space at the origin T N (E). Using left-plus and minus,

X = τ ⊕ X , τ = X ⊖ X ∈ T N (E) . (4.42)
This allows global specification of covariance matrices using left-minus in (4.40).

For example, a 3D orientation that is known up to rotations in the horizontal plane can be associated to a covariance Q = Diag(σ 2 φ , σ 2 θ , ∞) only if Q is specified in the global reference.

Discrete integration on manifolds

We can construct a sound integration of a sequence of (small) steps τ 1 , • • • , τ k ∈ T M onto the manifold (Fig. 4.6), i.e., X k X 0 ⊕τ 1 ⊕• • •⊕τ k , which we write in recursive form,

X k = X k-1 ⊕ τ k = X k-1 • retr(τ k ) . (4.43)
Notice here that the time-derivative of a point moving on a manifold lies precisely on the tangent space, so this space is sometimes called the velocity space. Thus, if we are to integrate discrete velocity data, say v k , then τ k = v k δt is a proper step in the tangent space. Common examples are the integration of 3D angular rates ω into the rotation matrix, R k = R k-1 exp([ω k δt] × ), or into the quaternion,

q k = q k-1 ⊗ exp(ω k δt/2).
Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

Composite manifolds

One can consider more complex manifolds as composites. Take as example the space of translations and rotations. We have for this the well-known SE(n) manifold of rigid motions, but we can also construct the R n × SO(n) manifold, which we call a composite of the (trivial) R n and the SO(n) manifolds. These are very similar, but not equivalent.

Composite manifolds may employ operators retract, lift, plus and minus that act either on the whole manifold, or on each of its components. Take R 3 × SO(3) as an example, with manifold elements S = (p, R) and tangents τ = (δp, δθ). For motion integration, it is beneficiary to use the whole manifold, as the different parts get conveniently coupled,

S ⊕ τ p + Rδp R exp(δθ) (4.44) S 2 ⊖ S 1 R ⊤ 1 (p 2 -p 1 ) log(R ⊤ 1 R 2 ) (4.45)
On the contrary, for differentiation, error evaluation and uncertainty management, it is usual to take each manifold block separately, i.e.,

S τ p ⊕ δp R ⊕ δθ = p + δp R exp(δθ) (4.46) S 2 S 1 p 2 ⊖ p 1 R 2 ⊖ R 1 = p 2 -p 1 log(R ⊤ 1 R 2 )
, (4.47) where we mark the block-operation of plus and minus with , . It is convenient to realise that these operators are indeed simpler than the regular plus and minus (4.44, 4.45).

The consequence of these considerations is that new derivatives can be defined, .48) With this derivative, Jacobian matrices of functions acting on composite manifolds can be determined in a per-block basis, which yields simpler expressions,

∂f (X ) ∂X lim τ →0 f (X ⊕ τ ) f (X ) τ . ( 4
∂f ∂X =     ∂f 1 ∂X 1 • • • ∂f 1 ∂X M . . . ∂f 1 ∂X N • • • ∂f N ∂X M     (4.49)
where M, N are respectively the number of composite blocks of X ∈ M and f (X ) ∈ N . Although never explicitly stated, this is the approach taken in many IMU works on manifolds, for example [START_REF] Forster | [END_REF]]. We present this concept of composite manifold for convenience, since this makes it possible to unify the methods presented

Graph-based motion estimation on manifolds

In graph-based motion estimation (as in e.g. odometry and SLAM, Fig. 4.7) we often have very different rates of motion sensor data and keyframe (KF) creation. Therefore, hundreds of motion measurements need to be integrated to generate a motion factor linking two consecutive KFs. It is convenient to integrate a relative motion or 'delta' ∆ ij with respect to the last KF with state x i (Fig. 4.8), so as to have, given some composition operator ⊞, (4.50) and use it to generate a relative motion factor linking x i to the next KF, i.e., through an error e ij so that

x j = x i ⊞ ∆ ij ,
e ij = ∆ ij ⊖ (x j ⊟ x i ) . (4.51)
The motion increment ∆ ij is found by integrating small motion contributions at the sensor data rate, i.e., at each arrival of the motion data y k , we do

∆ ik = ∆ ij ⊞ δ k with δ k = f (y k ) . (4.52)
This appears rather straightforward. But only in the most basic cases these operations are trivial. In many cases, the motion data comes from uncalibrated sensors, either in its extrinsic or intrinsic parameters. Some of these parameters may additionally drift with time, thus requiring continuous tracking. Moreover, in some complicated cases (e.g. IMU) the integrated deltas strongly depend on the initial conditions of orientation and velocity. So the general situation becomes,

∆ ik (x i , c i ) = ∆ ij (x i , c i ) ⊞ f (y k , x i , c i ) , ( 4.53)
where c i are (possibly time-varying) calibration parameters to be jointly estimated alongside the states x i . In all these cases, the changes in the estimates of x i and c i (inherent to the iterative nature of the optimisation) affect the whole motion integral. To avoid the need of re-integrating all data at each iteration of the optimisation algorithm, the delta pre-integration theory was developed for the IMU sensor [Lupton 2009[START_REF] Forster | [END_REF]]. On the one hand, this theory defines the deltas independently of the initial conditions of orientation and velocity, thus depending only on sensor data y and calibration parameters c. These are pre-integrated once, parameters is linearised so that the deltas can be corrected a posteriori, i.e., when computing the residual, using pre-integrated Jacobians and (4.24),

∆ ik (c i ) = ∆ ij (c i ) ⊞ f (y k , c i ) .
∆ ij = ∆ ij (c i ) ⊕ J ∆ ij c i (c i -c i ) . (4.55)
We generalise the pre-integration theory to any kind of motion sensor. We provide several contributions:

1. a segmentation of the computation pipeline (from measurements, to body magnitudes, to the current delta, and to the integrated delta);

2. a physical interpretation of the delta magnitudes, defined in a manifold;

3. a simple yet rigorous algebraic approach, valid for any type of manifold, which takes profit of the pipeline segmentation and the chain rule to compute the otherwise cumbersome [START_REF] Forster | [END_REF]] Jacobians. Important complements are provided in Subsection 4.2.2 and Section C.1 for the sake of background and completeness.

Notation

We note the deltas manifold as D, with dimension d. We define the integrated delta ∆ ij ∈ D and the current delta δ k ∈ D, with time indices defined as in Fig. 4.8. We define the additive and subtractive compositions on D as

∆ ij ⊞ ∆ jk ∆ ij • ∆ jk (4.56) ∆ ik ⊟ ∆ ij ∆ -1 ij • ∆ ik . ( 4
t i t j t k ∆ ij δ k t x j x k x i Figure 4
.8: The pre-integrated delta ∆ ij ∈ D contains all motion from the last KF at time i, up to time j, so that x j = x i ⊞ ∆ ij . The current delta δ k ∈ D contains the motion from times j to k, computed from the last motion measurement at time k, so that

x k = x j ⊞ δ k = x i ⊞ ∆ ij ⊞ δ k = x i ⊞ ∆ ik .
Similarly, we use the same symbols to define compositions between states x i and deltas ∆ ij so that (4.59) and notice that only in cases where the states are in the same manifold D these compositions are the same as above. We mark pre-integrated magnitudes with a bar, e.g. ∆ ij , c. We name Q ij , Q k and Q n the covariances of respectively the pre-integrated delta, the current delta, and the measurement noise. For a function Y = f (X ), we note the Jacobian J Y X ∂Y ∂X = ∂f (X ) ∂X in the manifold sense, i.e., (4.23). The chain rule becomes

x j = x i ⊞ ∆ ij (4.58) ∆ ij = x j ⊟ x i ,
J Z X = J Z Y • J Y X .

Pipeline

The pipeline has two distinct phases: pre-integration to create a factor, and factor evaluation to optimise the system. During the pre-integration phase we proceed as follows:

1. Compensate measured data with available calibration parameters. This typically results in an element of T D .

2. Compute the current motion delta δ k ∈ D.

3. Pre-integrate ∆ ik ∈ D, Q ik , and the Jacobian J ∆ ik c . Upon creation of a new KF, a motion factor is built with the result of these integrals. Then, at each solver iteration, we evaluate each of the factors as follows.

4. Compute the residual with the pre-integrated covariance. These are detailed in Algs. 5-7, and described hereafter.

Delta pre-integration (Algorithm 5)

At the arrival of each motion measurement y k , we use the currently available calibration parameters c to pre-compensate the measurements. Depending on the nature of the measurements, we may need to integrate the motion over the sampling time

}; Q n ∆ ii = E D , Q ii = 0, J ∆ ii c = 0, j = i while Not KF do k = j + 1 Compensate motion data b k = f c (c, y k , n, δt) ∈ T D → J b k c , J b k n
Compute current delta and covariance

δ k = retr(b k ) ∈ D → J δ k b k Q k = J δ k n Q n J δ k n ⊤ , with J δ k n = J δ k b k J b k n
Pre-integrate delta, covariance, and Jacobian

∆ ik = ∆ ij ⊞ δ k ∈ D → J ∆ ik ∆ ij , J ∆ ik δ k Q ik = J ∆ ik ∆ ij Q ij J ∆ ik ∆ ij ⊤ + J ∆ ik δ k Q k J ∆ ik δ k ⊤ J ∆ ik c = J ∆ ik ∆ ij J ∆ ij c + J ∆ ik δ k J δ k b k J b k c j = k
Compute upper square root of information matrix

Ω ⊤/2 ij = L ⊤ , with Q -1 ij = LL ⊤ via Cholesky Output: ∆ ij , Ω ⊤/2 ij , J ∆ ij c
δt. We also compute the Jacobians with respect to the calibration parameters c and the measurements' noise n ∼ N {0,

Q n }, b k = f b (c, y k , n, δt) (4.60a) J b k c = ∂b k ∂c J b k n = ∂b k ∂n (4.60b)
It is common, though not mandatory, that b k ∈ T D . The operations in f b () are completely dependent on each particular system and cannot be generalised.

We then wrap the compensated step b k onto a delta on the manifold (Fig. 4.6),

δ k = f δ (b k ) ∈ D (4.61a) J δ k b k = ∂δ k ∂b k ∈ R d×d (4.61b) Notice that if b k ∈ T D , then δ k = f δ (b k ) = retr(b k ).
We then compute the delta covariances matrix,

Q k = J δ k n Q n J δ k n ⊤ ∈ R d×d
Input: ∆ ij , Ω ⊤/2 ij , J ∆ ij c , c, x i , x j , c Predict delta from KFs x i and x j ∆ ij = x j ⊟ x i ∈ D → J ∆ ij x i , J ∆ ij x j
Correct pre-integrated delta with new calibration c

∆ ij = ∆ ij ⊕ J ∆ ij c (c -c) ∈ D → J ∆ ij c
Compute delta error and residual

e ij = ∆ ij ⊖ ∆ ij ∈ T D (∆ ij ) ≃ R d → J e ij ∆ ij , J e ij ∆ ij r ij = Ω ⊤/2 ij e ij ∈ R d Compute residual Jacobians J e ij x i = J e ij ∆ ij J ∆ ij x i , J e ij x j = J e ij ∆ ij J ∆ ij x j , J e ij c = J e ij ∆ ij J ∆ ij c J r ij * = Ω ⊤/2 ij J e ij * Output: r ij , J r ij x i , J r ij x j , J r ij c where J δ k n = J δ k b k J b k
n is obtained via the chain rule. We finally incorporate the current delta onto the pre-integrated delta (Fig. 4.6), and compute the Jacobians,

∆ ik = ∆ ij ⊞ δ k ∈ D (4.63a) J ∆ ik ∆ ij = ∂∆ ik ∂∆ ij , J ∆ ik δ k = ∂∆ ik ∂δ k ∈ R d×d , (4.63b)
we integrate the covariances matrix on T D (∆ ik ), (4.64) and integrate the Jacobian with respect to the calibration parameters, using the chain rule,

Q ik = J ∆ ik ∆ ij Q ij J ∆ ik ∆ ij ⊤ + J ∆ ik δ k Q k J ∆ ik δ k ⊤ ∈ R d×d ,
J ∆ ik c = J ∆ ik ∆ ij J ∆ ij c + J ∆ ik δ k J δ k b k J b k c ∈ R d×c . (4.65) Since c ∈ R c , this Jacobian maps R c → T D (∆ ik ).

Residual evaluation (Algs. 6 and 7)

At keyframe creation, we need to produce a factor storing ∆ ij , Q ij , J ∆ ij c , and c. These are used to evaluate the residual, which depends on the states x i , x j of the two KFs and the state c of the calibration parameters.

We propose two alternative algorithms. The first one (Algorithm 6) corresponds to the original algorithm [Lupton 2009] for the IMU, adapted to the SO(3) manifold 106 Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration Algorithm 7: Residual evaluation, our proposal

Input: ∆ ij , Ω ⊤/2 ij , J ∆ ij c , c, x i , x j , c Predict delta from KFs x i and x j ∆ ij = x j ⊟ x i ∈ D → J ∆ ij x i , J ∆ ij x j Predict delta correction step ŝ ∆ ij ⊖ ∆ ij ∈ T D (∆ ij ) → J ŝ ∆ ij , J ŝ ∆ ij
Compute delta correction step from c and c s ≈ J

∆ ij c (c -c) ∈ T D (∆ ij ) → J s c = J ∆ ij c
Compute correction step error and residual

e ij = ŝ -s ∈ T D (∆ ij ) ≃ R d r ij = Ω ⊤/2 ij e ij ∈ R d Compute residual Jacobians J e ij x i = J ŝ ∆ ij J ∆ ij x i , J e ij x j = J ŝ ∆ ij J ∆ ij x j , J e ij c = -J s c J r ij * = Ω ⊤/2 ij J e ij * Output: r ij , J r ij x i , J r ij x j , J r ij c
in [START_REF] Forster | [END_REF]], and generalised here for any kind of motion integration. We start by predicting a motion delta given KF's x i and x j ,

∆ ij = x j ⊟ x i ∈ D (4.66a)
Second, we correct the pre-integrated delta with the new calibration values c,

∆ ij = ∆ ij ⊕ J ∆ ij c (c -c) ∈ D (4.66b)
Third, we compute the tangent delta error,

e ij = ∆ ij ⊖ ∆ ij ∈ T D (∆ ij ) (4.66c)
And finally we weight it to produce the residual

r ij = Ω ⊤/2 ij e ij ∈ T D (∆ ij ) ≃ R d (4.66d)
where Ω ij = Q -1 ij is the information matrix of the pre-integrated delta, defined in T D (∆ ij ), and Ω ⊤/2 ij is its upper triangular square root, obtained by Cholesky factorisation.

This first algorithm has the (small) inconvenient that the pre-computed covariance Q ij belongs to T D (∆ ij ), while the computed error e ij belongs to T D (∆ ij ), so the weighting in (4.66d) is not proper. This can be corrected by propagating Q ij to T D (∆ ij ) using the Jacobian of (4.66b), but we prefer instead to evaluate the error directly on T D (∆ ij ). In our proposed algorithm (Algorithm 7), we predict a correction step ŝ given the predicted ∆ ij and pre-integrated ∆ ij deltas, and compare it against the actual correction step s computed through the pre-integrated Jacobian,

∆ ij = x j ⊟ x i ∈ D (4.67a) ŝ ∆ ij ⊖ ∆ ij ∈ T D (∆ ij ) (4.67b) s = J ∆ ij c (c -c) ∈ T D (∆ ij ) (4.67c) e ij = ŝ -s ∈ T D (∆ ij ) (4.67d) r ij = Ω ⊤/2 ij e ij ∈ T D (∆ ij ) ≃ R d . (4.67e)
Notice that all these results belong to the same tangent space. Moreover, this method is cheaper in that it has one fewer ⊕ (retract and compose) operation.

State reconstruction

Optimised states can be retrieved at any time t j , even if this t j does not correspond to any KF,

x j = x i ⊞ (∆ ij ⊕ J ∆ ij c (c i -c i )) , (4.68)
where x i is the last optimised KF before t j , and c i is the optimised calibration vector associated to x i . To do so we need to store all the history of ∆ ij and J ∆ ij c . The same is true for Q ij if we aim also at reconstructing the covariance trajectory.

In the next section, we present an example of application of the concepts presented until here. Thus the generalised preintegration method is used in SE (2) in the case of the self-calibration using a differential drive model.

The generalised preintegration method for the IMU

The preintegration method initially presented in Section 3.3 uses the pipeline exposed in Algs. 5 and 6. Here we show to what extent the initial preintegration method adheres to the general one.

The Delta preintegration is specific to the sensor

The first step of the preintegration method is to compute the current body magnitude with pre-compensation of the measurements given the current calibration parameters c following (3.11). In the IMU case, c are the bias parameters. As it was stated sooner, this operation and the computation of the related derivatives (Eq. (3.10)) cannot be generalised because of dependencies on the system. Finally, the computed body magnitude is used to define the current δ k as detailed in Eq. (3.16) as well as the jacobians of the current δ. These are therefore the only application to self-calibration equations that need to be coded for each new sensor providing a different set of measurements.

All remaining operations, that are the composition in the manifold, as well as the correction and residuals evaluation steps have a common generalised formulation. In the specific case of the IMU, these operations are processed in a composite manifold (see 4.2.6) of dimension 10 due to the dimension of δ itself. The corresponding composite manifold is defined by R 3 × S(3) × R 3 with manifold elements S = (p, q, v) and tangents τ = (δp, δq, δv). This delta preintegration step, represented by (4.54) in the general case, is manifold dependent and thus needs to be defined for each different preintegration method. We recall hereafter the incremental delta preintegration for IMUs.

∆p ik = ∆p ij + ∆v ij δt + 1 2 ∆q ij ⊙ δp jk ∆v ik = ∆v ij + ∆q ij ⊙ δv jk ∆q ik = ∆q ij ⊗ δq jk
Similarly, the delta composition as well as the state composition or differentiation equations implying the use of operators ⊞ or ⊟ are specific to each sensor.

Using the derivatives

Derivatives computation

The preintegration method implies the uses of several derivatives (Jacobians of the body magnitude, of the current delta and Jacobians of the delta composition). These derivatives are related to sensor specific operations that need to be redefined for each new sensor. As a consequence, these specific derivatives need to be coded again to use the preintegration method with a new type of sensor. The chain rule makes it possible to generalise the computation of other Jacobians and Covariances using the derivatives that are specific to each type of sensor. The first consequence is that the computation of covariances is made easier and does not need to be recoded. Furthermore, we can notice that the jacobian with respect to the calibration parameters i.e., the bias parameters in the IMU case, can be computed using the chain rule (see (4.65)) and thus their computation can be generalised. In the IMU case developed in Section 3.3, this Jacobian is the pre-integrated bias Jacobian J ∆ b b which computation we recall hereafter.

J ∆ ik b b = J ∆ ik ∆ ij J ∆ ij b b -J ∆ ik δ jk J δ jk b b .
This formula matches exactly (4.65) with J

δ jk b b = J δ jk b J b b b and J b k c = J b k b b = -I 6 (see (3.18)).

A generalised sensor self-calibration

Thus another consequence of the generalisation of the preintegration method to any manifold is that the self-calibration method itself is generalised. Indeed, the delta correction with new bias given as ∆

= ∆ + J ∆ b b (b b -b b
) is a formulation specific to the IMU and yet matching the more general preintegrated delta correction with new calibration values ∆ c). This correction does not depend on the sensor and is realised in the manifold formed by the preintegrated values. Thus it does not need to be coded again for each new type of sensor.

ij = ∆ ij ⊕ J ∆ ij c (c
Only the residual evaluation of the former IMU preintegration method is different to the proposal as formulated in Algorithm 7. Indeed, in the first case, the residual is defined using the computed error e ij defined in T D (∆ ij ) instead of T D (∆ ij ) where the pre-computed covariance Q ij (and by extension the information matrix Ω ij ) used to weight the error to produce the residual is defined. The new expression of the residual can be deduced from the methodology proposed in Algorithm 7 as

r ij = Ω ⊤/2 ij e ij = Ω ⊤/2 ij ( ∆ ij ∆ ij ) -J ∆ ij c (c -c) = Ω ⊤/2 ij    ∆p ij -∆p ij ∆v ij -∆v ij Log(∆q * ij ⊗ ∆q ij )    -J ∆ ij c (c -c) .
This generalised preintegration method is implemented in WOLF, a library presented with more details in Chapter 5.

Self-calibration example in SE(2) for a differential drive

The self-calibration application exposed hereafter is part of the work developed by Jérémy Deray with the help of Joan Solà.

Overview

The differential drive model consists of two actuated wheels on a single axis, one on each side of the robot base, with its origin frame located at the center of the axis. The robot is parametrised by its wheels radii (r l , r r ) and the length d of the axis. To each of these parameters is associated a correction factor such that c = [c l c r c d ] ⊤ is the intrinsic parameters calibration vector. The motion is usually measured by means of wheel encoders reporting incremental wheel angles y = [δψ l , δψ r ]+n every time step δt, where n is additive Gaussian noise.

Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

State and delta definitions

We define the states of position and orientation angle x = (p, θ), which act as a compact representation of SE (2). Consequently, state increments or deltas ∆ ij and δ k are also in SE(2).

Incremental delta pre-integration

For convenience, we define the body magnitudes b

= (δl, δθ) = f b (y, c, n) as δl = 1 2 (r r c r δψ r + r l c l δψ l ) δθ = 1 D (r r c r δψ r -r l c l δψ l ) .
(4.69)

with D = d c d . Its components, respectively, highlight the common (along track length), and differential (turn) components of the wheels reported motions. From the equation above, one can find the jacobians of the motion components. Their expressions are given hereafter.

J b y = J b n = 1 2 r l c l 1 2 r r c r -r l c l r r c r ∈ R 2×2 (4.70a) J b c = 1 2 ψ l r l 1 2 ψ r r r 0 -ψ l r l ψ r r r -δθ c d ∈ R 2×3 (4.70b)
Assuming constant control inputs over the sampling time period [t j , t k ], the robot moves along an arc of circle of radius

R = δl δθ . ( 4

.71)

The motion increment δ k can be expressed in SE (2) as

δx = R sin(δθ) δy = R(1 -cos(δθ)) (4.72) δθ = δθ , which is exactly δ = exp(τ ) ∈ SE(2) with τ = [δl, 0, δθ] ∈ se(2), i.e.,
J δ k b =    sin(δθ) δθ R cos(δθ) -sin(δθ) δθ 1-cos(δθ) δθ R sin(δθ) -1-cos(δθ) δθ 0 1    ∈ R 3×2 (4.74)
Similarly, an equivalent form can be found from (4.73),

J δ k b =    cos(δθ/2) -1 2 δl sin(δθ/2) sin(δθ/2) 1 2 δl cos(δθ/2) 0 1    ∈ R 3×2 (4.75)
The delta integration is simply the composition in SE( 2),

∆p ik = ∆p ij + ∆R ij δp k ∆θ ik = ∆θ ij + δθ k (4.76)
where ∆R ij = exp(∆θ ij ) is the rotation matrix delta corresponding to the rotation angle of ∆θ ij , see (C.17) in Appendix C.2, and δp k is the translation vector delta corresponding to δ k . The jacobians of the delta composition are given by

J ∆ ik ∆ ij = I ∆R ij [1] × δp k 0 1 ∈ R 3×3 (4.77a) J ∆ ik δ k = ∆R ij 0 0 1 ∈ R 3×3 (4.77b) with [1] × = 0 -1 1 0 , see Appendix C.2.3.

Integration of the delta covariance and the Jacobian

We follow the general procedure, obtaining Q k from (4.62). Here, the diagonal measurement covariance Q n is defined by,

Q n = σ 2 ψ l + α 2 0 0 σ 2 ψr + α 2 , (4.78) σ 2 ψ l = k l δψ l , σ 2 ψr = k r δψ r , α = 1 2 (µ l + µ r ) ,
where k r and k l are constant parameters, and α acts as an offset equal to half the wheels encoders resolution µ l and µ r [START_REF] Siegwart | [END_REF]]. The pre-integrated delta covariance Q ik is then integrated normally with (4.64), starting at Q ii = 0 3×3 .

For the Jacobian we use (4.65), starting at J ∆ ii c = 0 3×3 .

Chapter 4. Generalized motion estimation in manifolds, with application to self-calibration

Residual

Following Algorithm 6 with for errors, we have

∆ ij = R ⊤ i (p j -p i ) θ j -θ i (4.79) ∆ ij = ∆ ij ⊕ J ∆ ij c (c i -c i ) (4.80) r(x i , x j , c i ) = Ω ⊤/2 ij ∆p ij -∆p ij ∆θ ij -∆θ ij ∈ R 3 , (4.81)
where the angle differences must be brought to (-π, π ]. The information matrix is given by

Ω ij = Q -1 ∆ij .
As explained sooner, this residual evaluation is not proper due to the terms belonging to different spaces. Indeed the information matrix belongs to T D (∆ ij ) while the error computed as above is defined in T D (∆ ij ). For a more rigorous algebraic approach, we could also define the residuals following Algorithm 7 as

r(x i , x j , c i ) = Ω ⊤/2 ij e ij = Ω ⊤/2 ij ∆p ij -∆p ij ∆θ ij -∆θ ij -J ∆ ij c (c -c) ∈ R 3 (4.82)
where all the terms are defined in T D (∆ ij ) ≃ R 3 .

Conclusion

In this work, we have proceeded to a selection of materials that avoids abstract mathematical concepts as much as possible. This we hope helps focusing Lie theory to make its tools easier to understand and to use. Third, we have promoted the usage of handy operators, such as the lift() and retr() maps, and the plus and minus operators ⊕, ⊖, , . They allow us to work on the Cartesian representation of the tangent spaces, producing formulas for derivatives and covariance handling that greatly resemble their counterparts in standard vector spaces. Using these tools, we have generalised the correction step of the preintegration theory to its use on any manifold and by extension with any kind of motion sensor. Through the presentation of this generalised method, we provide a computation pipeline for implementation in real systems and that benefits to the computation of Jacobians on which we have made a special emphasis by using the chain rule. Furthermore, the delta magnitude defined in a manifold have a physical understanding. We believe these contributions will help to a better understanding of the preintegration theory, make its use easier for further applications and allow an efficient implementation of the method on real systems. Among possible applications, we can cite the sensor self-calibration as a side-effect of the generalisation of the theory through a general procedure to correct preintegrated deltas taking into account new calibration parameters. We illustrated this application with a differential drive and an IMU. Works are being conducted to provide further sensor calibration examples as well as experimental results. Again, the interested reader is invited to read [START_REF] Solà | [END_REF] for further explanations as well as examples for a better understanding. application to self-calibration Table 4.1: Typical manifolds used in 2D and 3D motion, including the trivial R n (some details omitted) Manifold M, • LOcalization Frames) is very useful to provide the perception side which is needed for a complete closed-loop control scheme that does not require any external infrastructure (for robot localisation) or previous knowledge of the environment (thus the need of online mapping).

size dim M T M Cartesian retract lift n-D vector space R n , + n n v ∈ R n v ∈ R n v ∈ R n identity identity circle group S 1 , • 2 1 z ∈ C iθ ∈ Im(C) θ ∈ R z = exp(iθ) log() Rotation group SO(2), • 4 1 R [θ] × ∈ so(2) θ ∈ R R = exp([θ] × ) log() Rigid motion group SE(2), • 9 3 R t 0 1 [θ] × v 0 1 ∈ se(2) (v, θ) ∈ R 3 exp() log() 3-sphere group S 3 , ⊗ 4 3 q ∈ H uθ/2 ∈ Im(H) θ ∈ R 3 q = exp(uθ/2) log() Rotation group SO(3), • 9 3 R [θ] × ∈ so(3) θ ∈ R 3 R = exp([θ] × ) log() Rigid motion group SE(3), • 16 6 R t 0 1 [θ] × v 0 1 ∈ se(3) (v, θ) ∈ R 6 exp() log

The Loco3D Project

Introduction

Planning, adapting and executing multi-contact locomotion movements on legged robots in complex environments remains an open problem. The Loco3D project aims at introducing a complete pipeline to address this issue in the context of humanoid robots inside industrial environments. This pipeline relies on a multi-stage approach in order to simplify the process flow and to exploit at best state-of-the-art techniques both in terms of contact planning, whole-body control and perception. Loco3D stands for Locomotion in 3D, in contrast to the classic locomotion on quasiflat terrains, where the motion of the centre of mass of the robot is mostly limited to a 2D plane. The main challenges lie in the choice of the different modules composing this pipeline as well as their mutual interactions [START_REF] Carpentier | [END_REF]].

Motivations

Many challenges remain open when it comes to applications involving the use of legged robots. Efforts are already made on the perception of the environment and on the interaction with it [Victorino 2003]. Amongst the challenges, we can also cite the multi-contact locomotion of legged robots in complex environments. Indeed, hardware limitations due to the design and constraints applied to robots may be overcome through the use of the environment so that the robot can finally achieve a task. For example, the use of a handrail by HRP-2 to climb stairs (Fig. 5.2) allows it to achieve the task while reducing the power consumption by 25% [Kudruss 2015]. An example of motion requiring the use of contacts with the environment is given in [START_REF] Carpentier | [END_REF]]. The presented method allows to generate feasible contact sequences for the robot to stand up using the proximal environment.

The multi-contact locomotion problem is hard, and the Loco3D project aims at decoupling it into specific subproblems that are simpler to solve and able to work

The need of a perception method

Since the generation of motions on the robot depends on the environment and the robot's configuration in it, a precise estimation of the surroundings of the robot is necessary as well as an estimate of its location and the direction of the gravity vector, which needs to be taken into account for the dynamics. Furthermore, in the case of multi-contact locomotion, a dense representation of the environment is required to compute feasible contact sequences, e.g. in Fig. 5.4. All these perception requirements can be fulfilled by SLAM methods. Another objective the perception method faces is the need to estimate the robot's localisation at control-rate, that is 1 kHz. To meet these challenges, we made the choice to contribute to the WOLF project and design a SLAM method that we could rely on the long-term and augment relating to future needs.

WOLF

Introduction

WOLF (acronym for Windowed LOcalization Frames) is a C++ library to solve localisation problems in mobile robotics, such as SLAM, map-based localisation, or visual odometry. The approach contemplates the coexistence of multiple sensors of different kinds, be them synchronised or not. WOLF organises the state vectors as a set of key-frames, defining the robot trajectory, plus other states such as landmarks in the environment or sensor parameters for calibration, WOLF computes error vectors given the available measurements in that window. The philosophy developed through WOLF is to have a structure for having the data accessible and organised, called the WOLF Tree, plus some functionalities for managing this data. It requires, on one side, several front-ends (one per sensor, or per sensor type), and a back-end constituted by the solver.

Efforts are made to have WOLF interface able with many kinds of solvers, including filters and nonlinear optimisers. Thus the choice of the solver is left to users so that they can use filters such as EKF, error-state KF, iterative EKF, information filters or others as well as nonlinear or incremental optimisers. The task of interfacing WOLF with these solvers is relegated to wrappers, which are coded out of WOLF. WOLF is currently used with Google Ceres through such a wrapper. However, we also provide an experimental wrapper-less solver for incremental, sparse, nonlinear optimisation based on the QR decomposition (implementing the iSAM algorithm [Kaess 2008b]).

Motivations

SLAM solutions are usually provided with a specific implementation of the solver. When the problem is tackled with a filtering approach such as one of those presented in Section 1.2, the solution can be provided as an application relying on no other external requirement. A good example of such solution is RT-SLAM [START_REF] Roussillon | [END_REF]] specifically designed to provide a practical remedy for online experiments on robots requiring real-time execution. RT-SLAM is an EKF-based solution implemented in C++. However, if one tries to cope with providing a SLAM solution based on nonlinear optimisation methods, it usually comes as an association of a front-end coping with the sensory data processing, a back-end solving for the optimal solution, with a specific philosophy regarding the structure of the problem.

The efforts put in the development of the WOLF library is motivated by the need to propose multiple front-ends that can be used together with state-of-theart back-ends for optimisation purposes. Several challenges need to be faced in order to construct the front-ends. Processing multiple sensors providing different types of information at different rates is one part of the problem that is of main importance. Sensor fusion is still a main topic of research as uncertainty management [Murphy 1998], fusion schemes [Kaempchen 2003[START_REF] Spanos | [END_REF], decisionmaking [Klein 2004] and other problems are still open challenges. Besides, there 5.2. WOLF 119 is a wide range of possible applications from calibration [START_REF] Olsson | [END_REF][START_REF] Hol | [END_REF] and robotics [De Silva 2017[START_REF] Olofsson | [END_REF], Santoso 2017, Novak 2015] to body pose estimation [START_REF] Kok | [END_REF]] and tracking problems [START_REF] Chavez-Garcia | [END_REF]. Thus there is an interest for an easy-to-use library with a flexible architecture to take care of sensor fusion tasks. WOLF is also designed to fulfil this demand in the context of localisation solving.

WOLF is a collaborative project from the Institut de Robòtica i Informàtica Industrial (IRI), PAL Robotics and LAAS-CNRS to propose a state-of-the-art SLAM solution we could rely on and update ourselves for long-term projects. But it may also be of interest to other research communities.

Objectives

On the application side, the objective of the WOLF development project is to propose an open-source visual-odometry and SLAM solutions based on factor graphs that can be extended and modified according to users' specific needs. The initial goal includes proposing methods to use most common sensors (cameras, LIDAR, IMUs, . . . ) with a plug-and-play approach making their use easier. The sensors are set to communicate one with others so that a key-frame creation results in a coherent factor graph containing all the information from one key-frame to another given the data timestamps provided by the sensors. The use of this library can be extended to other purposes such as calibration, inertial navigation, simulation, or even be used to investigate other research purposes such as Active SLAM problems that are defined as a combination of SLAM and path planning problems.

Architecture

The architecture behind WOLF is complex but its global view is made intuitive. The basic Wolf structure is a tree of base classes reproducing the elements of the robotic problem. This is called the Wolf Tree (Fig. 5.5). It has a robot, with sensors, with a trajectory formed by key-frames, and the map with its landmarks. These base classes can be derived to build specialised classes with specific behaviours based on the base classes functionalities. The Wolf Tree connectivity is augmented with the constraints linking different parts of it, becoming a real network of relations. This network is equivalent to the factor graph that would be solved by graphical models and nonlinear optimisation. Wrappers are the ones transferring the Wolf structure into a factor graph that can be provided to the solver.

Sensors produce Captures that are entities designed to contain the data provided by first ones. The use of captures is critical for state estimation process and calibration methods. Indeed, whereas sensors contain static intrinsic and extrinsic parameters, the captures can be given dynamic sensor parameters that are to be estimated and taken into account all along the trajectory. Finally, the captures' data are processed by Processor classes providing a common base to handle information provided by captures. Processors can be used to detect specific features in images or for tracking purposes as well as integrating motion measurements. They also are at the crux of the key-frame creation process and thus also the initiators of the constraints generation in the graph.

The resulting architecture allows one to easily create a factor graph-based representation of the problem using multiple sensors. The classes can be specialised according to the users' will using polymorphism, virtual inheritance and templates. The choice of the solver is let to the user and only needs a wrapper to connect it to WOLF.

Contributions

Implementation and team members

The development of WOLF is coordinated by Prof. Joan SOLÀ from IRI who also contributed to the development of almost all the subparts of the library. Joan Vallvé Navarro is a PhD student focusing on the aspects of information metrics for localisation and mapping. His main contributions to the project lie in his research towards a more efficient use of factor graphs [START_REF] Vallvé | [END_REF][START_REF] Vallvé | [END_REF][START_REF] Vallvé | [END_REF]] and the expertise he could bring on the development side. He also provided the wrapper that allows one to use WOLF with Google Ceres. Jérémie Deray is also a PhD from PAL robotics with major contributions regarding the loop closure strategy and the implementation of Bag of Words techniques in the library. Angel Santamaria Navarro is a post-doctoral researcher working at IRI. His main research interests are related to unmanned aerial vehicles (UAV). along with estimation methods based on visual information [Santamaria-Navarro 2017b, Santamaria-Navarro 2017a]. He is a main contributor to the visual perception method implementation. Finally, my main personal contributions in WOLF lie in the development of the preintegration method (Section 3.3) and its integration the library as well as the use of IMU sensors and both static and dynamic state estimation methods.

Related Projects

WOLF is involved in several projects as a SLAM solution provider. This SLAM solution is to be used on straddle carriers for port scenarios in the context of the Logimatic project [Gonzalez 2016] aiming at finding more efficient and cost-effective ways to handle containers in ports based on innovative technical solutions. It will also be used for autonomous vehicles in warehouses with the ASTI project as well as for the perception side of aerial robotic system with multiple arms and advances manipulation capabilities (AEROARMS [Ollero 2015]) to be applied in industrial inspection and maintenance. Finally, WOLF's SLAM solution will also be used by the GAUSS project [Jimenez- Gonzalez 2018] for robust GNSS localisation of large drone floats.

Conclusion

The work presented in this thesis was motivated by the need of a high frequency state estimation method for closed loop control of the humanoid robot HRP-2.

Our objective is to give a first part of the answer to the following question: is it relevant to use low-cost IMUs on different places of a robot in order to get a new sensing capability? The question behind is: can we use this new sensing capability to overcome some estimation problems on legged robots? In order to complete this objective, we investigated the use of inertial estimation techniques applied to pedestrian navigation and robot locomotion using a factor graph representation and a probabilistic formulation. We also investigated the use of IMUs on an endeffector of the robot with the intention to give it a new sensing capability that would be complementary with the kinematic odometry and provide some useful motion-related information. Despite real-time requirements, we showed that batchoptimisation techniques can be applied for estimations with a very high update rate (1 kHz in our case). This can be achieved by using sensors in an efficient manner and by optimising the implementations.

In Chapter 3, we described the quaternion-based preintegration technique for an efficient use of IMU sensors giving a physical explanation to the preintegrated values. Additional efforts were made in order to present the derivatives clearly in the hope of helping the community to better understand and to facilitate the implementation of this method requiring analytic derivatives to fully benefit of its efficiency. The estimation method was presented with a factor graph containing all the kinematic and odometry constraints. The optimisation was performed with a nonlinear least squares solver. The proposed method was applied to pedestrian navigation and tests have been conducted on the humanoid robot HRP-2 with an IMU on its foot to estimate its trajectory. These investigations raised the concern of a better integration of the IMU in the robot itself as opposed to the plug-and-play approach that we used. However, despite the problems introduced by the mechanical vibrations that are propagated through the robot, we were able to have coherent estimates of the foot's trajectory on simple scenarii.

In Chapter 4, we investigated self-calibration procedures on manifold using graph-based motion estimation. This work extends some solutions usually expressed in the mathematical language of the Group Theory to any kind of manifold while presenting the method with a vocabulary that is more familiar to roboticists. The described method can be used for calibration procedures using various sensors.

In Chapter 5, we presented the WOLF library to which the GEPETTO group is contributing to in the context of the Loco3D project. Special attention was given to the integration of IMU sensors in WOLF not only for integration and estimation purposes with the preintegration method on manifolds, but also for the estimation of intrinsic and extrinsic parameters.

As a result, the use of IMUs on the end-effectors of legged-robots deserves to be Conclusion studied further. Indeed, despite the results highlighted in this thesis, the application on a legged-robot seems to require specific cares. The estimation method explained through this thesis was tested on the pedestrian case before it was successfully applied on the HRP-2 robot with simple study cases. Some perspectives need to be investigate to have a clear understanding in whether this new sensing capability is relevant or not. And with these perspectives, new challenges are likely to be faced.

Perspectives Short-term perspectives

Several perspectives arise from the work presented in this thesis. In the short-term, the high frequency estimation technique should be fused with SLAM to investigate the use of an auto-calibrating kHz SLAM method with low-cost sensors. The resulting SLAM method could be of interest for applications with humanoid robots and UAVs to close the control loop with a fast perception method. Furthermore, the self-calibration method is currently capable of estimating intrinsic and extrinsic parameters of a sensor. However, the intrinsinc parameters of the IMU are currently limited to the estimation of the biases. Those intrinsic parameters should be extended to the correction of the scale factor and misalignment errors due to the sensor in order to cope with effects introduced by motions with high dynamics. The result would then be an easy IMU calibration method that may be of interest for research communities using UAVs or investigating motion analysis. Furthermore, the application presented through the pedestrian navigation estimation could be extended to use with other sensors such as ultrawidebands [START_REF] Kok | [END_REF]] or magnetometers [START_REF] Kok | [END_REF]].

Mid-term perspectives

With regard to mid-term perspectives it would be interesting to investigate the use of IMUs directly integrated to end-effectors for legged locomotion and interaction with the environment. This requires specific design as low-level integration of IMU sensors in the system which needs to be planned in advance. The investigations detailed by Rotella et al. [START_REF] Rotella | [END_REF]] are a first step towards a stronger integration of inertial measurement units in robots for improved state estimation. The authors managed to improve the quality of the velocity and acceleration estimates for the control feedback. Furthermore, the generalisation of the preintegration method by including the possibility to work on SE3 leads to possible investigations on closedloop control using velocity measurements.

Long-term perspectives

In a long-term, a logical perspective would be to improve the state estimation of a humanoid robot with IMUs attached not only to end-effectors but to each segment providing synchronised data. Although a state estimation providing estimates at control-rate may seem appealing in this context, the importance of vibrations propagated through the robot and measured by the IMUs may constitute an obstacle if tackled with low-cost sensors. Furthermore, the higher the number of sensors involved in the fusion strategy, the more computationally demanding the estimation process may be. Thus we could investigate the limits of a fusion scheme designed for real-time whole-body control of a humanoid robots. Another interesting investigation could be the design of an estimator that would be able to not only estimate the state of the system on a wide time interval but also to consider only specific states of the system given sensory inputs. Such an estimator would be a first step towards realtime reactions to unexpected events while complying with the whole-body control phylosophy of robots.

Another long term motivational perspective oriented to biomechanical applications would be the use of several IMUs with sensor fusion methods in the context of neuroprosthesis and exoskeletons to estimate the dynamics of the system and close the control-loop. There are interesting works on passive prosthesis [START_REF][END_REF]] and powered exoskeletons [START_REF] Bortole | [END_REF][START_REF] Esquenazi | [END_REF]. Furthermore, the use of IMUs in the design of prosthesis has been of growing interest to improve the control of prosthetic devices [START_REF] Aisen | [END_REF][START_REF] Lauretti | [END_REF][START_REF] Krasoulis | [END_REF], Seel 2016b]. Nevertheless these researches usually only cope with standard IMU integration and filter-based methods. Similarly, IMUs are also of interest in the analysis of the human motions [Zhao 2016, Seel 2016a[START_REF] Seel | [END_REF]. Due to the importance recently given to IMUs, my personal belief is that the methods presented through this manuscript can be of interest for these research areas. Indeed, real-time control is one of the specifications prosthetic devices need to satisfy. Various methods can be used to achieve this objective while minimising the embedded computing power such as the use of filters [START_REF] Fakoorian | [END_REF][START_REF] Lenzi | [END_REF] or continuous integration along with stance phase detection [START_REF] Moreno | [END_REF]]. These methods can benefit from an IMU integration method providing real-time results. Although a unified architecture for sensor fusion providing real-time estimates may also be useful, the computation requirements from these applications make the development of such a solution very challenging. The development of motion analysis systems using IMUs along with other sensors as described in [Mueller 2011[START_REF] Steins | [END_REF][START_REF] Šlajpah | [END_REF] seems to result in encouraging outcomes that may gain advantages from future customisable sensor fusion solutions. Thus yet another long-term perspective would be to make possible the use of WOLF on systems with limited computing power. This thesis resulted in the development of a high-rate estimation method based on an IMU and batch-optimisation techniques. Due to the use of low-cost sensors, special attention was required for the estimation of intrinsic parameters and we investigated the use of a common preintegration formulation for the calibration of various sensors. The contributions developped through this thesis come with a sight on the preintegration subject from a roboticist point of view. Thus efforts were made to explain this point of view in a way that roboticists may understand with more ease so that they can use these methods and bring their contributions to the subject. We believe the preintegration theory and the use of multiple IMUs to be interesting methods to be used together and with other sensors.

B.3 Quaternion and rotations

Let us define a vector x in R 3 and R the rotation matrix that transforms x into x ′ x ′ = Rx There exist an equivalent rotation q described in the quaternion space H to perform the same transformation so that x′ = q ⊗ x ⊗ q * (B.23) where x is in quaternion form, that is

x = xi + yj + zk = 0 x (B.24)
However, since we can easily distinguish the context by the presence of the quaternion product operator ⊗, we will abuse of the notation in the following and write

x ′ = q ⊗ x ⊗ q * = Rx (B.25)
The expression of the rotation matrix that is equivalent to the quaternion can be found by developing both q ⊗ x ⊗ q * and Rx and by identifying terms. This yields to R{q} =    q 2 w + q 2 xq 2 yq 2 z 2(q x q yq w q z ) 2(q x q y + q w q y ) 2(q x q y + q w q z ) q 2 wq 2 x + q 2 yq 2 z 2(q y q zq w q x ) 2(q x q zq w q y ) 2(q y q z + q w q x ) q 2 wq 2 xq 2 y + q 2 where one can the following properties:

• the identity quaternion encodes the null rotation (B.27)

• a quaternion q and its negative encode the same rotation (B.28)

• the conjugate quaternion encodes the inverse rotation (B.29)

• the quaternion product and rotation matrices compose rotations in the same order (B.30)

B.3. Quaternion and rotations 135

From (B.30) we can find out how the composition of rotations work. Indeed, let us define x A a 3D vector so that

x A = R AB R BC x C
where the chain rule makes it simple to understand the successive rotation operations. The rotation matrix R BC is used to rotate the 3D vector x C into x B = R BC x C and R AB is the rotation matrix that is used to transform x B into x A . The equivalent transformation using quaternion is given by

x A = q AB ⊗ q BC ⊗ x C ⊗ q * BC ⊗ q * AB and recalling (B.29), we finally get

x A = q AB ⊗ q BC q AC ⊗ x C ⊗ q CB ⊗ q BA q * AC
We define the quaternion-by-vector product ⊙ so that q ⊙ v q ⊗ v ⊗ q * , (B.31)

where q * denotes the dual quaternion of q. The symbol ⊗ stands for the product of quaternions and ⊙ corresponds to the quaternion-by-vector which performs a 3D rotation of an input vector v. Notice that if R is the rotation matrix equivalent to the quaternion q, then q ⊙ v = R v. This straightforward equivalence enables us to define all the forthcoming IMU pre-integration algebra in a way that allows a direct transcription between the S 3 and SO(3) spaces of representation.

θ q S 3 R 3 q =exp(θ) θ = log(q) S 3 R 3 θ q S 3 R 3 θ q = p ⊕ θ θ = q p q p Figure C
.1: The S 3 manifold is a unit sphere in R 4 , here represented by a unit circle (blue), where all unit quaternions live. The tangent space to the manifold is the hyperplane R 3 , here represented by a line (red). The Exp() and Log() operators map elements of R 3 to/from elements of S 3 . The ⊕ and ⊖ operators relate elements of the manifold with elements in the tangent space. (Likewise, these figures illustrate the SO(3) manifold.)

for both q and R, q = p ⊕ θ p ⊗ Exp(θ) (C.3)

S = R ⊕ θ R Exp(θ) (C.

C.1.5 Examples

Since our defined derivatives map tangent Cartesian spaces, and these spaces coincide for the 3D rotation manifolds of S 3 and SO(3), i.e., θ = log(q) = log(R), it follows that the Jacobians are independent of the representation used (q or R). We can thus consider generic 3D rotation elements, and note them with the sans-serif font R.

C.1.5.1 Function R 3 → S 3

The function f (ω) = Exp(ωδt) produces elements of S 3 from vectors ω ∈ R 3 . Its Jacobian with respect to ω follows from (C.9), but is better obtained from (C.

C.2.3 Derivative blocks

From (4.23) and the properties above, the following scalar derivative blocks become trivial,

J R -1 R = lim δθ→0 (-θ -δθ) -(-θ) δθ = -1 (C.24) J Q•R Q = lim δθ Q →0 (θ Q + δθ Q + θ R ) -(θ Q + θ R ) δθ Q = 1 (C.25) J Q•R R = lim δθ R →0 (θ Q + θ R + δθ R ) -(θ Q + θ R ) δθ R = 1 (C.

1

  Photos of the eyes of different species. . . . . . . . . . . . . . . . . . 2 Vestibular apparatus . . . . . . . . . . . . . . . . . . . . . . . . . . . 3 Humanoid robots . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4 [Siegwart 2011] Examples of robots with multi-sensor systems: (a) HelpMate from Transition Research Corp, (b) B21 from Real World Interface, (c) Roboart II, built by H.R. Everett [Everett 1995], (d) The Savannah River Site nuclear surveillance robot . . . . . . . . . . 5 Simplified representation of the structure of a humanoid robot. This figure only shows one possible design. . . . . . . . . . . . . . . . . . 6 Snapshots of the cheetah robot jumping over an obstacle. . . . . . . 1.1 Representation of 88 trajectories acquired with 15 human subjects who were asked to walk straight forward during an experiment, c Jean-René Cazalets . . . . . . . . . . . . . . . . . . . . . . . 1.2 Representation of the growth of uncertainties in odometry due to sensor imperfections and the environment itself. . . . . . . . . . . . . 1.3 SLAM process representation. . . . . . . . . . . . . . . . . . . . . . . 1.4 Problem representation and relation between data depending on the chosen estimation strategy . . . . . . . . . . . . . . . . . . . . . . . . 1.5 Possible introduction of errors in EKF due to linearisation and the Gaussian distribution assumption. . . . . . . . . . . . . . . . . . . . 1.6 Representation of the DBN focusing on the relationship between states and variables. . . . . . . . . . . . . . . . . . . . . . . . . . . . 1.7 Representation of the DBN focusing on the multi-sensor aspect. . . . 1.8 Representation of the DBN on a full trajectory. . . . . . . . . . . . . 1.9 Factor graph and DBN representations of the same problem. . . . . 2.1 Simplified scheme of gyroscopes and accelerometers in an IMU [Tedaldi 2013]. . . . . . . . . . . . . . . . . . . . . . . . . . . . 2.2 Mechanical representation of an accelerometer and a gyroscope. . . . 2.3 Representation of the misalignment and non-orthogonality of accelerometer and gyroscope axes in an IMU. . . . . . . . . . . . . . . 2.4 Tools designed for calibration purposes . . . . . . . . . . . . . . . . . 3.1 Setting an IMU on the foot of the robot. . . . . . . . . . . . . . . . . 3.2 Detailed factor graph for the initial keyframe and two steps. . . . . . 3.3 A physical interpretation of the deltas. . . . . . . . . . . . . . . . . . 3.4 A representation of the ∆ composition law. . . . . . . . . . . . . . . 3.5 Cyclic phases of foot during walking (source: [Schauer 2016]). . . . . 3.6 Representation of the method used for pedestrian tracking. . . . . . viii List of Figures 3.7 Visualisation of the graph in the pedestrian tracking case using only zero-velocity detection. . . . . . . . . . . . . . . . . . . . . . . . . . . 68 3.8 Estimation results during human 8-shaped walking phases with an IMU attached to a foot. . . . . . . . . . . . . . . . . . . . . . . . . . 69 3.9 Estimation results during a curved walk with an IMU attached to a foot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 70 3.10 Estimation results during human walking with an IMU attached to a foot. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 78 3.11 Representation of the flexibility between the robot's foot and ankle in HRP-2 [Mifsud 2017]. . . . . . . . . . . . . . . . . . . . . . . . . . 79 3.12 IMU on the robot's foot . . . . . . . . . . . . . . . . . . . . . . . . . 79 3.13 A quick insight to tests currently being conducted on the robot. . . . 80 3.14 Possible factor graph from an experiment conducted with the robot. 80 3.15 HRP2's drift during the 'walk in place' test. . . . . . . . . . . . . . . 81 3.16 Position and velocity estimation of the foot of the HRP-2 robot while walking in place. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 82 3.17 Position estimation of the foot of the HRP-2 robot while walking in place with kinematic odometry. . . . . . . . . . . . . . . . . . . . . . 82 3.18 Bias and orientation estimation of the IMU on the foot of the HRP-2 robot while walking in place. . . . . . . . . . . . . . . . . . . . . . . 83 3.19 Position and velocity estimation of the foot of the HRP-2 robot while walking forward for 1 metre. . . . . . . . . . . . . . . . . . . . . . . . 84 3.20 Bias and orientation estimation of the IMU on the foot of the HRP-2 robot while walking in place. . . . . . . . . . . . . . . . . . . . . . . 85 4.1 Representation of the relation between the Lie group and the Lie algebra. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 89 4.2 A manifold M and a linear vector space T M ≃ R 2 tangent at the point X , and a convenient side-cut. . . . . . . . . . . . . . . . . . . . 90 4.3 A representation of the S 3 manifold. . . . . . . . . . . . . . . . . . . 91 4.4 Let a point z ∈ S 1 move at constant rotation rate ω, z(t) =

  . . . . . . . . . . . . . . . . . . . . . 92 4.5 Derivative of functions acting between two manifolds N and M. . . 95 4.6 Motion integration on a manifold. . . . . . . . . . . . . . . . . . . . . 99 4.7 Graph-based motion estimation with self-calibration. . . . . . . . . . 102

Figure 1 :

 1 Figure 1: Photos of the eyes of different species.

Figure 2 :

 2 Figure 2: Vestibular apparatus. Tomography 3D of the vestibular system. The yellow parts are the three semicircular canals in charge of sensing rotational movements. Otolithic organs are located at the base of the semicircular canal system. They sense the linear accelerations of the head.

Figure 3 :

 3 Figure 3: Humanoid robots. Illustration of some humanoid robots cited in this manuscript.

Figure 4 :

 4 Figure 4: [Siegwart 2011] Examples of robots with multi-sensor systems: (a) HelpMate from Transition Research Corp, (b) B21 from Real World Interface, (c) Roboart II, built by H.R. Everett [Everett 1995], (d) The Savannah River Site nuclear surveillance robot

Figure 5 :

 5 Figure 5: Simplified representation of the structure of a humanoid robot. This figure only shows one possible design.

Figure 6 :

 6 Figure 6: Snapshots of the cheetah robot jumping over an obstacle.

10Chapter 1 .

 1 Figure 1.1: Representation of 88 trajectories acquired with 15 human subjects who were asked to walk straight forward during an experiment, c Jean-René Cazalets

Figure 1

 1 Figure 1.3: SLAM process representation. 1:The problem is initialised. New landmarks are detected and added in the map with their respective covariance due to sensor specifications. 2: Computation of the predicted states for the state composed of both robot's and landmarks' pose. These states are computed using the control sequence between 1 and 2 only thus resulting in an increase of pose uncertainties for the estimated states. 3: Landmarks are detected and a data association method is used to assign the landmarks to those already present in the map. Assuming that the data association is correct for 100 % of the landmarks, the new observation is used to compute the most probable state for all the states. Newly detected landmarks are added to the map. 4: The pose correction is associated to a decrease of uncertainties represented by the ellipses boundaries.

Chapter 1 .Figure 1 . 4 :

 114 Figure 1.4: Problem representation and relation between data depending on the chosen estimation strategy [Strasdat 2010]. (a): Bayesian network for a usual SLAM/SfM problem. (b): Equivalent graph problem represented as a Markov random field without the measurements z i (c): Visualisation of the effect of marginalisation on the inference in filtering methods (d): Visualisation of the effect of estimating selective states in key-frame-based bundle adjustment techniques on inference.

Figure 1 . 5 :

 15 Figure 1.5: Possible introduction of errors in EKF due to linearisation and the Gaussian distribution assumption. (Left): In the case of the linear system the EKF works as well as the EKF. (Centre): The linearisation with Gaussian approximation works fine because the system is almost linear in this region. (Right):The system is far from linear in the region of linearisation. Linearisation with the Gaussian approximation is not suited for this case. EKF will perform the operation sketched in dashed line to estimate the output while the true density is represented by the solid line. As a result, the estimates are biased and the filter diverges.(Source:[Sola 2007])

Figure 1 . 7 :

 17 Figure1.7: Representation of the DBN focusing on the multi-sensor aspect. The state x i is not only dependent of the prior x i-1 (prior), but also on J control sequences u j=1:J i

Figure 2

 2 Figure 2.1:Simplified scheme of gyroscopes and accelerometers in an IMU[START_REF] Tedaldi | IMU calibration without mechanical equipment[END_REF]].

  Figure 2.2: Mechanical representation of an accelerometer and a gyroscope.

Figure 2 .

 2 Figure 2.3: Representation of the misalignment and non-orthogonality of accelerometer and gyroscope axes in an IMU [Tedaldi 2013]. The 3D frame formed by the set of axes (x B , y B , z B ) is orthogonal while the frame formed by the axes (x S , y S , z S ) is misaligned and non-orthogonal.

  (a) 3-Axis Rate

Figure 2 . 4 :

 24 Figure 2.4: Tools designed for calibration purposes

Contents 3 . 1

 31 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.1.1 Context . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.1.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 52 3.1.3 Methodology . . . . . . . . . . . . . . . . . . . . . . . . . . . 55 3.1.4 Contributions . . . . . . . . . . . . . . . . . . . . . . . . -based iterative optimisation . . . . . . . . . . . . . . . 56 3.2.2 Where to put Keyframes? . . . . . . . . . . . . . . . . . . . . 57 3.2.3 Description of factors . . . . . . . . . . . . . . . . . . . . . . 58 3.3 IMU pre-integration in S 3 and SO(3) . . . . . . . . . . . . . 59 3.3.1 State integration in the absolute reference frame . . . . . . . 59 3.3.2 Delta definitions . . . . . . . . . . . . . . . . . . . . . . . . . 60 3.3.3 Incremental delta pre-integration . . . . . . . . . . . . . . . . 62 3.3.4 Jacobians . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62 3.3.5 Incremental delta covariance integration . . . . . . . . . . . . 63 3.3.6 Delta correction with new bias . . . . . . . . . . . . . . . . . 63 3.3.7 Residuals . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64 3.4.2 Related Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 65 3.4.3 Experiments . . . . . . . . . . . . . . . . . . . . . . . . . . . 66 3.4.4 Conclusion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 71 3.5.2 Experimental setup . . . . . . . . . . . . . . . . . . . . . . . . 52 Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation

Figure 3

 3 Figure 3.1: The flying foot trajectory is reconstructed through an IMU set on the foot (in green), and by fusing the information coming from the kinematic chain from the support foot to the flying one (in light brown). In the middle images (a)-(d) shows slippage while performing a multicontacts motion depicted on the right (e) (from [Carpentier 2016]).

Figure 3

 3 Figure3.3: A physical interpretation of the deltas. The blue frame G i is the initial frame at time t = i. At this moment, the IMU is moved with an instant velocity v i from the pose given by {p i , R i }. If the rigid body the IMU is attached on does not move with a proper acceleration and rate of turn from time i to j, then the trajectory of the IMU would be given by the free-falling frame and the device would be on the frame G j at time t = j. However if the IMU moves with a proper acceleration and rate of turn, then its pose at time j is given by ∆ ij with respect to the free-falling frame.

Figure 3 . 4 :

 34 Figure 3.4: A representation of the ∆ composition law.

Figure 3 . 5 :

 35 Figure 3.5: Cyclic phases of foot during walking (source: [Schauer 2016]).

Figure 3 . 6 :

 36 Figure 3.6: Representation of the method used for pedestrian tracking.

  Figure 3.7: Visualisation of the graph in the pedestrian tracking case using only zero-velocity detection. The same graph can be used by adding other factors due to information provided by other possible sensors.

Figure 3 . 8 :

 38 Figure3.8: Top: trajectory estimation during human 8-shaped walking phases with an IMU attached to a foot. Stars in blue and red colours are respectively the reference estimated states provided in the dataset and the estimation with our approach. The continuous green line is the ground truth as given by the motion capture system. Bottom: Evolution of Euclidean distance errors between estimates and the ground truth for this 8-shaped walking trajectory.

Figure 3 . 9 :

 39 Figure 3.9: Evolution of estimated IMU biases and orientation of the device during a curved walk. Biases are estimated only for keyframes and considered as constant between them.

Figure 3

 3 Figure3.10: Top: trajectory estimation during human walking with an IMU attached to a foot. Continuous, dashed and dashed-dot lines are respectively: ground truth, estimation with zero velocity constraints only, estimation using zero velocity constraints and 1 odometry measurement during foot's flying phase. Odometry was here built from motion capture data. Bottom: Evolution of corresponding estimated IMU biases

Figure 3 .

 3 Figure3.12: The IMU (in a white 3D-printed case) is fixed to the foot and connected to the STM32 microcontroller by I2C.

  Figure3.14: Possible factor graph from an experiment conducted with the robot. This case depicts the decision not to create a kinematic factor between key-frames 1 and 2. The key-frame created while the foot was in the air does not have any constraint on the velocity.

Figure 3 .Figure 3

 33 Figure3.15: HRP2 drifts during the 'walk in place' test. We can notice the difference in both position and orientation between the initial and final poses after performing 5 steps with both right and left foots. The position drift is higher than the orientation one.

Figure 3 .

 3 Figure 3.18: Estimation of accelerometer bias a b (Top Left), gyrometer bias ω b (Top Right) and orientation (Bottom) of the IMU with respect to time while walking in place. Each diamond on the graphs represents a key-frame while bars under and over provide a way to visualise the 2σ bounds around the estimates.Although it is difficult to evaluate the estimation of the biases, the stability of both accelerometer and gyrometer biases may tend to give the impression of a reliable estimate. However, this stability is due also to constraints imposed over the evolution of the biases through time. Furthermore, we note that the Oy orientation is slowly drifting during contact phases despite the fact that the foot is supposed to be still.

Figure 3 .

 3 Figure 3.20: Estimation of accelerometer bias a b (Top Left), gyrometer bias ω b(Top Right) and orientation (Bottom) of the IMU with respect to time while walking 1 metre forward. Each diamond on the graphs represents a key-frame while bars under and over provide a way to visualise the 2σ bounds around the estimates. The stability of both accelerometer and gyrometer biases may tend to give the impression of a reliable estimate. However, this stability is due also to constraints imposed over the evolution of the biases through time. Furthermore, we note that the Oy orientation is slowly drifting during contact phases despite the fact that the foot is supposed to be still. We can also clearly see the effect of the vibrations on the IMU on this graph. The estimated value for a by (-0.55m/s 2 ) is also too big. Hence some doubts remain on the estimation of the biases. We have here one possible explanation for the estimation of the biases given the motion described by the robot and the dynamics applied on the IMU.

Figure 4 .

 4 Figure 4.1: Representation of the relation between the Lie group and the Lie algebra.The Lie algebra T M E (red plane) is the tangent space to the Lie group's manifold M (here represented as a blue sphere) at the identity E. Through the exponential map, each straight path vt through the origin on the Lie algebra produces a path exp(vt) over the manifold which runs along the respective geodesic. Conversely, each element of the group has an equivalent in the Lie algebra. This relation is so profound that (nearly) all operations in the group, which is curved and nonlinear, have an exact equivalent in the Lie algebra, which is a linear vector space. Though the sphere in R 3 is not a Lie group (we just use it as a representation that can be drawn on paper), that in R 4 is, and describes the group of unit quaternions -see Fig.4.3.

Figure 4 . 2 :

 42 Figure 4.2: A manifold M and a linear vector space T M ≃ R 2 tangent at the point X , and a convenient side-cut.

Figure 4 . 4 :

 44 Figure 4.4: Let a point z ∈ S 1 move at constant rotation rate ω, z(t) = cos ωt + i sin ωt. Its velocities when passing through 1 and z are in the respective tangent spaces, T S 1 1 and T S 1 z . In the case of T S 1 z , the velocity is ż = z iω = -ω sin ωt + iω cos ωt when expressed in the global coordinates, and z v ∧ = iω when expressed locally. Their relation is given by z v ∧ = z -1 ż = z * ż. In the case of T S 1 1 , this relation is the identity 1 v ∧ = ż = iω. Clearly, the structure of all tangent spaces is iR, which is the Lie algebra. This is also the structure of ż at the identity, and this is why the Lie algebra is defined as the tangent space at the identity.

Figure 4 . 5 :

 45 Figure 4.5: Derivative of functions acting between two manifolds N and M. The perturbation τ ∈ T N around X ∈ N is propagated to a perturbation σ ∈ T M around f (X ) ∈ M through the processes of plus, apply f (), and minus (arrows). The derivative is simply lim τ →0 σ/τ .

Figure 4

 4 Figure 4.7: Graph-based motion estimation with self-calibration. Pre-integrated factors (black squares) link consecutive KF states and the calibration parameters (circles). Observability is provided by other sensors observing motion at a lower rate (wireframe squares), such as vision or LIDAR. Left: fixed calibration parameters. Right: time-varying calibration parameters. Additional factors limiting parameter drifts are shown in grey. See Fig. 3.2 for a real implementation.

Chapter 4 .

 4 Generalized motion estimation in manifolds, with application to self-calibrationAlgorithm 5: Pre-integration on the deltas manifold D Input: i; c; Y = {y k

  Residual evaluation, Lupton and Forster

Figure 5 . 2 :

 52 Figure 5.2: Snapshots of the climbing up 15-cm high steps motion with the HRP-2 using the stair railing.

Figure 5 . 3 :

 53 Figure5.3: Overview of our two-stage framework. Given a path request between start and goal positions (left image), P1 is the problem of computing a guide path in the space of equilibrium feasible root configurations. We achieve this by defining a geometric condition, the reachability condition (abstracted with the transparent cylinders on the middle image). P2 is then the problem of extending the path into a discrete sequence of contact configurations.

Figure 5 . 4 :

 54 Figure 5.4: Representation of some stages in the HRP-2 standing up simulation. The robot needs to use contacts with its environment to change poses and get to the final goal: standing up.

Figure 5 . 5 :

 55 Figure 5.5: Representation of the WOLF Tree.

  R{q} has the following properties with respect to quaternionsR{q I } = I (B.27) R{-q} = R{q} (B.28) R{q * } = R{q} T (B.29) R{q 1 ⊗ q 2 } = R{q 1 }R{q 2 } (B.30)

  Figure C.2: Representation of the derivatives operation. We consider a situation so that variable can be either in the R n or in one of the manifolds (blue and red spaces). Then we define the functions f : R m → R n , g : S 3 → S 3 , h : R m → S 3 and k : S 3 → R n .

  S 3 × R 3 → R 3The rotation f (R, v) = q ⊙ v = R v (coresponding to rotation action) produces vectors of R 3 from elements R ∈ S 3 and vectors v ∈ R 3 . The first Jacobian is defined by (C.10) and developed asJ v] × δθ δθ = -R [v] ×where we used the properties Exp(δθ) ≈ I + [δθ] × and [a] × b = -[b] × a.The second Jacobian is defined by (C.7) and yields, S 3 × S 3 → S 3The function f (Q, R) = q ⊗ r = Q R produces rotation composition. Its Jacobians are computed from (C.8), using the property Exp(Rθ) = R Exp(θ)R ⊤ ,

20 Chapter 1. Estimation as filtering or online batch-optimisation problems Algorithm 1: BayesFilter

  (x t-1 , u t , z t )

	1: loop
	2:	{Prediction(x t-1 , u t , Σ t-1 ):}
	3:	Get control sequence u t
	4: 5:	p(x t | u 1:t , z 1:t-1 ) = p(x t | x t-1 , u t )p(x t-1 | u 1:t-1 , z 1:t-1 )dx t-1 {Correction(x t , z t ):}
	6:	Get measurements z t
	7: 8:	p(x t | u 1:t , z 1:t ) = η p(z t | x t )p(x t | u 1:t , z 1:t-1 ) return x t , Σ t
	9: end loop
		p(z t |z 1:t-1 , u 1:t )	1:t-1 )	(1.16)

t |u 1:t , z 1:t ) = p(z t |x t )p(x t |x t-1 , u t )p(x 0:t-1 |u 1:t , z

Chapter 2. The technology behind IMUs and

  , (2.10)(2.11) we get the following IMU error model.

  Table Series AC3337-TC from ACUTRONIC. The rate table is specifically designed for testing IMUs. Accuracy and high rate control as well as rate stability are very important features for rate tables.

Graph-based motion estimation on manifolds 103

	4.3.
	.57)

5. Self-calibration example in SE(2) for a differential drive 111

  assuming no wheel slippage. In case the robot follows a straight trajectory, we have δθ → 0, and R → +∞. This case is handled by means of the approximation

	Jacobian of δ from (4.72) as
	δx = δl cos(δθ/2)
	δy = δl sin(δθ/2)	(4.73)
	δθ = δθ .

As it is done for the IMU case in SO(3) (Section 3.3.4.2), we can define the 4.

In a Lie algebra, the vector space is endowed with a non-associative product called the Lie bracket. In this work, we will not make use of it.

For additive Lie groups the constraint X -X = 0 differentiates to Ẋ = Ẋ , that is, no constraint affects the tangent space. This means that the tangent space is the same as the group space.

A plain definition cov(X ) E[(X -X )(X -X ) ⊤ ] is always ill-defined if size(X ) > dim(N ),which is the case for most non-trivial manifolds.

Acknowledgments

For this work, I have been given the title of 'Doctor'. But It would be unfair to say that I did it alone. It is now my pleasure to thank all the people who contributed to it. Some of them already know that they helped me. But many simply do not realise it! Below will be given only an incomplete list of the people who made it possible to stand for three years.

Chapter 2

The technology behind IMUs Contents Chapter 3. Graph based IMU preintegration on the S 3 manifold, with application to localisation account a specific user and motion types. Prior information can be exploited when merging the measurements of several IMUs, for example relative to the maximum step length the pedestrian could do when using two foot-mounted IMUs [START_REF] Skog | [END_REF]].

Thus the inequality constraints limit the distance between both IMUs to give better position estimation results when compared to single foot-mounted IMU solution.

Fusion strategies are also used to overcome the drift observed in methods using IMU and to obtain positioning errors of approximately 1.5 m [START_REF] Ruiz | [END_REF]].

In [Chdid 2011], a foot-mounted IMU is fused with a waist-mounted visual odometry system to update the state of the system composed of its position, velocity and acceleration.

More information can be structured into a map following a SLAM approach [START_REF] Angermann | [END_REF]], leading to a bounded error growth to 1 metre. This FootSLAM uses dynamic Bayesian network and loop-closure strategies. The idea exploited here is to use the normal human behaviour consisting in relying on visual information to guide the motion and avoid obstacles. FootSLAM's idea gave birth to several variants for use in different conditions or slightly different purposes ( [Puyol 2012[START_REF] Bruno | [END_REF]). Hardegger et al. use contextual information to bodymounted IMUs with a FastSLAM-base implementation in ActionSLAM, landmarks being location-related actions [START_REF] Hardegger | [END_REF]]. A foot-mounted IMU is then used not only for inertial navigation purposes but also as a landmark observation system through action recognition strategies by applying machine learning techniques for motion classification. The main idea behind ActionSLAM is to consider that some specific actions are done only at some specific locations on the map. Results tend to show that using both foot-mounted and wrist-mounted IMUs is giving results that are robust enough for indoor applications.

Previously cited examples tend to show how important it is for pedestrian inertial navigation system to be able to deal with the localisation drifts due to the integration of IMU's data. Two different methods have been aforementioned to reach this goal: fusion strategies using different complementary sensors and IMU only based methods using specificities of human behaviour (walking patterns, action recognition, contextual information).

From this analysis, we see that two important aspects have been investigated to solve pedestrian localisation: i) exploiting some specificities of human behaviour with the inertial measurements as prior knowledge and ii) fusing IMU with additional complementary sensors. We show that using a graphical model is a sane and efficient way to encode prior knowledge about the human behaviour (horizontal foot during zero-velocity phases). An additional feature of our approach is that it is easy to extend the graphical model, either with additional prior knowledge, or with measurements coming from additional sensors. For example, fusing absolute but noisy measurements like GPS, RFID or RSSI would be straight forward. Figure 3.19: Position (Top) and velocity (Bottom) estimation of the foot of the HRP-2 robot while walking forward for 1 metre. Each diamond on the graphs represents a key-frame while bars under and over provide a way to visualise the 2σ bounds around the estimates. We can notice that P y and P z drift slightly away from 0. The variance of the zero-velocity constraint being fixed manually to a small value, its evolution is not represented here. The results are coherent with the motion described by the robot for both P x and P y axis. However, Since we suppose the floor to be flat, P z should be 0.

Chapter 4

Generalized motion estimation in manifolds, with application to self-calibration

Contents

Chapter 5 One of the current research focus in GEPETTO team is to face the multi-contact locomotion problem of legged robots in complex environments through the Loco3D project [START_REF] Carpentier | [END_REF]]. While this is clearly a planning and control problem in order to generate precise and complex motions for the robot to navigate and interact with its environment, it is required to close the control loop through onboard perception. SLAM methods are perfect candidates to enhance the control with perception techniques. In this context, the library WOLF (acronym for Windowed

WOLF as part of the Loco3D project

Probabilistic formulation

Hereafter are introduced main notions the reader may need to understand the work presented in this thesis. The presented topics are not exclusive.

A.1 Probability

Probability is a mathematical formulation of knowledge, representing the probability distribution of values that are likely to be taken by random variables. The more likely the variable is to fall in the range of a specific value the higher the corresponding probability will be. Probabilities are given by a function encoding the density of probability that the statement "the random variable X takes a value in the range x ± dx" becomes true relatively to the other values that can be taken. Such function is usually called probability density function (pdf). The probability density that X actually takes the value x is thus given by the function p X (x)

where P (A) ∈ [0, 1] is the probability for the event A to be true. For the sake of clarity, p X (x) will be noted p(x) in the following.

A.2 Expectation

Expectation is a central notion of the probability theory and denotes intuitively the long-run average value one can expect the random variable to take. The expectation of a random variable X is a linear function defined by

From these definitions, we can introduce some other notions that will be used hereafter

where µ x , σ x and σ 2

x are respectively the mean, the standard deviation and the variance of X. They will be noted respectively µ, σ and σ 2 hereafter. An intuitive understanding of these notions is given in the following introduction related to the Gaussian distribution.

A.3 Gaussian distribution

The Gaussian distribution (also called Gauss, Laplace-Gauss or normal distribution) is a continuous probability distribution. It is an important distribution used in statistics to describe the densities distribution of probability density functions, used not only to model measurement errors but also in natural and social sciences. Describing the real value of a random variable whose distribution is actually unknown with a Gaussian one is a common assumption.

The probability density of a 1D random variable x following a normal distribution is described by the mean (or expectation) µ of this distribution and its variance

Note that σ is the standard deviation of the distribution. Its effect on the distribution itself and its meaning are shown in Fig. A.1. The generalised normal distribution for multidimensional random variables is usually described by the mean µ and the covariance Σ. n being the dimension of the random variable x, we have

where Σ is a n × n matrix. We can notice that if n = 1 then (A.8) simplifies to (A.7).

A.4 Bayes' rule

Bayes' rule (also called Bayes' theorem or Bayes' law), describes the probability of an event based on prior knowledge. This law is described by a simple mathematical formulation. Let us note A and B events, P (A) and P (B) are respectively the likelihood for events A and B to occur. Bayes' rule allows one to describe the likelihood A (respectively B) to occur given prior knowledge about B (respectively A) and noted P (A|B) (respectively P (B|A)). Appendix B

Quaternion and rotation representation

The following description of quaternions comes from [START_REF] Ortega | [END_REF].

B.1 Definition of quaternion

Quaternions are a tool amongst others to describe orientations. Lets us note Q a quaternion defined in the space of quaternions H. Q is defined as

where {a, b, c, d} ∈ R and {i, j, k} are three imaginary unit number so that

We define numbers that we will refer to as pure quaternions in the tri-dimensional imaginary subspace of H,

Please note that the definition mentioned above are derived from the specific quaternion definition in Eq. (B.1) where the real part is the first number of the quaternion. An alternative quaternion definition sets this scalar part at the end so that

For a more convenient use, quaternions can be defined as a sum of a scalar number and an imaginary vector so that

where q w is referred to as the scalar part, and q v = q x i + q y j + q z k = (q x , q y , q z ) as the imaginary or vector part. Finally, a quaternion can be defined using a pair of scalar and vector

or also with as a four-dimensional vector q q

and for sake of clarity, we may sometimes define the quaternions using only subscript indices in the following so that q = (w, x, y, z). We may also abuse of the notations so that

where q w and q v are used to represent respectively real quaternions and pure quaternions.

B.2 quaternion properties

B.2.0.3 Sum / difference

The sum and difference of two quaternions are straightforward and defined so that q 1 ± q 2 = q w1 q v1 ± q w2 q v2 = q w1 ± q w2 q v1 ± q v2 (B.9)

The sum is commutative and associative q 1 + q 2 = q 2 + q 1 (B.10)

where the identity operation is operated using the zero quaternion defined by q 0 = 0 and the inverse quaternion is given by the negative -q.

B.2.0.4 Product

The product operator of quaternions is referred to using the symbol ⊗ and defined so that q 1 ⊗ q 2 = q w1 q w2 -q T v1 q v2 q w1 q v2 + q w2 q v1 + q v1 × q v2 (B.12)

The product of quaternions is not commutative in the general case due to use of the cross-product × q 1 ⊗ q 2 = q 2 ⊗ q 1 (B.13)

B.2. quaternion properties 133

The commutativity is, however, true if the cross-product term yields to 0, which is the case when either one of the quaternions is actually a pure real quaternion or when both vector q v1 and q v2 are parallel. The quaternion product is however associative

and distributed over the sum so that

Quaternions endowed with the product operation ⊗ form a non-commutative group in which the identity element is q I = 1 = 1 0 v .

B.2.1 Conjugate

The conjugate of a quaternion is defined by

with the following properties q ⊗ q * = q * ⊗ q = q 2 w + q 2 x + q 2 y + q 2 z 0 v (B.17) (q 1 ⊗ q 2 ) * = q * 1 ⊗ q * 2 (B.18)

B.2.1.1 Norm

The norm of a quaternion is defined by q = q ⊗ q * = q * ⊗ q = q 2 w + q 2 x + q 2 y + q 2 z (B.19) so that q w1 ⊗ q w2 = q w1 q w2 (B.20)

B.2.2 Inverse

The inverse quaternion q -1 is defined so that q ⊗ q = q -1 ⊗ q = q I (B.21) and can be computed with We use vectorized versions of the exponential and logarithmic maps in the rotation groups S 3 (quaternion) and SO(3) (rotation matrix), and denote them with capitalized names Exp() and Log() (see Fig. C.1,left). They operate directly on the vector space R 3 , and use either quaternions for S 3 ,

where (θ, u) ∈ R 3 is the angle-axis representation of the rotation, and q (q w , q v ) ∈

H is a quaternion with its real and imaginary parts. For rotation matrices in SO(3) we have,

, and where • ∨ , known as the vee operator, is the inverse of the skew operator [•] × . Their exact form (q or R) is always clear by the context. Since the quaternion implementation is one of our contributions, in the following we will refer to the rotation groups S 3 and SO(3) with the unique name S 3 , although everything applies equally to SO(3).

C.1.2 The additive and subtractive operators in S 3 and SO(3)

The 'plus' operator, ⊕ : S 3 × R 3 → S 3 , composes a reference element R ∈ S 3 with a (often small) rotation specified by a vector of θ ∈ R 3 that is tangent to the S 3 manifold at R, yielding an element S ∈ S 3 (see Fig. C.1,right). The 'minus' operator, ⊖ : S 3 × S 3 → R 3 is the inverse of the above. These operators are defined C.2. Maps, operators and derivatives of S 1 and SO (2)

141

C.1.6 Adjoint

We have from (4.21)

C.2 Maps, operators and derivatives of S 1 and SO( 2)

C.2.1 Exp and log maps

Lift and retract are implemented via exponential maps directly from the scalar tangent space so(2) = T SO( 2

C.2.2 Inverse, composition

We have

since planar rotations are commutative. It follows that,

(C.23)

Abstract:

Estimation in robotics is an important subject affected by trade-offs between some major critera from which we can cite the computation time and the accuracy. The importance of these two criteria are application-dependent. If the computation time is not important for off-line methods, it becomes critical when the application has to run on real-time. Similarly, accuracy requirements are dependant on the applications. EKF estimators are widely used to satisfy real-time constraints while achieving acceptable accuracies. One sensor widely used in trajectory estimation problems remains the inertial measurement units (IMUs) providing data at a high rate. The main contribution of this thesis is a clear presentation of the preintegration theory yielding in a better use IMUs. We apply this method for estimation problems in both pedestrian and humanoid robots navigation to show that real-time estimation using a lowcost IMU is possible with smoothing methods while formulating the problems with a factor graph. We also investigate the calibration of the IMUs as it is a critical part of those sensors. All the development made during this thesis was thought with a visual-inertial SLAM background as a mid-term perspective. Firthermore, this work tries to rise another question when it comes to legged robots. In opposition to their usual architecture, could we use multiple lowcost IMUs on the robot to get valuable information about the motion being executed? Keywords:

Factor Graph, low-cost IMU, sensor calibration, real-time state estimation, SLAM, pedestrian navigation, IMU preintegration on manifolds, robot