diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index b9fef6e6fa56..5610885d1498 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -416,6 +416,7 @@ struct parameters { float flow_noise{0.15f}; ///< observation noise for optical flow LOS rate measurements (rad/sec) float flow_noise_qual_min{0.5f}; ///< observation noise for optical flow LOS rate measurements when flow sensor quality is at the minimum useable (rad/sec) int32_t flow_qual_min{1}; ///< minimum acceptable quality integer from the flow sensor + int32_t flow_qual_min_gnd{0}; ///< minimum acceptable quality integer from the flow sensor when on ground float flow_innov_gate{3.0f}; ///< optical flow fusion innovation consistency gate size (STD) Vector3f flow_pos_body{}; ///< xyz position of range sensor focal point in body frame (m) diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 9e89016abb60..df8c41ee8651 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -760,9 +760,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; -#if defined(CONFIG_EKF2_RANGE_FINDER) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); -#endif // CONFIG_EKF2_RANGE_FINDER soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_horiz_abs; @@ -1026,15 +1024,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { -#if defined(CONFIG_EKF2_RANGE_FINDER) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); - } else -#endif // CONFIG_EKF2_RANGE_FINDER - if (_control_status.flags.gnd_effect) { + } else if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; diff --git a/src/modules/ekf2/EKF/optflow_fusion.cpp b/src/modules/ekf2/EKF/optflow_fusion.cpp index a061e2c7f121..0f179233cb52 100644 --- a/src/modules/ekf2/EKF/optflow_fusion.cpp +++ b/src/modules/ekf2/EKF/optflow_fusion.cpp @@ -77,6 +77,16 @@ void Ekf::updateOptFlow(estimator_aid_source2d_s &aid_src) const float R_LOS = calcOptFlowMeasVar(_flow_sample_delayed); aid_src.observation_variance[0] = R_LOS; aid_src.observation_variance[1] = R_LOS; + + const Vector24f state_vector = getStateAtFusionHorizonAsVector(); + + Vector2f innov_var; + Vector24f H; + sym::ComputeFlowXyInnovVarAndHx(state_vector, P, range, R_LOS, FLT_EPSILON, &innov_var, &H); + innov_var.copyTo(aid_src.innovation_variance); + + // run the innovation consistency check and record result + setEstimatorAidStatusTestRatio(aid_src, math::max(_params.flow_innov_gate, 1.f)); } void Ekf::fuseOptFlow() diff --git a/src/modules/ekf2/EKF/optical_flow_control.cpp b/src/modules/ekf2/EKF/optical_flow_control.cpp index ae77e228cf48..8b99d2e5c7a0 100644 --- a/src/modules/ekf2/EKF/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/optical_flow_control.cpp @@ -59,7 +59,7 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) // Accumulate autopilot gyro data across the same time interval as the flow sensor const Vector3f delta_angle(imu_delayed.delta_ang - (getGyroBias() * imu_delayed.delta_ang_dt)); - if (_delta_time_of < 0.1f) { + if (_delta_time_of < 0.2f) { _imu_del_ang_of += delta_angle; _delta_time_of += imu_delayed.delta_ang_dt; @@ -70,7 +70,12 @@ void Ekf::controlOpticalFlowFusion(const imuSample &imu_delayed) } if (_flow_data_ready) { - const bool is_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); + int32_t min_quality = _params.flow_qual_min; + if (!_control_status.flags.in_air) { + min_quality = _params.flow_qual_min_gnd; + } + + const bool is_quality_good = (_flow_sample_delayed.quality >= min_quality); const bool is_magnitude_good = !_flow_sample_delayed.flow_xy_rad.longerThan(_flow_sample_delayed.dt * _flow_max_rate); const bool is_tilt_good = (_R_to_earth(2, 2) > _params.range_cos_max_tilt); diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index a44e6cf3a32c..657b989f4882 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -415,10 +415,12 @@ void Ekf::controlHaglFakeFusion() bool Ekf::isTerrainEstimateValid() const { +#if defined(CONFIG_EKF2_RANGE_FINDER) // we have been fusing range finder measurements in the last 5 seconds if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { return true; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index a9fad97550ab..f5e2ae703409 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -154,6 +154,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _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), + _param_ekf2_of_qmin_gnd(_params->flow_qual_min_gnd), _param_ekf2_of_gate(_params->flow_innov_gate), _param_ekf2_of_pos_x(_params->flow_pos_body(0)), _param_ekf2_of_pos_y(_params->flow_pos_body(1)), @@ -1295,6 +1296,10 @@ void EKF2::PublishInnovations(const hrt_abstime ×tamp) _preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps); #if defined(CONFIG_EKF2_OPTICAL_FLOW) _preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow); + + // set dist bottom to scale flow innovation + const float dist_bottom = _ekf.getTerrainVertPos() - _ekf.getPosition()(2); + _preflt_checker.setDistBottom(dist_bottom); #endif // CONFIG_EKF2_OPTICAL_FLOW #if defined(CONFIG_EKF2_EXTERNAL_VISION) @@ -1461,13 +1466,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // Distance to bottom surface (ground) in meters - // constrain the distance to ground to _rng_gnd_clearance - lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); + // Distance to bottom surface (ground) in meters, must be positive + lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); -#endif // CONFIG_EKF2_RANGE_FINDER _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv); diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index a40971e79fbe..c3da91371614 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -662,7 +662,9 @@ class EKF2 final : public ModuleParams, public px4::ScheduledWorkItem (ParamExtFloat) _param_ekf2_of_n_max, ///< worst quality observation noise for optical flow LOS rate measurements (rad/sec) (ParamExtInt) - _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor + _param_ekf2_of_qmin, ///< minimum acceptable quality integer from the flow sensor when in air + (ParamExtInt) + _param_ekf2_of_qmin_gnd, ///< minimum acceptable quality integer from the flow sensor when on ground (ParamExtFloat) _param_ekf2_of_gate, ///< optical flow fusion innovation consistency gate size (STD) (ParamExtFloat) diff --git a/src/modules/ekf2/Utility/PreFlightChecker.cpp b/src/modules/ekf2/Utility/PreFlightChecker.cpp index bf7b62f6aca6..1ac4b2dd2629 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.cpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.cpp @@ -85,7 +85,7 @@ bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_ #if defined(CONFIG_EKF2_OPTICAL_FLOW) if (_is_using_flow_aiding) { - const Vector2f flow_innov = Vector2f(innov.flow); + const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom; Vector2f flow_innov_lpf; flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim); flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim); diff --git a/src/modules/ekf2/Utility/PreFlightChecker.hpp b/src/modules/ekf2/Utility/PreFlightChecker.hpp index 5e9af2c4a96f..a4fe6ab482a1 100644 --- a/src/modules/ekf2/Utility/PreFlightChecker.hpp +++ b/src/modules/ekf2/Utility/PreFlightChecker.hpp @@ -77,6 +77,7 @@ class PreFlightChecker void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; } #if defined(CONFIG_EKF2_OPTICAL_FLOW) void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; } + void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; } #endif // CONFIG_EKF2_OPTICAL_FLOW void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; } void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; } @@ -179,6 +180,7 @@ class PreFlightChecker #if defined(CONFIG_EKF2_OPTICAL_FLOW) bool _is_using_flow_aiding {}; + float _flow_dist_bottom {}; InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad) InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index cf4a49ff7574..34ffdecb5b2e 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -901,7 +901,7 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MIN, 0.15f); PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); /** - * Optical Flow data will only be used if the sensor reports a quality metric >= EKF2_OF_QMIN. + * Optical Flow data will only be used in air if the sensor reports a quality metric >= EKF2_OF_QMIN. * * @group EKF2 * @min 0 @@ -909,6 +909,15 @@ PARAM_DEFINE_FLOAT(EKF2_OF_N_MAX, 0.5f); */ PARAM_DEFINE_INT32(EKF2_OF_QMIN, 1); +/** + * Optical Flow data will only be used on the ground if the sensor reports a quality metric >= EKF2_OF_QMIN_GND. + * + * @group EKF2 + * @min 0 + * @max 255 + */ +PARAM_DEFINE_INT32(EKF2_OF_QMIN_GND, 0); + /** * Gate size for optical flow fusion *