From a4e035d338c40870ae00c382a3e291674db2ac26 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 10 Oct 2019 08:20:16 +0200 Subject: [PATCH] Add param to choose vision observation noise source --- src/modules/ekf2/ekf2_main.cpp | 15 +++++++++------ src/modules/ekf2/ekf2_params.c | 10 ++++++++++ 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 9f8282a899..cfa77509b5 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -422,6 +422,8 @@ private: _param_ekf2_rng_a_igate, ///< gate size used for innovation consistency checks for range aid fusion (STD) // vision estimate fusion + (ParamInt) + _param_ekf2_ev_noise_md, ///< determine source of vision observation noise (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) (ParamFloat) @@ -1162,8 +1164,8 @@ void Ekf2::Run() ev_data.vel(1) = _ev_odom.vy; ev_data.vel(2) = _ev_odom.vz; - // velocity measurement error from parameters - if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) { + // velocity measurement error from ev_data or parameters + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE])) { ev_data.velErr = fmaxf(_param_ekf2_evv_noise.get(), sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VX_VARIANCE], fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VY_VARIANCE], @@ -1180,8 +1182,8 @@ void Ekf2::Run() ev_data.pos(1) = _ev_odom.y; ev_data.pos(2) = _ev_odom.z; - // position measurement error from parameter - if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) { + // position measurement error from ev_data or parameters + if (!_param_ekf2_ev_noise_md.get() && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE])) { ev_data.posErr = fmaxf(_param_ekf2_evp_noise.get(), sqrtf(fmaxf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_X_VARIANCE], _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_Y_VARIANCE]))); @@ -1198,8 +1200,9 @@ void Ekf2::Run() if (PX4_ISFINITE(_ev_odom.q[0])) { ev_data.quat = matrix::Quatf(_ev_odom.q); - // orientation measurement error from parameters - if (PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_ROLL_VARIANCE])) { + // orientation measurement error from ev_data or parameters + if (!_param_ekf2_ev_noise_md.get() + && PX4_ISFINITE(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])) { ev_data.angErr = fmaxf(_param_ekf2_eva_noise.get(), sqrtf(_ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_YAW_VARIANCE])); diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index dc610d3392..d9360f9582 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -701,6 +701,16 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); +/** + * Whether to set the external vision observation noise from the parameter or from vision message + * + * If set to true the observation noise is set from the parameters directly, if set to false the measurement noise is taken from the vision message and the parameter are used as a lower bound. + * + * @boolean + * @group EKF2 + */ +PARAM_DEFINE_INT32(EKF2_EV_NOISE_MD, 0); + /** * Measurement noise for vision position observations used when the vision system does not supply error estimates *