changed the function convertIntoBodyFrame(request, est, state, yaw_measured) such that it uses the measured yaw without considering the setpoint - eliminates the drifting problem but is probably still wrong regarding the controlling logic. Added an rqt-plot to the launch file.