, Ingeniarius, Ltd, mailing address: Rua da Vacaria, n.37, 3050-381 Mealhada, Portugal, e-mail address: Institute of Systems and Robotics (ISR), University of Coimbra, e-mail address: micaelcouceiro@isr.uc.pt