Added velocity estimation in the simulated gps and in the simulated sonar
Added velocity estimation in the simulated gps and in the simulated sonar

lis-epfl/MAVRIC_Library,lis-epfl/MAVRIC_Library,lis-epfl/MAVRIC_Library