AP_NavEKF3: Allow accel switch on bias uncertainty to be adjusted

libraries/AP_NavEKF3/AP_NavEKF3.cpp

@@ -516,6 +516,15 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = {
     // @Units: m
     AP_GROUPINFO("RNG_USE_SPD", 47, NavEKF3, _useRngSwSpd, 2.0f),
 
+    // @Param: ACC_BIAS_LIM
+    // @DisplayName: Accelerometer bias limit
+    // @Description: The accelerometer bias state will be limited to +- this vlaue
+    // @Range: 0.5 2.5
+    // @Increment: 0.1
+    // @User: Advanced
+    // @Units: m/s/s
+    AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f),
+
     AP_GROUPEND
 };
 

libraries/AP_NavEKF3/AP_NavEKF3.h

@@ -356,6 +356,7 @@ private:
     AP_Int16 _rngBcnInnovGate;      // Percentage number of standard deviations applied to range beacon innovation consistency check
     AP_Int8  _rngBcnDelay_ms;       // effective average delay of range beacon measurements rel to IMU (msec)
     AP_Float _useRngSwSpd;          // Maximum horizontal ground speed to use range finder as the primary height source (m/s)
+    AP_Float _accBiasLim;           // Accelerometer bias limit (m/s/s)
 
     // Tuning parameters
     const float gpsNEVelVarAccScale;    // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration

libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp

@@ -142,7 +142,7 @@ void NavEKF3_core::setWindMagStateLearningMode()
         // activate the states
         inhibitDelVelBiasStates = false;
         // set the initial covariance values
-        P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
+        P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
         P[14][14] = P[13][13];
         P[15][15] = P[13][13];
     }

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -425,7 +425,7 @@ void NavEKF3_core::CovarianceInit()
     P[11][11] = P[10][10];
     P[12][12] = P[10][10];
     // delta velocity biases
-    P[13][13] = sq(INIT_ACCEL_BIAS_UNCERTAINTY * dtEkfAvg);
+    P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
     P[14][14] = P[13][13];
     P[15][15] = P[13][13];
     // earth magnetic field
@@ -1370,8 +1370,8 @@ void NavEKF3_core::ConstrainStates()
     stateStruct.position.z = constrain_float(stateStruct.position.z,-4.0e4f,1.0e4f);
     // gyro bias limit (this needs to be set based on manufacturers specs)
     for (uint8_t i=10; i<=12; i++) statesArray[i] = constrain_float(statesArray[i],-GYRO_BIAS_LIMIT*dtEkfAvg,GYRO_BIAS_LIMIT*dtEkfAvg);
-    // accel bias limit 1.0 m/s^2	(this needs to be finalised from test data)
-    for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f*dtEkfAvg,1.0f*dtEkfAvg);
+    // the accelerometer bias limit is controlled by a user adjustable parameter
+    for (uint8_t i=13; i<=15; i++) statesArray[i] = constrain_float(statesArray[i],-frontend->_accBiasLim*dtEkfAvg,frontend->_accBiasLim*dtEkfAvg);
     // earth magnetic field limit
     for (uint8_t i=16; i<=18; i++) statesArray[i] = constrain_float(statesArray[i],-1.0f,1.0f);
     // body magnetic field limit

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -52,12 +52,12 @@
 // assume 3m/s to start
 #define STARTUP_WIND_SPEED 3.0f
 
-// initial imu bias error (m/s/s)
-#define INIT_ACCEL_BIAS_UNCERTAINTY 0.2f
-
 // maximum allowed gyro bias (rad/sec)
 #define GYRO_BIAS_LIMIT 0.5f
 
+// initial accel bias uncertainty as a fraction of the state limit
+#define ACCEL_BIAS_LIM_SCALER 0.2f
+
 class AP_AHRS;
 
 class NavEKF3_core