AP_Math: add sqrt_controller function
AC_AttitudeControl: use sqrt_controller function
AC_PosControl: Use sqrt_controller function
AC_AttitudeControl: cleanup angle_ef_roll_pitch_rate_ef_yaw_smooth