Initial WP5 software ready for integration on real robot

Summary
This Deliverable corresponds to the initial human belief manager based on Perspectivetaking and geometric reasoning as well as a more elaborate version of the humanaware navigation planner