Use AngularVelocitys like in RidgidBodyState defined in MarsIMU

Correction of the AngularVelocitys returned by the MarsIMU. The AngularVelocitys are now like defined in RidgidBodyState (before in World-Frame).
I'm not sure regarding this, why you not multiply by the quaterion itself, it seems that you are only apliing the yaw-part  and not the complete orientation of the orginial "imu"..

i assuming this would be more correct:

    rbs.angular_velocity =  rbs.orientation * control->nodes->getAngularVelocity( node_id);

maybe with inverse() if needed (seems so)