PID-control on both angle and angular rate