Rover: use EKF velocity for high rate ground_speed updates

this may give smoother throttle response when the EKF is enabled
AC_WPNav: bug fix for loiter and waypoint update rate

Thanks to EmileC for spotting this
AP_NavEKF: Improved divergence detection and reset

Divergence is now detected by looking for very large changes in the gyro
bias. This will cause the filter to be reset and declared unhealthy for
10 seconds.
AP_NavEKF: Debug Code to log divergence test ratio for flight testing