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..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) @@ -566,8 +570,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), @@ -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), @@ -1153,12 +1158,29 @@ 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)) { - 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])) { 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.