From 3d8f720aaf388b4cc417754573e3832310820848 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 30 Oct 2015 11:20:39 +1100
Subject: [PATCH] AP_NavEKF: Improve initial accel bias learning

Fuse synthetic velocity data at zero before arming to improve stability margins for initial bias offset learning.
---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index a1c3ae279d..e75688bbdf 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -923,12 +923,16 @@ void NavEKF::SelectVelPosFusion()
         // If no height has been received for 200 msec, then fuse anyway so we have a guaranteed minimum aiding rate equivalent to GPS
         // only fuse synthetic measurements when rate of change of velocity is less than 0.5g to reduce attitude errors due to launch acceleration
         // do not use velocity fusion to reduce the effect of movement on attitude
+        if (!vehicleArmed) {
+            fuseVelData = true;
+        } else {
+            fuseVelData = false;
+        }
         if (accNavMag < 4.9f) {
             fusePosData = true;
         } else {
             fusePosData = false;
         }
-        fuseVelData = false;
         // record the fusion time - used to control fusion rate when there is no baro data
         lastConstPosFuseTime_ms = imuSampleTime_ms;
     } else {