From cd5ec3a3e0b99a78e9275c0886a3aba5d0c32b8b Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p-riseborough@live.com.au>
Date: Fri, 23 Oct 2015 13:41:30 +1100
Subject: [PATCH] AP_NavEKF2: Reduce angle errors when flying without GPS

Turn off aiding during >1g manoeuvres when not using GPS
Use larger velocity and position uncertainties when flying without GPS
---
 .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp    | 45 +++++++++++++------
 1 file changed, 32 insertions(+), 13 deletions(-)

diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
index c2b4ee12e4..05bc2913c3 100644
--- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
+++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
@@ -156,8 +156,14 @@ void NavEKF2_core::SelectVelPosFusion()
         gpsDataDelayed.vel.zero();
         gpsDataDelayed.pos.x = lastKnownPositionNE.x;
         gpsDataDelayed.pos.y = lastKnownPositionNE.y;
-        fuseVelData = true;
-        fusePosData = true;
+        // only fuse synthetic measurements when rate of change of velocity is less than 1g to reduce attitude errors due to launch acceleration
+        if (accNavMag < 9.8f) {
+            fusePosData = true;
+            fuseVelData = true;
+        } else {
+            fusePosData = false;
+            fuseVelData = false;
+        }
     }
 
     // perform fusion
@@ -225,18 +231,30 @@ void NavEKF2_core::FuseVelPosNED()
         // estimate the GPS Velocity, GPS horiz position and height measurement variances.
         // 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 - floor at value set by gps noise parameter
-            R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
-            R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
+        // Use different errors if flying without GPS using synthetic position and velocity data
+        if (PV_AidingMode == AID_NONE && inFlight) {
+            // Assume the vehicle will be flown with velocity changes less than 10 m/s in this mode (realistic for indoor use)
+            // This is a compromise between corrections for gyro errors and reducing angular errors due to maneouvres
+            R_OBS[0] = sq(10.0f);
+            R_OBS[1] = R_OBS[0];
+            R_OBS[2] = R_OBS[0];
+            // Assume a large position uncertainty so as to contrain position states in this mode but minimise angular errors due to manoeuvres
+            R_OBS[3] = sq(25.0f);
+            R_OBS[4] = R_OBS[3];
         } else {
-            // calculate additional error in GPS velocity caused by manoeuvring
-            R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
-            R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise,  0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale  * accNavMag);
+            if (gpsSpdAccuracy > 0.0f) {
+                // use GPS receivers reported speed accuracy if available and floor at value set by gps noise parameter
+                R_OBS[0] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsHorizVelNoise, 50.0f));
+                R_OBS[2] = sq(constrain_float(gpsSpdAccuracy, frontend._gpsVertVelNoise, 50.0f));
+            } else {
+                // calculate additional error in GPS velocity caused by manoeuvring
+                R_OBS[0] = sq(constrain_float(frontend._gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend.gpsNEVelVarAccScale * accNavMag);
+                R_OBS[2] = sq(constrain_float(frontend._gpsVertVelNoise,  0.05f, 5.0f)) + sq(frontend.gpsDVelVarAccScale  * accNavMag);
+            }
+            R_OBS[1] = R_OBS[0];
+            R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
+            R_OBS[4] = R_OBS[3];
         }
-        R_OBS[1] = R_OBS[0];
-        R_OBS[3] = sq(constrain_float(frontend._gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
-        R_OBS[4] = R_OBS[3];
         R_OBS[5] = sq(constrain_float(frontend._baroAltNoise, 0.1f, 10.0f));
 
         // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
@@ -312,7 +330,8 @@ void NavEKF2_core::FuseVelPosNED()
         if (fuseVelData) {
             // test velocity measurements
             uint8_t imax = 2;
-            if (frontend._fusionModeGPS == 1) {
+            // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
+            if (frontend._fusionModeGPS >= 1 || PV_AidingMode != AID_ABSOLUTE) {
                 imax = 1;
             }
             float innovVelSumSq = 0; // sum of squares of velocity innovations