From 9001c23926477e215e646efc90f235e5e23d3277 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 20 Jun 2024 14:42:33 +0200 Subject: [PATCH] ekf2: clean up hagl vs terrain naming Terrain is the state: terrain vertical position Hagl (height above ground level) is the vertical distance between the vertical position and the terrain vertical position --- .../optical_flow/optical_flow_control.cpp | 6 ++--- .../range_finder/range_height_control.cpp | 22 +++++++------------ src/modules/ekf2/EKF/control.cpp | 4 ++++ src/modules/ekf2/EKF/covariance.cpp | 5 +++++ src/modules/ekf2/EKF/ekf.cpp | 14 +++++------- src/modules/ekf2/EKF/ekf.h | 17 +++++--------- src/modules/ekf2/EKF/height_control.cpp | 2 +- .../terrain_estimator/terrain_estimator.cpp | 13 ++++------- src/modules/ekf2/Kconfig | 1 - 9 files changed, 36 insertions(+), 48 deletions(-) diff --git a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp index ec2aec1ba4..27e05af756 100644 --- a/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/optical_flow/optical_flow_control.cpp @@ -133,7 +133,7 @@ void Ekf::startFlowFusion() fuseOptFlow(_hagl_sensor_status.flags.flow); } else if (_hagl_sensor_status.flags.flow && !_hagl_sensor_status.flags.range_finder) { - resetHaglFlow(); + resetTerrainToFlow(); } else { ECL_INFO("optical flow fusion failed to start"); @@ -146,7 +146,7 @@ void Ekf::startFlowFusion() resetFlowFusion(); } else if (_hagl_sensor_status.flags.flow) { - resetHaglFlow(); + resetTerrainToFlow(); } } @@ -172,7 +172,7 @@ void Ekf::resetFlowFusion() _aid_src_optical_flow.time_last_fuse = _time_delayed_us; } -void Ekf::resetHaglFlow() +void Ekf::resetTerrainToFlow() { ECL_INFO("reset hagl to flow"); // TODO: use the flow data diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index d085579956..104ec47d91 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -39,7 +39,7 @@ #include "ekf.h" #include "ekf_derivation/generated/compute_hagl_innov_var.h" -void Ekf::controlRangeHeightFusion() +void Ekf::controlRangeHaglFusion() { static constexpr const char *HGT_SRC_NAME = "RNG"; @@ -97,7 +97,7 @@ void Ekf::controlRangeHeightFusion() if (rng_data_ready && _range_sensor.getSampleAddress()) { - updateRangeHeight(aid_src); + updateRangeHagl(aid_src); const bool measurement_valid = PX4_ISFINITE(aid_src.observation) && PX4_ISFINITE(aid_src.observation_variance); const bool continuing_conditions_passing = ((_params.rng_ctrl == static_cast(RngCtrl::ENABLED)) @@ -136,7 +136,7 @@ void Ekf::controlRangeHeightFusion() stopRngTerrFusion(); if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) { - resetHaglRng(aid_src); + resetTerrainToRng(aid_src); } } else if (do_range_aid) { @@ -160,7 +160,7 @@ void Ekf::controlRangeHeightFusion() _control_status.flags.rng_hgt = true; if (!_hagl_sensor_status.flags.flow && aid_src.innovation_rejected) { - resetHaglRng(aid_src); + resetTerrainToRng(aid_src); } } } @@ -193,7 +193,7 @@ void Ekf::controlRangeHeightFusion() stopRngTerrFusion(); } else { - resetHaglRng(aid_src); + resetTerrainToRng(aid_src); } } @@ -213,7 +213,7 @@ void Ekf::controlRangeHeightFusion() } else { if (aid_src.innovation_rejected) { - resetHaglRng(aid_src); + resetTerrainToRng(aid_src); } _hagl_sensor_status.flags.range_finder = true; @@ -230,7 +230,7 @@ void Ekf::controlRangeHeightFusion() } } -void Ekf::updateRangeHeight(estimator_aid_source1d_s &aid_src) +void Ekf::updateRangeHagl(estimator_aid_source1d_s &aid_src) { aid_src.observation = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); aid_src.innovation = getHagl() - aid_src.observation; @@ -266,7 +266,7 @@ float Ekf::getRngVar() const 0.f); } -void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src) +void Ekf::resetTerrainToRng(estimator_aid_source1d_s &aid_src) { _state.terrain = _state.pos(2) + aid_src.observation; P.uncorrelateCovarianceSetVariance(State::terrain.idx, aid_src.observation_variance); @@ -277,8 +277,6 @@ void Ekf::resetHaglRng(estimator_aid_source1d_s &aid_src) bool Ekf::isConditionalRangeAidSuitable() { -#if defined(CONFIG_EKF2_TERRAIN) - // 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; @@ -303,10 +301,6 @@ bool Ekf::isConditionalRangeAidSuitable() } return is_in_range && is_hagl_stable && is_below_max_speed; - -#endif // CONFIG_EKF2_TERRAIN - - return false; } void Ekf::stopRngHgtFusion() diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 34d26273e7..5215570d14 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -144,6 +144,10 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed) // Additional horizontal velocity data from an auxiliary sensor can be fused controlAuxVelFusion(); #endif // CONFIG_EKF2_AUXVEL + // +#if defined(CONFIG_EKF2_TERRAIN) + controlTerrainFakeFusion(); +#endif // CONFIG_EKF2_TERRAIN controlZeroInnovationHeadingUpdate(); diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 2310e644a0..60a865d2de 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -99,6 +99,11 @@ void Ekf::initialiseCovariance() #if defined(CONFIG_EKF2_WIND) resetWindCov(); #endif // CONFIG_EKF2_WIND + +#if defined(CONFIG_EKF2_TERRAIN) + // use the ground clearance value as our uncertainty + P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); +#endif // CONFIG_EKF2_TERRAIN } void Ekf::predictCovariance(const imuSample &imu_delayed) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 61036e4bc7..c16ca24476 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -71,6 +71,11 @@ void Ekf::reset() #if defined(CONFIG_EKF2_WIND) _state.wind_vel.setZero(); #endif // CONFIG_EKF2_WIND + // +#if defined(CONFIG_EKF2_TERRAIN) + // assume a ground clearance + _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; +#endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) _range_sensor.setPitchOffset(_params.rng_sens_pitch); @@ -160,10 +165,6 @@ bool Ekf::update() // control fusion of observation data controlFusionModes(imu_sample_delayed); -#if defined(CONFIG_EKF2_TERRAIN) - runTerrainEstimator(imu_sample_delayed); -#endif // CONFIG_EKF2_TERRAIN - _output_predictor.correctOutputStates(imu_sample_delayed.time_us, _state.quat_nominal, _state.vel, _state.pos, _state.gyro_bias, _state.accel_bias); return true; @@ -199,11 +200,6 @@ bool Ekf::initialiseFilter() // initialise the state covariance matrix now we have starting values for all the states initialiseCovariance(); -#if defined(CONFIG_EKF2_TERRAIN) - // Initialise the terrain state - initHagl(); -#endif // CONFIG_EKF2_TERRAIN - // reset the output predictor state history to match the EKF initial values _output_predictor.alignOutputFilter(_state.quat_nominal, _state.vel, _state.pos); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index ac953e570b..46e03bcaae 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -807,32 +807,27 @@ private: bool fuseVelocity(estimator_aid_source3d_s &vel_aid_src); #if defined(CONFIG_EKF2_TERRAIN) - // terrain vertical position estimator - void initHagl(); - void runTerrainEstimator(const imuSample &imu_delayed); - + void initTerrain(); float getTerrainVPos() const { return isTerrainEstimateValid() ? _state.terrain : _last_on_ground_posD; } - - void controlHaglFakeFusion(); + void controlTerrainFakeFusion(); # if defined(CONFIG_EKF2_RANGE_FINDER) // update the terrain vertical position estimate using a height above ground measurement from the range finder bool fuseHaglRng(estimator_aid_source1d_s &aid_src, bool update_height, bool update_terrain); - void updateRangeHeight(estimator_aid_source1d_s &aid_src); - void resetHaglRng(estimator_aid_source1d_s &aid_src); + void updateRangeHagl(estimator_aid_source1d_s &aid_src); + void resetTerrainToRng(estimator_aid_source1d_s &aid_src); float getRngVar() const; # endif // CONFIG_EKF2_RANGE_FINDER # if defined(CONFIG_EKF2_OPTICAL_FLOW) - // update the terrain vertical position estimate using an optical flow measurement - void resetHaglFlow(); + void resetTerrainToFlow(); # endif // CONFIG_EKF2_OPTICAL_FLOW #endif // CONFIG_EKF2_TERRAIN #if defined(CONFIG_EKF2_RANGE_FINDER) // range height - void controlRangeHeightFusion(); + void controlRangeHaglFusion(); bool isConditionalRangeAidSuitable(); void stopRngHgtFusion(); void stopRngTerrFusion(); diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp index d81c284838..aad5c286b8 100644 --- a/src/modules/ekf2/EKF/height_control.cpp +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -53,7 +53,7 @@ void Ekf::controlHeightFusion(const imuSample &imu_delayed) #endif // CONFIG_EKF2_GNSS #if defined(CONFIG_EKF2_RANGE_FINDER) - controlRangeHeightFusion(); + controlRangeHaglFusion(); #endif // CONFIG_EKF2_RANGE_FINDER checkHeightSensorRefFallback(); diff --git a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp index ea6a4259d1..2afafce2d0 100644 --- a/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator/terrain_estimator.cpp @@ -42,7 +42,7 @@ #include -void Ekf::initHagl() +void Ekf::initTerrain() { // assume a ground clearance _state.terrain = _state.pos(2) + _params.rng_gnd_clearance; @@ -51,7 +51,7 @@ void Ekf::initHagl() P.uncorrelateCovarianceSetVariance(State::terrain.idx, sq(_params.rng_gnd_clearance)); } -void Ekf::runTerrainEstimator(const imuSample &imu_delayed) +void Ekf::controlTerrainFakeFusion() { // If we are on ground, store the local position and time to use as a reference if (!_control_status.flags.in_air) { @@ -62,14 +62,9 @@ void Ekf::runTerrainEstimator(const imuSample &imu_delayed) // Let the estimator run freely before arming for bench testing purposes, but reset on takeoff // because when using optical flow measurements, it is safer to start with a small distance to ground // as an overestimated distance leads to an overestimated velocity, causing a dangerous behavior. - initHagl(); + initTerrain(); } - controlHaglFakeFusion(); -} - -void Ekf::controlHaglFakeFusion() -{ if (!_control_status.flags.in_air && !_hagl_sensor_status.flags.range_finder && !_hagl_sensor_status.flags.flow) { @@ -85,7 +80,7 @@ void Ekf::controlHaglFakeFusion() #endif // CONFIG_EKF2_OPTICAL_FLOW if (_control_status.flags.vehicle_at_rest || !recent_terrain_aiding) { - initHagl(); + initTerrain(); } } } diff --git a/src/modules/ekf2/Kconfig b/src/modules/ekf2/Kconfig index a189934415..93f96a656c 100644 --- a/src/modules/ekf2/Kconfig +++ b/src/modules/ekf2/Kconfig @@ -110,7 +110,6 @@ depends on MODULES_EKF2 bool "optical flow fusion support" default y select EKF2_TERRAIN - depends on EKF2_RANGE_FINDER ---help--- EKF2 optical flow fusion support.