From c4ae5e92d701a41cf9c342a0828e99d8e3cb7198 Mon Sep 17 00:00:00 2001
From: Jonathan Challinger <mr.challinger@gmail.com>
Date: Thu, 12 Feb 2015 17:07:27 -0800
Subject: [PATCH] AP_NavEKF: floor GPS velocity noise at parameter value for
 conservatism

---
 libraries/AP_NavEKF/AP_NavEKF.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
index 07a58f56db..dd1ace023b 100644
--- a/libraries/AP_NavEKF/AP_NavEKF.cpp
+++ b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -27,8 +27,8 @@
  */
 #if APM_BUILD_TYPE(APM_BUILD_ArduCopter)
 // copter defaults
-#define VELNE_NOISE_DEFAULT     1.0f
-#define VELD_NOISE_DEFAULT      1.4f
+#define VELNE_NOISE_DEFAULT     0.5f
+#define VELD_NOISE_DEFAULT      0.7f
 #define POSNE_NOISE_DEFAULT     0.5f
 #define ALT_NOISE_DEFAULT       1.0f
 #define MAG_NOISE_DEFAULT       0.05f
@@ -51,8 +51,8 @@
 
 #elif APM_BUILD_TYPE(APM_BUILD_APMrover2)
 // rover defaults
-#define VELNE_NOISE_DEFAULT     1.0f
-#define VELD_NOISE_DEFAULT      1.4f
+#define VELNE_NOISE_DEFAULT     0.5f
+#define VELD_NOISE_DEFAULT      0.7f
 #define POSNE_NOISE_DEFAULT     0.5f
 #define ALT_NOISE_DEFAULT       1.0f
 #define MAG_NOISE_DEFAULT       0.05f
@@ -75,8 +75,8 @@
 
 #else
 // generic defaults (and for plane)
-#define VELNE_NOISE_DEFAULT     1.0f
-#define VELD_NOISE_DEFAULT      1.4f
+#define VELNE_NOISE_DEFAULT     0.5f
+#define VELD_NOISE_DEFAULT      0.7f
 #define POSNE_NOISE_DEFAULT     0.5f
 #define ALT_NOISE_DEFAULT       0.5f
 #define MAG_NOISE_DEFAULT       0.05f
@@ -1920,9 +1920,9 @@ void NavEKF::FuseVelPosNED()
         // if the GPS is able to report a speed error, we use it to adjust the observation noise for GPS velocity
         // otherwise we scale it using manoeuvre acceleration
         if (gpsSpdAccuracy > 0.0f) {
-            // use GPS receivers reported speed accuracy
-            R_OBS[0] = sq(constrain_float(gpsSpdAccuracy * _gpsHorizVelNoise, 0.05f, 50.0f));
-            R_OBS[2] = sq(constrain_float(gpsSpdAccuracy * _gpsVertVelNoise, 0.05f, 50.0f));
+            // use GPS receivers reported speed accuracy - floor at 0.5m/s reported speed accuracy
+            R_OBS[0] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsHorizVelNoise, _gpsHorizVelNoise, 50.0f));
+            R_OBS[2] = sq(constrain_float(gpsSpdAccuracy/0.5f * _gpsVertVelNoise, _gpsVertVelNoise, 50.0f));
         } else {
             // calculate additional error in GPS velocity caused by manoeuvring
             R_OBS[0] = sq(constrain_float(_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(gpsNEVelVarAccScale * accNavMag);