AP_InertialNav: freeze horiz_vel when !velned_ok

This addresses concerns about brief failures of get_velocity_NED causing
abrupt changes to the horizonatl velocity estimate.

libraries/AP_InertialNav/AP_InertialNav.cpp

@@ -28,19 +28,21 @@ void AP_InertialNav::update(bool high_vibes)
 
     // get the velocity relative to the local earth frame
     Vector3f velNED;
+
     const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED);
-    /*
-      during high vibration events use vertical position change. If
-      get_velocity_NED() fails we use a zero horizontal velocity
-    */
+    if (velned_ok) {
+        _velocity_cm = velNED * 100; // convert to cm/s
+        _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
+    }
+    //  During high vibration events, or failure of get_velocity_NED, use the
+    //  fallback vertical velocity estimate. For get_velocity_NED failure, freeze
+    //  the horizontal velocity at the last good value.
     if (!velned_ok || high_vibes) {
         float rate_z;
         if (_ahrs_ekf.get_vert_pos_rate_D(rate_z)) {
-            velNED.z = rate_z;
+            _velocity_cm.z = -rate_z * 100; // convert from m/s in NED to cm/s in NEU
         }
     }
-    _velocity_cm = velNED * 100; // convert to cm/s
-    _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
 }
 
 /**