only some small additions for data recording. safecontroller is also writing to bagfile allowing to find the controller mistake.

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.

supposedly fixed problem caused when multiple Cf-Objects are selected in Vicon. Plus some cosmetic changes

implemented Kalman filter as state estimator, but position is converging very slowly and causes oscillations in x, y and z direction