From 925d625ed1554c224af9e3d4000ed2e55224f6a2 Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Sat, 31 Jan 2015 22:29:17 +1100
Subject: [PATCH] AP_NavEKF: fix bug in small EKF velocity fusion

---
 libraries/AP_NavEKF/AP_SmallEKF.cpp | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_NavEKF/AP_SmallEKF.cpp b/libraries/AP_NavEKF/AP_SmallEKF.cpp
index 57adc40a98..f4426fa576 100644
--- a/libraries/AP_NavEKF/AP_SmallEKF.cpp
+++ b/libraries/AP_NavEKF/AP_SmallEKF.cpp
@@ -566,7 +566,9 @@ void SmallEKF::fuseVelocity(bool yawInit)
         // Calculate the velocity measurement innovation using the SmallEKF estimate as the observation
         // if heading isn't aligned, use zero velocity (static assumption)
         if (yawInit) {
-            innovation[obsIndex] = state.velocity[obsIndex] - state.velocity[obsIndex];
+            Vector3f measVelNED;
+            _main_ekf.getVelNED(measVelNED);
+            innovation[obsIndex] = state.velocity[obsIndex] - measVelNED[obsIndex];
         } else {
             innovation[obsIndex] = state.velocity[obsIndex];
         }