From be15d3d898dfa713ca1a71e9e24754aa2f99e1e4 Mon Sep 17 00:00:00 2001 From: bresch <[brescianimathieu@gmail.com](mailto:brescianimathieu@gmail.com)> Date: Fri, 24 Nov 2023 14:57:53 +0100 Subject: [PATCH] WIP: migrate range height control to use terrain state --- src/modules/ekf2/EKF/common.h | 2 - src/modules/ekf2/EKF/ekf.h | 3 - src/modules/ekf2/EKF/ekf_helper.cpp | 10 +- src/modules/ekf2/EKF/mag_control.cpp | 2 +- src/modules/ekf2/EKF/range_height_control.cpp | 56 +------- src/modules/ekf2/EKF/terrain_estimator.cpp | 123 ------------------ 6 files changed, 7 insertions(+), 189 deletions(-) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index aba810ceb4..741417d673 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -423,10 +423,8 @@ struct parameters { float range_delay_ms{5.0f}; ///< range finder measurement delay relative to the IMU (mSec) float range_noise{0.1f}; ///< observation noise for range finder measurements (m) float range_innov_gate{5.0f}; ///< range finder fusion innovation consistency gate size (STD) - float rng_hgt_bias_nsd{0.13f}; ///< process noise for range height bias estimation (m/s/sqrt(Hz)) float rng_sens_pitch{0.0f}; ///< Pitch offset of the range sensor (rad). Sensor points out along Z axis when offset is zero. Positive rotation is RH about Y axis. float range_noise_scaler{0.0f}; ///< scaling from range measurement to noise (m/m) - const float vehicle_variance_scaler{0.0f}; ///< gain applied to vehicle height variance used in calculation of height above ground observation variance float max_hagl_for_range_aid{5.0f}; ///< maximum height above ground for which we allow to use the range finder as height source (if rng_control == 1) float max_vel_for_range_aid{1.0f}; ///< maximum ground velocity for which we allow to use the range finder as height source (if rng_control == 1) float range_aid_innov_gate{1.0f}; ///< gate size used for innovation consistency checks for range aid fusion diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 7c4d0a1968..1eba95c171 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -837,10 +837,7 @@ private: void runTerrainEstimator(const imuSample &imu_delayed); void predictHagl(const imuSample &imu_delayed); - float getTerrainVPos() const { return isTerrainEstimateValid() ? _terrain_vpos : _last_on_ground_posD; } - void controlHaglFakeFusion(); - void terrainHandleVerticalPositionReset(float delta_z); # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 7d0510000e..58923df9f6 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -207,13 +207,9 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_v #if defined(CONFIG_EKF2_GNSS) _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + delta_z); #endif // CONFIG_EKF2_GNSS -#if defined(CONFIG_EKF2_RANGE_FINDER) - _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + delta_z); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_TERRAIN) - terrainHandleVerticalPositionReset(delta_z); -#endif +#if defined(CONFIG_EKF2_RANGE_FINDER) || defined(CONFIG_EKF2_OPTICAL_FLOW) + _state.terrain_vpos += delta_z; +#endif // CONFIG_EKF2_RANGE_FINDER || CONFIG_EKF2_OPTICAL_FLOW // Reset the timout timer _time_last_hgt_fuse = _time_delayed_us; diff --git a/src/modules/ekf2/EKF/mag_control.cpp b/src/modules/ekf2/EKF/mag_control.cpp index cd714e91ec..83c66f6ff2 100644 --- a/src/modules/ekf2/EKF/mag_control.cpp +++ b/src/modules/ekf2/EKF/mag_control.cpp @@ -130,7 +130,7 @@ bool Ekf::checkHaglYawResetReq() const // Check if height has increased sufficiently to be away from ground magnetic anomalies // and request a yaw reset if not already requested. static constexpr float mag_anomalies_max_hagl = 1.5f; - const bool above_mag_anomalies = (getTerrainVPos() - _state.pos(2)) > mag_anomalies_max_hagl; + const bool above_mag_anomalies = (getTerrainVertPos() - _state.pos(2)) > mag_anomalies_max_hagl; return above_mag_anomalies; } #endif // CONFIG_EKF2_TERRAIN diff --git a/src/modules/ekf2/EKF/range_height_control.cpp b/src/modules/ekf2/EKF/range_height_control.cpp index 177b841393..f1a7f9789d 100644 --- a/src/modules/ekf2/EKF/range_height_control.cpp +++ b/src/modules/ekf2/EKF/range_height_control.cpp @@ -112,17 +112,8 @@ void Ekf::controlRangeHeightFusion() innov_gate, aid_src); - // update the bias estimator before updating the main filter but after - // using its current state to compute the vertical position innovation - if (measurement_valid && _range_sensor.isDataHealthy()) { - bias_est.setMaxStateNoise(sqrtf(measurement_var)); - bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd); - bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(State::pos.idx + 2, State::pos.idx + 2)); - } - // determine if we should use height aiding - const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable(); - const bool continuing_conditions_passing = ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid) + const bool continuing_conditions_passing = (_params.rng_ctrl == RngCtrl::ENABLED); && measurement_valid && _range_sensor.isDataHealthy(); @@ -142,8 +133,8 @@ void Ekf::controlRangeHeightFusion() ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; - resetVerticalPositionTo(-(measurement - bias_est.getBias())); - bias_est.setBias(_state.pos(2) + measurement); + resetVerticalPositionTo(-(measurement - _state.terrain_vpos)); + _state.terrain_vpos = _state.pos(2) + measurement; // reset vertical velocity resetVerticalVelocityToZero(); @@ -200,47 +191,6 @@ void Ekf::controlRangeHeightFusion() } } -bool Ekf::isConditionalRangeAidSuitable() -{ -#if defined(CONFIG_EKF2_TERRAIN) - if (_control_status.flags.in_air - && _range_sensor.isHealthy() - && isTerrainEstimateValid()) { - // check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching - // Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice - float range_hagl_max = _params.max_hagl_for_range_aid; - float max_vel_xy = _params.max_vel_for_range_aid; - - const float hagl_innov = _aid_src_terrain_range_finder.innovation; - const float hagl_innov_var = _aid_src_terrain_range_finder.innovation_variance; - - const float hagl_test_ratio = (hagl_innov * hagl_innov / (sq(_params.range_aid_innov_gate) * hagl_innov_var)); - - bool is_hagl_stable = (hagl_test_ratio < 1.f); - - if (!_control_status.flags.rng_hgt) { - range_hagl_max = 0.7f * _params.max_hagl_for_range_aid; - max_vel_xy = 0.7f * _params.max_vel_for_range_aid; - is_hagl_stable = (hagl_test_ratio < 0.01f); - } - - const float range_hagl = _terrain_vpos - _state.pos(2); - - const bool is_in_range = (range_hagl < range_hagl_max); - - bool is_below_max_speed = true; - - if (isHorizontalAidingActive()) { - is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy); - } - - return is_in_range && is_hagl_stable && is_below_max_speed; - } -#endif // CONFIG_EKF2_TERRAIN - - return false; -} - void Ekf::stopRngHgtFusion() { if (_control_status.flags.rng_hgt) { diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 7e450f1253..23bf9ad326 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -45,11 +45,6 @@ void Ekf::initHagl() { - // assume a ground clearance - _terrain_vpos = _state.pos(2) + _params.rng_gnd_clearance; - - // use the ground clearance value as our uncertainty - _terrain_var = sq(_params.rng_gnd_clearance); #if defined(CONFIG_EKF2_RANGE_FINDER) _aid_src_terrain_range_finder.time_last_fuse = _time_delayed_us; @@ -73,38 +68,6 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) // as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior. initHagl(); } - - predictHagl(imu_delayed); - -#if defined(CONFIG_EKF2_RANGE_FINDER) - controlHaglRngFusion(); -#endif // CONFIG_EKF2_RANGE_FINDER - -#if defined(CONFIG_EKF2_OPTICAL_FLOW) - controlHaglFlowFusion(); -#endif // CONFIG_EKF2_OPTICAL_FLOW - - controlHaglFakeFusion(); - - // constrain _terrain_vpos to be a minimum of _params.rng_gnd_clearance larger than _state.pos(2) - if (_terrain_vpos - _state.pos(2) < _params.rng_gnd_clearance) { - _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); - } -} - -void Ekf::predictHagl(const imuSample &imu_delayed) -{ - // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle - - // process noise due to errors in vehicle height estimate - _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_p_noise); - - // process noise due to terrain gradient - _terrain_var += sq(imu_delayed.delta_vel_dt * _params.terrain_gradient) - * (sq(_state.vel(0)) + sq(_state.vel(1))); - - // limit the variance to prevent it becoming badly conditioned - _terrain_var = math::constrain(_terrain_var, 0.0f, 1e4f); } #if defined(CONFIG_EKF2_RANGE_FINDER) @@ -183,13 +146,6 @@ void Ekf::controlHaglRngFusion() } } -float Ekf::getRngVar() const -{ - return fmaxf(P(State::pos.idx + 2, State::pos.idx + 2) * _params.vehicle_variance_scaler, 0.0f) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()); -} - void Ekf::resetHaglRng() { _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); @@ -343,81 +299,6 @@ void Ekf::resetHaglFlow() _aid_src_terrain_optical_flow.time_last_fuse = _time_delayed_us; } -void Ekf::fuseFlowForTerrain(estimator_aid_source2d_s &flow) -{ - flow.fused = true; - - const float R_LOS = flow.observation_variance[0]; - - // calculate the height above the ground of the optical flow camera. Since earth frame is NED - // a positive offset in earth frame leads to a smaller height above the ground. - float range = predictFlowRange(); - - const float state = _terrain_vpos; // linearize both axes using the same state value - Vector2f innov_var; - float H; - sym::TerrEstComputeFlowXyInnovVarAndHx(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &innov_var, &H); - innov_var.copyTo(flow.innovation_variance); - - if ((flow.innovation_variance[0] < R_LOS) - || (flow.innovation_variance[1] < R_LOS)) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - _terrain_var = 100.0f; - return; - } - - // run the innovation consistency check and record result - setEstimatorAidStatusTestRatio(flow, math::max(_params.flow_innov_gate, 1.f)); - - _innov_check_fail_status.flags.reject_optflow_X = (flow.test_ratio[0] > 1.f); - _innov_check_fail_status.flags.reject_optflow_Y = (flow.test_ratio[1] > 1.f); - - // if either axis fails we abort the fusion - if (flow.innovation_rejected) { - return; - } - - // fuse observation axes sequentially - for (uint8_t index = 0; index <= 1; index++) { - if (index == 0) { - // everything was already computed above - - } else if (index == 1) { - // recalculate innovation variance because state covariances have changed due to previous fusion (linearise using the same initial state for all axes) - sym::TerrEstComputeFlowYInnovVarAndH(state, _terrain_var, _state.quat_nominal, _state.vel, _state.pos(2), R_LOS, FLT_EPSILON, &flow.innovation_variance[1], &H); - - // recalculate the innovation using the updated state - const Vector2f vel_body = predictFlowVelBody(); - range = predictFlowRange(); - flow.innovation[1] = (-vel_body(0) / range) - flow.observation[1]; - - if (flow.innovation_variance[1] < R_LOS) { - // we need to reinitialise the covariance matrix and abort this fusion step - ECL_ERR("Opt flow error - covariance reset"); - _terrain_var = 100.0f; - return; - } - } - - float Kfusion = _terrain_var * H / flow.innovation_variance[index]; - - _terrain_vpos += Kfusion * flow.innovation[0]; - // constrain terrain to minimum allowed value and predict height above ground - _terrain_vpos = fmaxf(_terrain_vpos, _params.rng_gnd_clearance + _state.pos(2)); - - // guard against negative variance - _terrain_var = fmaxf(_terrain_var - Kfusion * H * _terrain_var, sq(0.01f)); - } - - _fault_status.flags.bad_optflow_X = false; - _fault_status.flags.bad_optflow_Y = false; - - flow.time_last_fuse = _time_delayed_us; - flow.fused = true; -} -#endif // CONFIG_EKF2_OPTICAL_FLOW - void Ekf::controlHaglFakeFusion() { if (!_control_status.flags.in_air @@ -467,7 +348,3 @@ bool Ekf::isTerrainEstimateValid() const return valid; } - -void Ekf::terrainHandleVerticalPositionReset(const float delta_z) { - _terrain_vpos += delta_z; -}