fixed NaN return values of the attitude in the imu message

(1 - cos_alpha) can be a negative value and leads to NaN in the attitude because of the square root