From 6cfee6506043713ad3f5850a49bd1dc8d032661c Mon Sep 17 00:00:00 2001
From: Paul Riseborough <p_riseborough@live.com.au>
Date: Mon, 13 Mar 2017 10:03:20 +1100
Subject: [PATCH] ekf2: Add parameters to tune accelerometer bias learning

These parameters enable the accel bias limiting and learning inhibit logic in the ecl EKF to be easily tuned during testing and replay.
---
 src/modules/ekf2/ekf2_main.cpp | 14 ++++++++--
 src/modules/ekf2/ekf2_params.c | 50 ++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp
index 6505ad2570..599a9b8baa 100644
--- a/src/modules/ekf2/ekf2_main.cpp
+++ b/src/modules/ekf2/ekf2_main.cpp
@@ -297,6 +297,12 @@ private:
 	_mag_bias_saved_variance; ///< Assumed error variance of previously saved magnetometer bias estimates (mGauss**2)
 	BlockParamFloat _mag_bias_alpha;	///< maximum fraction of the learned magnetometer bias that is saved at each disarm
 
+	// EKF accel bias learning control
+	BlockParamExtFloat _acc_bias_lim;	///< TODO
+	BlockParamExtFloat _acc_bias_learn_acc_lim;	///< TODO
+	BlockParamExtFloat _acc_bias_learn_gyr_lim;	///< TODO
+	BlockParamExtFloat _acc_bias_learn_tc;	///< TODO
+
 	// Multi-rotor drag specific force fusion
 	BlockParamExtFloat _drag_noise;	///< observation noise variance for drag specific force measurements (m/sec**2)**2
 	BlockParamExtFloat _bcoef_x;		///< ballistic coefficient along the X-axis (kg/m**2)
@@ -310,8 +316,8 @@ private:
 	BlockParamFloat _K_pstatic_coef_y;	///< static pressure position error coefficient along the Y body axis
 	BlockParamFloat _K_pstatic_coef_z;	///< static pressure position error coefficient along the Z body axis
 
-	// airspeed mode parameter
-	BlockParamInt _airspeed_disabled;
+	BlockParamInt _airspeed_disabled;	///< airspeed mode parameter
+
 };
 
 Ekf2::Ekf2():
@@ -410,6 +416,10 @@ Ekf2::Ekf2():
 	_mag_bias_id(this, "MAGBIAS_ID"),
 	_mag_bias_saved_variance(this, "MAGB_VREF"),
 	_mag_bias_alpha(this, "MAGB_K"),
+	_acc_bias_lim(this, "EKF2_ABL_LIM", false, _params->acc_bias_lim),
+	_acc_bias_learn_acc_lim(this, "EKF2_ABL_ACCLIM", false, _params->acc_bias_learn_acc_lim),
+	_acc_bias_learn_gyr_lim(this, "EKF2_ABL_GYRLIM", false, _params->acc_bias_learn_gyr_lim),
+	_acc_bias_learn_tc(this, "EKF2_ABL_TAU", false, _params->acc_bias_learn_tc),
 	_drag_noise(this, "DRAG_NOISE", true, _params->drag_noise),
 	_bcoef_x(this, "BCOEF_X", true, _params->bcoef_x),
 	_bcoef_y(this, "BCOEF_Y", true, _params->bcoef_y),
diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c
index a8607424e4..4d72bc5312 100644
--- a/src/modules/ekf2/ekf2_params.c
+++ b/src/modules/ekf2/ekf2_params.c
@@ -1162,3 +1162,53 @@ PARAM_DEFINE_FLOAT(EKF2_PCOEF_Y, 0.0f);
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(EKF2_PCOEF_Z, 0.0f);
+
+/**
+ * Accelerometer bias learning limit. The ekf delta velocity bias states will be limited to within a range equivalent to +- of this value.
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.8
+ * @unit m/s/s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABL_LIM, 0.4f);
+
+/**
+ * Maximum IMU accel magnitude that allows IMU bias learning.
+ * If the magnitude of the IMU accelerometer vector exceeds this value, the EKF delta velocity state estimation will be inhibited.
+ * This reduces the adverse effect of high manoeuvre accelerations and IMU nonlinerity and scale factor errors on the delta velocity bias estimates.
+ *
+ * @group EKF2
+ * @min 20.0
+ * @max 200.0
+ * @unit m/s/s
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABL_ACCLIM, 25.0f);
+
+/**
+ * Maximum IMU gyro angular rate magnitude that allows IMU bias learning.
+ * If the magnitude of the IMU angular rate vector exceeds this value, the EKF delta velocity state estimation will be inhibited.
+ * This reduces the adverse effect of rapid rotation rates and associated errors on the delta velocity bias estimates.
+ *
+ * @group EKF2
+ * @min 2.0
+ * @max 20.0
+ * @unit rad/s
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABL_GYRLIM, 3.0f);
+
+/**
+ * Time constant used by acceleration and angular rate magnitude checks used to inhibit delta velocity bias learning.
+ * The vector magnitude of angular rate and acceleration used to check if learning should be inhibited has a peak hold filter applied to it with an exponential decay.
+ * This parameter controls the time constant of the decay.
+ *
+ * @group EKF2
+ * @min 0.1
+ * @max 1.0
+ * @unit s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABL_TAU, 0.5f);