InertialNav: add get_velocity method
AC_WPNav: add set_loiter_target to use velocity

Target can be set in front of the copter to reduce the bounce back to
the position where loiter was engaged.
Copter: use velocity based set_loiter_target