Modify measurement_server to correctly estimate vessel position and rotation