From e7512438a6b850b9a36fac2f88d3a81fb7d69824 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 Sep 2019 11:52:30 +1000 Subject: [PATCH 1/3] ekf2: update to latest ecl version Includes compatibility changes for renamed variables. --- src/lib/ecl | 2 +- src/modules/ekf2/ekf2_main.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl b/src/lib/ecl index 62a1e0751206..bb88d4ce5e72 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 62a1e0751206e76ccd482a8b2a0f77550649f39d +Subproject commit bb88d4ce5e72cd5a24d8fdf2959fa85d73a76bfe diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index bf582314e876..c661a0cb1365 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -566,8 +566,8 @@ Ekf2::Ekf2(): _param_ekf2_baro_gate(_params->baro_innov_gate), _param_ekf2_gnd_eff_dz(_params->gnd_effect_deadzone), _param_ekf2_gnd_max_hgt(_params->gnd_effect_max_hgt), - _param_ekf2_gps_p_gate(_params->posNE_innov_gate), - _param_ekf2_gps_v_gate(_params->vel_innov_gate), + _param_ekf2_gps_p_gate(_params->gps_pos_innov_gate), + _param_ekf2_gps_v_gate(_params->gps_vel_innov_gate), _param_ekf2_tas_gate(_params->tas_innov_gate), _param_ekf2_head_noise(_params->mag_heading_noise), _param_ekf2_mag_noise(_params->mag_noise), @@ -1156,9 +1156,9 @@ void Ekf2::run() // check for valid position data if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) { - ev_data.posNED(0) = _ev_odom.x; - ev_data.posNED(1) = _ev_odom.y; - ev_data.posNED(2) = _ev_odom.z; + ev_data.pos(0) = _ev_odom.x; + 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])) { From 4b7218c3b657311400ad41e6f37937caf4659112 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 Sep 2019 19:04:09 +1000 Subject: [PATCH 2/3] ekf2: Add write of odometry velocity data Co-Authored-By: kritz --- src/modules/ekf2/ekf2_main.cpp | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index c661a0cb1365..77d507aabad4 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -1153,6 +1153,23 @@ void Ekf2::run() _ev_odom_sub.copy(&_ev_odom); ext_vision_message ev_data; + // check for valid velocity data + if (PX4_ISFINITE(_ev_odom.vx) && PX4_ISFINITE(_ev_odom.vy) && PX4_ISFINITE(_ev_odom.vz)) { + ev_data.vel(0) = _ev_odom.vx; + 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])) { + 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], + _ev_odom.pose_covariance[_ev_odom.COVARIANCE_MATRIX_VZ_VARIANCE])))); + + } else { + ev_data.velErr = _param_ekf2_evv_noise.get(); + } + } // check for valid position data if (PX4_ISFINITE(_ev_odom.x) && PX4_ISFINITE(_ev_odom.y) && PX4_ISFINITE(_ev_odom.z)) { From ebb0f2dc590605e45e1acfa7b37148d392c1111a Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 24 Sep 2019 20:39:05 +1000 Subject: [PATCH 3/3] ekf2: Modify parameters to enable control of vision velocity fusion --- src/modules/ekf2/ekf2_main.cpp | 11 ++++++--- src/modules/ekf2/ekf2_params.c | 44 +++++++++++++++++++++++----------- 2 files changed, 38 insertions(+), 17 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 77d507aabad4..ade6dd60c660 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -421,10 +421,14 @@ class Ekf2 final : public ModuleBase, public ModuleParams // vision estimate fusion (ParamFloat) _param_ekf2_evp_noise, ///< default position observation noise for exernal vision measurements (m) + (ParamFloat) + _param_ekf2_evv_noise, ///< default velocity observation noise for exernal vision measurements (m/s) (ParamFloat) _param_ekf2_eva_noise, ///< default angular observation noise for exernal vision measurements (rad) - (ParamExtFloat) - _param_ekf2_ev_gate, ///< external vision position innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_evv_gate, ///< external vision velocity innovation consistency gate size (STD) + (ParamExtFloat) + _param_ekf2_evp_gate, ///< external vision position innovation consistency gate size (STD) // optical flow fusion (ParamExtFloat) @@ -601,7 +605,8 @@ Ekf2::Ekf2(): _param_ekf2_rng_a_vmax(_params->max_vel_for_range_aid), _param_ekf2_rng_a_hmax(_params->max_hagl_for_range_aid), _param_ekf2_rng_a_igate(_params->range_aid_innov_gate), - _param_ekf2_ev_gate(_params->ev_innov_gate), + _param_ekf2_evv_gate(_params->ev_vel_innov_gate), + _param_ekf2_evp_gate(_params->ev_pos_innov_gate), _param_ekf2_of_n_min(_params->flow_noise), _param_ekf2_of_n_max(_params->flow_noise_qual_min), _param_ekf2_of_qmin(_params->flow_qual_min), diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index dcc8461d4444..2a483046681d 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -700,7 +700,6 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_GATE, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); - /** * Measurement noise for vision position observations used when the vision system does not supply error estimates * @@ -709,29 +708,26 @@ PARAM_DEFINE_FLOAT(EKF2_MIN_RNG, 0.1f); * @unit m * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.05f); +PARAM_DEFINE_FLOAT(EKF2_EVP_NOISE, 0.1f); /** - * Measurement noise for vision angle observations used when the vision system does not supply error estimates - * + * Measurement noise for vision velocity observations used when the vision system does not supply error estimates * @group EKF2 * @min 0.01 - * @unit rad + * @Unit m/s * @decimal 2 - */ -PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); +*/ +PARAM_DEFINE_FLOAT(EKF2_EVV_NOISE, 0.1f); /** - * Gate size for vision estimate fusion - * - * Sets the number of standard deviations used by the innovation consistency test. + * Measurement noise for vision angle observations used when the vision system does not supply error estimates * * @group EKF2 - * @min 1.0 - * @unit SD - * @decimal 1 + * @min 0.01 + * @unit rad + * @decimal 2 */ -PARAM_DEFINE_FLOAT(EKF2_EV_GATE, 5.0f); +PARAM_DEFINE_FLOAT(EKF2_EVA_NOISE, 0.05f); /** * Measurement noise for the optical flow sensor when it's reported quality metric is at the maximum @@ -1161,6 +1157,26 @@ PARAM_DEFINE_FLOAT(EKF2_RNG_A_HMAX, 5.0f); */ PARAM_DEFINE_FLOAT(EKF2_RNG_A_IGATE, 1.0f); +/** + * Gate size for vision velocity estimate fusion + * Sets the number of standard deviations used by the innovation consistency test. + * @group EKF2 + * @min 1.0 + * @Unit SD + * @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_EVV_GATE, 3.0f); + +/** + * Gate size for vision position fusion + * Sets the number of standard deviations used by the innovation consistency test. + * @group EKF2 + * @min 1.0 + * @Unit SD + * @decimal 1 +*/ +PARAM_DEFINE_FLOAT(EKF2_EVP_GATE, 5.0f); + /** * Specific drag force observation noise variance used by the multi-rotor specific drag force model. * Increasing it makes the multi-rotor wind estimates adjust more slowly.