ekf2: Add tuning for IMU switch-on bias errors

src/modules/ekf2/ekf2_main.cpp

@@ -248,6 +248,10 @@ private:
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
 	control::BlockParamFloat _tau_pos;	// time constant used by the output position complementary filter (s)
 
+	// IMU switch on bias paameters
+	control::BlockParamFloat _gyr_bias_init;	// 1-sigma gyro bias uncertainty at switch-on (rad/sec)
+	control::BlockParamFloat _acc_bias_init;	// 1-sigma accelerometer bias uncertainty at switch-on (m/s**2)
+
 	int update_subscriptions();
 
 };
@@ -332,7 +336,9 @@ Ekf2::Ekf2():
 	_flow_pos_y(this, "EKF2_OF_POS_Y", false, &_params->flow_pos_body(1)),
 	_flow_pos_z(this, "EKF2_OF_POS_Z", false, &_params->flow_pos_body(2)),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
-	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau)
+	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
+	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),
+	_acc_bias_init(this, "EKF2_ABIAS_INIT", false, &_params->switch_on_accel_bias)
 {
 
 }

src/modules/ekf2/ekf2_params.c

@@ -721,3 +721,25 @@ PARAM_DEFINE_FLOAT(EKF2_TAU_VEL, 0.5f);
  * @decimal 2
  */
 PARAM_DEFINE_FLOAT(EKF2_TAU_POS, 0.25f);
+
+/**
+ * 1-sigma IMU gyro switch-on bias
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.2
+ * @unit rad/sec
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_GBIAS_INIT, 0.1f);
+
+/**
+ * 1-sigma IMU accelerometer switch-on bias
+ *
+ * @group EKF2
+ * @min 0.0
+ * @max 0.5
+ * @unit m/s/s
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_ABIAS_INIT, 0.2f);