Autonomous Systems and Robotics Lab, Computer Science and System Engineering Department (U2IS), École Nationale Supérieure de Techniques Avancées (ENSTA Paris), Institut Polytechnique de Paris (IP Paris), 828 Boulevard des Maréchaux, 91120 Palaiseau, France