From 33fd1849e06cc78e96ff442bdf51e2423632174f Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 7 Feb 2022 17:25:20 +0100 Subject: [PATCH] ekf2_terr: refactor terrain estimator - rng aiding --- src/modules/ekf2/EKF/ekf.cpp | 4 +- src/modules/ekf2/EKF/ekf.h | 21 +- src/modules/ekf2/EKF/terrain_estimator.cpp | 256 ++++++++++++++------- 3 files changed, 185 insertions(+), 96 deletions(-) diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 7bc524f4c0..534b4acdd3 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -209,8 +209,8 @@ bool Ekf::initialiseFilter() increaseQuatYawErrVariance(sq(fmaxf(_params.mag_heading_noise, 1.0e-2f))); } - // try to initialise the terrain estimator - _terrain_initialised = initHagl(); + // Initialise the terrain estimator + initHagl(); // reset the essential fusion timeout counters _time_last_hgt_fuse = _time_last_imu; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index b7d213c65a..6ceffd79be 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -386,6 +386,7 @@ private: uint64_t _time_last_fake_pos_fuse{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) + uint64_t _time_last_healthy_rng_data{0}; uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source Vector2f _last_known_posNE{}; ///< last known local NE position vector (m) @@ -535,8 +536,6 @@ private: float _terrain_var{1e4f}; ///< variance of terrain position estimate (m**2) uint8_t _terrain_vpos_reset_counter{0}; ///< number of times _terrain_vpos has been reset uint64_t _time_last_hagl_fuse{0}; ///< last system time that a range sample was fused by the terrain estimator - uint64_t _time_last_fake_hagl_fuse{0}; ///< last system time that a fake range sample was fused by the terrain estimator - bool _terrain_initialised{false}; ///< true when the terrain estimator has been initialized bool _hagl_valid{false}; ///< true when the height above ground estimate is valid terrain_fusion_status_u _hagl_sensor_status{}; ///< Struct indicating type of sensor used to estimate height above ground @@ -679,20 +678,28 @@ private: bool calcOptFlowBodyRateComp(); // initialise the terrain vertical position estimator - // return true if the initialisation is successful - bool initHagl(); + void initHagl(); - bool shouldUseRangeFinderForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder); } bool shouldUseOpticalFlowForHagl() const { return (_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseOpticalFlow); } - // run the terrain estimator void runTerrainEstimator(); + void predictHagl(); // update the terrain vertical position estimate using a height above ground measurement from the range finder - void fuseHagl(); + void controlHaglRngFusion(); + void fuseHaglRng(); + void startHaglRngFusion(); + void resetHaglRngIfNeeded(); + void resetHaglRng(); + void stopHaglRngFusion(); + float getRngVar(); // update the terrain vertical position estimate using an optical flow measurement void fuseFlowForTerrain(); + void resetHaglFlow(); + + void controlHaglFakeFusion(); + void resetHaglFake(); // reset the heading and magnetic field states using the declination and magnetometer measurements // return true if successful diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index 31b3024611..dcb6028138 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -43,44 +43,9 @@ #include -bool Ekf::initHagl() +void Ekf::initHagl() { - bool initialized = false; - - if (!_control_status.flags.in_air) { - // if on ground, do not trust the range sensor, but 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); - _time_last_fake_hagl_fuse = _time_last_imu; - initialized = true; - - } else if (shouldUseRangeFinderForHagl() - && _range_sensor.isDataHealthy()) { - // if we have a fresh measurement, use it to initialise the terrain estimator - _terrain_vpos = _state.pos(2) + _range_sensor.getDistBottom(); - // initialise state variance to variance of measurement - _terrain_var = sq(_params.range_noise); - // success - initialized = true; - - } else if (shouldUseOpticalFlowForHagl() - && _flow_for_terrain_data_ready) { - // initialise terrain vertical position to origin as this is the best guess we have - _terrain_vpos = fmaxf(0.0f, _state.pos(2)); - _terrain_var = 100.0f; - initialized = true; - - } else { - // no information - cannot initialise - } - - if (initialized) { - // has initialized with valid data - _time_last_hagl_fuse = _time_last_imu; - } - - return initialized; + resetHaglFake(); } void Ekf::runTerrainEstimator() @@ -90,47 +55,154 @@ void Ekf::runTerrainEstimator() _last_on_ground_posD = _state.pos(2); } - // Perform initialisation check and - // on ground, continuously reset the terrain estimator - if (!_terrain_initialised || !_control_status.flags.in_air) { - _terrain_initialised = initHagl(); + predictHagl(); - } else { + // Fuse range finder data if available + controlHaglRngFusion(); - // predict the state variance growth where the state is the vertical position of the terrain underneath the vehicle + if (shouldUseOpticalFlowForHagl() + && _flow_for_terrain_data_ready) { + fuseFlowForTerrain(); + _flow_for_terrain_data_ready = false; + } - // process noise due to errors in vehicle height estimate - _terrain_var += sq(_imu_sample_delayed.delta_vel_dt * _params.terrain_p_noise); + controlHaglFakeFusion(); - // process noise due to terrain gradient - _terrain_var += sq(_imu_sample_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); - - // Fuse range finder data if available - if (shouldUseRangeFinderForHagl() - && _range_sensor.isDataHealthy()) { - fuseHagl(); - } - - if (shouldUseOpticalFlowForHagl() - && _flow_for_terrain_data_ready) { - fuseFlowForTerrain(); - _flow_for_terrain_data_ready = false; - } - - // 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); - } + // 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); } updateTerrainValidity(); } -void Ekf::fuseHagl() +void Ekf::predictHagl() +{ + // 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_sample_delayed.delta_vel_dt * _params.terrain_p_noise); + + // process noise due to terrain gradient + _terrain_var += sq(_imu_sample_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); +} + +void Ekf::controlHaglRngFusion() +{ + if (!(_params.terrain_fusion_mode & TerrainFusionMask::TerrainFuseRangeFinder) + || _control_status.flags.rng_fault) { + + stopHaglRngFusion(); + return; + } + + if (_range_sensor.isDataHealthy()) { + const bool continuing_conditions_passing = _control_status.flags.in_air; + //const bool continuing_conditions_passing = _control_status.flags.in_air && !_control_status.flags.rng_hgt; // TODO: should not be fused when using range height + const bool starting_conditions_passing = continuing_conditions_passing && _range_sensor.isRegularlySendingData(); + + _time_last_healthy_rng_data = _time_last_imu; + + if (_hagl_sensor_status.flags.range_finder) { + if (continuing_conditions_passing) { + fuseHaglRng(); + + // We have been rejecting range data for too long + const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); + const bool is_fusion_failing = isTimedOut(_time_last_hagl_fuse, timeout); + + if (is_fusion_failing) { + if (_range_sensor.getDistBottom() > 2.f * _params.rng_gnd_clearance) { + // Data seems good, attempt a reset + resetHaglRng(); + + } else if (starting_conditions_passing) { + // The sensor can probably not detect the ground properly + // declare the sensor faulty and stop the fusion + _control_status.flags.rng_fault = true; + _range_sensor.setFaulty(); + stopHaglRngFusion(); + + } else { + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + stopHaglRngFusion(); + } + } + + } else { + stopHaglRngFusion(); + } + + } else { + if (starting_conditions_passing) { + startHaglRngFusion(); + } + } + + } else if (_hagl_sensor_status.flags.range_finder && isTimedOut(_time_last_healthy_rng_data, _params.reset_timeout_max)) { + // No data anymore. Stop until it comes back. + stopHaglRngFusion(); + } +} + +void Ekf::startHaglRngFusion() +{ + _hagl_sensor_status.flags.range_finder = true; + resetHaglRngIfNeeded(); +} + +void Ekf::resetHaglRngIfNeeded() +{ + if (_hagl_sensor_status.flags.flow) { + const float meas_hagl = _range_sensor.getDistBottom(); + const float pred_hagl = _terrain_vpos - _state.pos(2); + const float hagl_innov = pred_hagl - meas_hagl; + const float obs_variance = getRngVar(); + + const float hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); + + const float gate_size = fmaxf(_params.range_innov_gate, 1.0f); + const float hagl_test_ratio = sq(hagl_innov) / (sq(gate_size) * hagl_innov_var); + + // Reset the state to the measurement only if the test ratio is large, + // otherwise let it converge through the fusion + if (hagl_test_ratio > 0.2f) { + resetHaglRng(); + + } else { + fuseHaglRng(); + } + + } else { + resetHaglRng(); + } +} + +float Ekf::getRngVar() +{ + return fmaxf(P(9, 9) * _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(); + _terrain_var = getRngVar(); + _terrain_vpos_reset_counter++; + _time_last_hagl_fuse = _time_last_imu; +} + +void Ekf::stopHaglRngFusion() +{ + _hagl_sensor_status.flags.range_finder = false; +} + +void Ekf::fuseHaglRng() { // get a height above ground measurement from the range finder assuming a flat earth const float meas_hagl = _range_sensor.getDistBottom(); @@ -142,9 +214,7 @@ void Ekf::fuseHagl() _hagl_innov = pred_hagl - meas_hagl; // calculate the observation variance adding the variance of the vehicles own height uncertainty - const float obs_variance = fmaxf(P(9, 9) * _params.vehicle_variance_scaler, 0.0f) - + sq(_params.range_noise) - + sq(_params.range_noise_scaler * _range_sensor.getRange()); + const float obs_variance = getRngVar(); // calculate the innovation variance - limiting it to prevent a badly conditioned fusion _hagl_innov_var = fmaxf(_terrain_var + obs_variance, obs_variance); @@ -165,20 +235,17 @@ void Ekf::fuseHagl() _innov_check_fail_status.flags.reject_hagl = false; } else { - // If we have been rejecting range data for too long, reset to measurement - const uint64_t timeout = static_cast(_params.terrain_timeout * 1e6f); - - if (isTimedOut(_time_last_hagl_fuse, timeout)) { - _terrain_vpos = _state.pos(2) + meas_hagl; - _terrain_var = obs_variance; - _terrain_vpos_reset_counter++; - - } else { - _innov_check_fail_status.flags.reject_hagl = true; - } + _innov_check_fail_status.flags.reject_hagl = true; } } +void Ekf::resetHaglFlow() +{ + _terrain_vpos = fmaxf(0.0f, _state.pos(2)); + _terrain_var = 100.0f; + _terrain_vpos_reset_counter++; +} + void Ekf::fuseFlowForTerrain() { // calculate optical LOS rates using optical flow rates that have had the body angular rate contribution removed @@ -279,6 +346,24 @@ void Ekf::fuseFlowForTerrain() } } +void Ekf::controlHaglFakeFusion() +{ + if (!_control_status.flags.in_air + && !_hagl_sensor_status.flags.range_finder + && !_hagl_sensor_status.flags.flow) { + resetHaglFake(); + } +} + +void Ekf::resetHaglFake() +{ + // 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); + _time_last_hagl_fuse = _time_last_imu; +} + void Ekf::updateTerrainValidity() { // we have been fusing range finder measurements in the last 5 seconds @@ -288,10 +373,7 @@ void Ekf::updateTerrainValidity() // this can only be the case if the main filter does not fuse optical flow const bool recent_flow_for_terrain_fusion = isRecent(_time_last_flow_terrain_fuse, (uint64_t)5e6); - _hagl_valid = (_terrain_initialised && (recent_range_fusion || recent_flow_for_terrain_fusion)); - - _hagl_sensor_status.flags.range_finder = shouldUseRangeFinderForHagl() - && recent_range_fusion && (_time_last_fake_hagl_fuse != _time_last_hagl_fuse); + _hagl_valid = (recent_range_fusion || recent_flow_for_terrain_fusion); _hagl_sensor_status.flags.flow = shouldUseOpticalFlowForHagl() && recent_flow_for_terrain_fusion; }