ekf2: add parameters for control of external vision fusion

src/modules/ekf2/ekf2_main.cpp

@@ -168,6 +168,7 @@ private:
 	control::BlockParamFloat _flow_delay_ms;
 	control::BlockParamFloat _rng_delay_ms;
 	control::BlockParamFloat _airspeed_delay_ms;
+	control::BlockParamFloat _ev_delay_ms;
 
 	control::BlockParamFloat _gyro_noise;
 	control::BlockParamFloat _accel_noise;
@@ -220,6 +221,11 @@ private:
 	control::BlockParamFloat _range_innov_gate;	// range finder fusion innovation consistency gate size (STD)
 	control::BlockParamFloat _rng_gnd_clearance;	// minimum valid value for range when on ground (m)
 
+	// vision estimate fusion
+	control::BlockParamFloat _ev_noise;		// observation noise for range finder measurements (m)
+	control::BlockParamFloat _ev_innov_gate;	// range finder fusion innovation consistency gate size (STD)
+	control::BlockParamFloat _ev_gnd_clearance;	// minimum valid value for range when on ground (m)
+
 	// optical flow fusion
 	control::BlockParamFloat
 	_flow_noise;		// best quality observation noise for optical flow LOS rate measurements (rad/sec)
@@ -242,6 +248,9 @@ private:
 	control::BlockParamFloat _flow_pos_x;	// X position of optical flow sensor focal point in body frame (m)
 	control::BlockParamFloat _flow_pos_y;	// Y position of optical flow sensor focal point in body frame (m)
 	control::BlockParamFloat _flow_pos_z;	// Z position of optical flow sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_x;	// X position of VI sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_y;	// Y position of VI sensor focal point in body frame (m)
+	control::BlockParamFloat _ev_pos_z;	// Z position of VI sensor focal point in body frame (m)
 
 	// output predictor filter time constants
 	control::BlockParamFloat _tau_vel;	// time constant used by the output velocity complementary filter (s)
@@ -279,6 +288,7 @@ Ekf2::Ekf2():
 	_flow_delay_ms(this, "EKF2_OF_DELAY", false, &_params->flow_delay_ms),
 	_rng_delay_ms(this, "EKF2_RNG_DELAY", false, &_params->range_delay_ms),
 	_airspeed_delay_ms(this, "EKF2_ASP_DELAY", false, &_params->airspeed_delay_ms),
+	_ev_delay_ms(this, "EKF2_EV_DELAY", false, &_params->ev_delay_ms),
 	_gyro_noise(this, "EKF2_GYR_NOISE", false, &_params->gyro_noise),
 	_accel_noise(this, "EKF2_ACC_NOISE", false, &_params->accel_noise),
 	_gyro_bias_p_noise(this, "EKF2_GYR_B_NOISE", false, &_params->gyro_bias_p_noise),
@@ -318,6 +328,9 @@ Ekf2::Ekf2():
 	_range_noise(this, "EKF2_RNG_NOISE", false, &_params->range_noise),
 	_range_innov_gate(this, "EKF2_RNG_GATE", false, &_params->range_innov_gate),
 	_rng_gnd_clearance(this, "EKF2_MIN_RNG", false, &_params->rng_gnd_clearance),
+	_ev_noise(this, "EKF2_EV_NOISE", false, &_params->ev_noise),
+	_ev_innov_gate(this, "EKF2_EV_GATE", false, &_params->ev_innov_gate),
+	_ev_gnd_clearance(this, "EKF2_MIN_EV", false, &_params->ev_gnd_clearance),
 	_flow_noise(this, "EKF2_OF_N_MIN", false, &_params->flow_noise),
 	_flow_noise_qual_min(this, "EKF2_OF_N_MAX", false, &_params->flow_noise_qual_min),
 	_flow_qual_min(this, "EKF2_OF_QMIN", false, &_params->flow_qual_min),
@@ -335,6 +348,9 @@ Ekf2::Ekf2():
 	_flow_pos_x(this, "EKF2_OF_POS_X", false, &_params->flow_pos_body(0)),
 	_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)),
+	_ev_pos_x(this, "EKF2_EV_POS_X", false, &_params->ev_pos_body(0)),
+	_ev_pos_y(this, "EKF2_EV_POS_Y", false, &_params->ev_pos_body(1)),
+	_ev_pos_z(this, "EKF2_EV_POS_Z", false, &_params->ev_pos_body(2)),
 	_tau_vel(this, "EKF2_TAU_VEL", false, &_params->vel_Tau),
 	_tau_pos(this, "EKF2_TAU_POS", false, &_params->pos_Tau),
 	_gyr_bias_init(this, "EKF2_GBIAS_INIT", false, &_params->switch_on_gyro_bias),

src/modules/ekf2/ekf2_params.c

@@ -108,6 +108,17 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_DELAY, 5);
  */
 PARAM_DEFINE_FLOAT(EKF2_ASP_DELAY, 200);
 
+/**
+ * Vision Position Estimator delay relative to IMU measurements
+ *
+ * @group EKF2
+ * @min 0
+ * @max 300
+ * @unit ms
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_DELAY, 175);
+
 /**
  * Integer bitmask controlling GPS checks.
  * 
@@ -471,10 +482,12 @@ PARAM_DEFINE_INT32(EKF2_REC_RPL, 0);
  * 0 : Set to true to use GPS data if available
  * 1 : Set to true to use optical flow data if available
  * 2 : Set to true to inhibit IMU bias estimation
- *
+ * 3 : Set to true to enable vision position fusion
+ * 4 : Set to true to enable vision yaw fusion
+ * *
  * @group EKF2
  * @min 0
- * @max 15
+ * @max 28
  */
 PARAM_DEFINE_INT32(EKF2_AID_MASK, 1);
 
@@ -520,6 +533,36 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f);
  */
 PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f);
 
+
+/**
+ * Measurement noise for vision estimate fusion
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_NOISE, 0.05f);
+
+/**
+ * Gate size for vision estimate fusion
+ *
+ * @group EKF2
+ * @min 1.0
+ * @unit SD
+ * @decimal 1
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f);
+
+/**
+ * Minimum valid range for the vision estimate
+ *
+ * @group EKF2
+ * @min 0.01
+ * @unit m
+ * @decimal 2
+ */
+PARAM_DEFINE_FLOAT(EKF2_MIN_EV, 0.01f);
 /**
  * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum
  *
@@ -701,7 +744,35 @@ PARAM_DEFINE_FLOAT(EKF2_OF_POS_Y, 0.0f);
 PARAM_DEFINE_FLOAT(EKF2_OF_POS_Z, 0.0f);
 
 /**
- * Time constant of the velocity output prediction and smoothing filter. Controls how tightly the velocity output tracks the EKF states. Set to a negative number to disable the EKF velocity state tracking. This will cause the output velocity to track the output position time derivative.
+* X position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_X, 0.0f);
+
+/**
+ * Y position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_Y, 0.0f);
+
+/**
+ * Z position of VI sensor focal point in body frame
+ *
+ * @group EKF2
+ * @unit m
+ * @decimal 3
+ */
+PARAM_DEFINE_FLOAT(EKF2_EV_POS_Z, 0.0f);
+
+/**
+
+ * Time constant of the velocity output prediction and smoothing filter
  *
  * @group EKF2
  * @max 1.0