AP_NavEKF : fixed bug in velocity rate of change filter