From 8fd79688c0898e16bcaf92db9f7337030ad05de0 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 19 May 2022 16:59:35 +0200 Subject: [PATCH] ekf2: enable multiple height sources fusion Instead of having a single height source fused into the EKF and the other ones "waiting" for a failure or the primary sensor, fuse all sources in EKF2 at the same time. To prevent the sources from fighting against each other, the "primary" source is set as reference and the other ones are running a bias estimator in order to make all the secondary height sources converge to the primary one. If the reference isn't available, another one is automatically selected from a priority list. This secondary reference keeps its current bias estimate but stops updating it in order to be the new reference as close as possible to the primary one. --- src/modules/ekf2/EKF/bias_estimator.cpp | 2 +- src/modules/ekf2/EKF/bias_estimator.hpp | 12 +- src/modules/ekf2/EKF/common.h | 23 +- src/modules/ekf2/EKF/control.cpp | 487 +++++++++--------- src/modules/ekf2/EKF/ekf.cpp | 8 +- src/modules/ekf2/EKF/ekf.h | 54 +- src/modules/ekf2/EKF/ekf_helper.cpp | 243 +++++---- src/modules/ekf2/EKF/estimator_interface.cpp | 4 +- src/modules/ekf2/EKF/gps_fusion.cpp | 15 +- src/modules/ekf2/EKF/height_fusion.cpp | 58 ++- src/modules/ekf2/EKF2.cpp | 4 +- src/modules/ekf2/ekf2_params.c | 27 +- .../test/sensor_simulator/ekf_wrapper.cpp | 56 +- .../ekf2/test/sensor_simulator/ekf_wrapper.h | 16 +- .../ekf2/test/test_EKF_fusionLogic.cpp | 15 +- src/modules/ekf2/test/test_EKF_gps.cpp | 4 +- .../ekf2/test/test_EKF_terrain_estimator.cpp | 1 - 17 files changed, 573 insertions(+), 456 deletions(-) diff --git a/src/modules/ekf2/EKF/bias_estimator.cpp b/src/modules/ekf2/EKF/bias_estimator.cpp index 389b3e9b18..545b969d16 100644 --- a/src/modules/ekf2/EKF/bias_estimator.cpp +++ b/src/modules/ekf2/EKF/bias_estimator.cpp @@ -97,7 +97,7 @@ inline bool BiasEstimator::isTestRatioPassing(const float innov_test_ratio) cons inline void BiasEstimator::updateState(const float K, const float innov) { - _state = math::constrain(_state + K * innov, -_state_max, _state_max); + _state = _state + K * innov; } inline void BiasEstimator::updateStateCovariance(const float K) diff --git a/src/modules/ekf2/EKF/bias_estimator.hpp b/src/modules/ekf2/EKF/bias_estimator.hpp index 5f70ec5204..637efc14a8 100644 --- a/src/modules/ekf2/EKF/bias_estimator.hpp +++ b/src/modules/ekf2/EKF/bias_estimator.hpp @@ -64,9 +64,18 @@ public: float innov_test_ratio; }; - BiasEstimator() = default; + BiasEstimator(float state_init, float state_var_init): _state{state_init}, _state_var{state_var_init} {}; virtual ~BiasEstimator() = default; + void reset() + { + _state = 0.f; + _state_var = 0.f; + _signed_innov_test_ratio_lpf.reset(0.f); + _time_since_last_negative_innov = 0.f; + _time_since_last_positive_innov = 0.f; + } + virtual void predict(float dt); virtual void fuseBias(float measurement, float measurement_var); @@ -86,7 +95,6 @@ public: private: float _state{0.f}; - float _state_max{100.f}; float _dt{0.01f}; float _gate_size{3.f}; ///< Used for innovation filtering (innovation test ratio) diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 2387dc5a15..36370606bc 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -105,12 +105,12 @@ enum TerrainFusionMask : uint8_t { TerrainFuseOpticalFlow = (1 << 1) }; -enum VerticalHeightSensor : uint8_t { - // Integer definitions for vdist_sensor_type - BARO = 0, ///< Use baro height - GPS = 1, ///< Use GPS height - RANGE = 2, ///< Use range finder height - EV = 3 ///< Use external vision +enum HeightSensor : uint8_t { + BARO = 0, + GNSS = 1, + RANGE = 2, + EV = 3, + UNKNOWN = 4 }; enum SensorFusionMask : uint16_t { @@ -123,7 +123,11 @@ enum SensorFusionMask : uint16_t { USE_DRAG = (1<<5), ///< set to true to use the multi-rotor drag model to estimate wind ROTATE_EXT_VIS = (1<<6), ///< set to true to if the EV observations are in a non NED reference frame and need to be rotated before being used USE_GPS_YAW = (1<<7), ///< set to true to use GPS yaw data if available - USE_EXT_VIS_VEL = (1<<8) ///< set to true to use external vision velocity data + USE_EXT_VIS_VEL = (1<<8), ///< set to true to use external vision velocity data + USE_BARO_HGT = (1<<9), ///< set to true to use baro data + USE_GPS_HGT = (1<<10), ///< set to true to use GPS height data + USE_RNG_HGT = (1<<11), ///< set to true to use range height data + USE_EXT_VIS_HGT = (1<<12) ///< set to true to use external vision height data }; struct gpsMessage { @@ -247,8 +251,9 @@ struct parameters { int32_t filter_update_interval_us{10000}; ///< filter update interval in microseconds // measurement source control - int32_t fusion_mode{SensorFusionMask::USE_GPS}; ///< bitmasked integer that selects which aiding sources will be used - int32_t vdist_sensor_type{VerticalHeightSensor::BARO}; ///< selects the primary source for height data + int32_t fusion_mode{SensorFusionMask::USE_GPS | + SensorFusionMask::USE_BARO_HGT}; ///< bitmasked integer that selects which aiding sources will be used + int32_t height_sensor_ref{HeightSensor::BARO}; int32_t terrain_fusion_mode{TerrainFusionMask::TerrainFuseRangeFinder | TerrainFusionMask::TerrainFuseOpticalFlow}; ///< aiding source(s) selection bitmask for the terrain estimator diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 68e4f61267..ac7e73dc84 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -172,9 +172,6 @@ void Ekf::controlFusionModes() // run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available runYawEKFGSF(); - // check for height sensor timeouts and reset and change sensor if necessary - controlHeightSensorTimeouts(); - // control use of observations for aiding controlMagFusion(); controlOpticalFlowFusion(); @@ -660,134 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } -void Ekf::controlHeightSensorTimeouts() -{ - /* - * Handle the case where we have not fused height measurements recently and - * uncertainty exceeds the max allowable. Reset using the best available height - * measurement source, continue using it after the reset and declare the current - * source failed if we have switched. - */ - - checkVerticalAccelerationHealth(); - - // check if height is continuously failing because of accel errors - const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); - - // check if height has been inertial deadreckoning for too long - // in vision hgt mode check for vision data - const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); - - if (hgt_fusion_timeout || continuous_bad_accel_hgt) { - - const char *failing_height_source = nullptr; - const char *new_height_source = nullptr; - - if (_control_status.flags.baro_hgt) { - bool reset_to_gps = false; - - // reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data - if (!_gps_intermittent) { - reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent; - } - - if (reset_to_gps) { - // set height sensor health - _baro_hgt_faulty = true; - - startGpsHgtFusion(); - - failing_height_source = "baro"; - new_height_source = "gps"; - - } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { - resetHeightToBaro(); - - failing_height_source = "baro"; - new_height_source = "baro"; - } - - } else if (_control_status.flags.gps_hgt) { - bool reset_to_baro = false; - - // if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro - if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { - reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent; - } - - if (reset_to_baro) { - startBaroHgtFusion(); - - failing_height_source = "gps"; - new_height_source = "baro"; - - } else if (!_gps_intermittent) { - resetHeightToGps(); - - failing_height_source = "gps"; - new_height_source = "gps"; - } - - } else if (_control_status.flags.rng_hgt) { - - if (_range_sensor.isHealthy()) { - resetHeightToRng(); - - failing_height_source = "rng"; - new_height_source = "rng"; - - } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { - startBaroHgtFusion(); - - failing_height_source = "rng"; - new_height_source = "baro"; - } - - } else if (_control_status.flags.ev_hgt) { - // check if vision data is available - bool ev_data_available = false; - - if (_ext_vision_buffer) { - const extVisionSample &ev_init = _ext_vision_buffer->get_newest(); - ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL); - } - - if (ev_data_available) { - resetHeightToEv(); - - failing_height_source = "ev"; - new_height_source = "ev"; - - } else if (_range_sensor.isHealthy()) { - // Fallback to rangefinder data if available - startRngHgtFusion(); - - failing_height_source = "ev"; - new_height_source = "rng"; - - } else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { - startBaroHgtFusion(); - - failing_height_source = "ev"; - new_height_source = "baro"; - } - } - - if (failing_height_source && new_height_source) { - _warning_events.flags.height_sensor_timeout = true; - ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source); - } - - // Also reset the vertical velocity - if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) { - resetVerticalVelocityToGps(_gps_sample_delayed); - - } else { - resetVerticalVelocityToZero(); - } - } -} - void Ekf::checkVerticalAccelerationHealth() { // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical @@ -797,6 +666,7 @@ void Ekf::checkVerticalAccelerationHealth() // Don't use stale innovation data. bool is_inertial_nav_falling = false; bool are_vertical_pos_and_vel_independant = false; + bool is_using_multiple_hgt_sources = false; if (_control_status.flags.gps) { // GNSS velocity @@ -827,6 +697,8 @@ void Ekf::checkVerticalAccelerationHealth() } if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) { + is_using_multiple_hgt_sources = _control_status.flags.gps_hgt && _control_status.flags.baro_hgt; + if (isRecent(_vert_vel_fuse_time_us, 1000000)) { // If vertical position and velocity come from independent sensors then we can // trust them more if they disagree with the IMU, but need to check that they agree @@ -861,7 +733,7 @@ void Ekf::checkVerticalAccelerationHealth() // if vertical velocity and position are independent and agree, then do not require evidence of clipping if // innovations are large - const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling; + const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling; if (bad_vert_accel) { _time_bad_vert_accel = _time_last_imu; @@ -882,115 +754,75 @@ void Ekf::checkVerticalAccelerationHealth() void Ekf::controlHeightFusion() { + checkVerticalAccelerationHealth(); checkRangeAidSuitability(); - const bool do_range_aid = (_params.range_aid == 1) && _is_range_aid_suitable; - switch (_params.vdist_sensor_type) { - default: - ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type); - - // FALLTHROUGH - case VerticalHeightSensor::BARO: - if (do_range_aid) { - if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { - startRngAidHgtFusion(); - } - - } else { - if (!_control_status.flags.baro_hgt) { - if (!_baro_hgt_faulty && !_baro_hgt_intermittent) { - startBaroHgtFusion(); - - } else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) { - // Use GPS as a fallback - startGpsHgtFusion(); - } - } - } - - break; - - case VerticalHeightSensor::RANGE: - - // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements - // and are on the ground, then synthesise a measurement at the expected on ground value - if (!_control_status.flags.in_air - && !_range_sensor.isDataHealthy() - && _range_sensor.isRegularlySendingData() - && _range_sensor.isDataReady()) { - - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); // bypass the checks - } - - if (!_control_status.flags.rng_hgt) { - if (_range_sensor.isDataHealthy()) { - startRngHgtFusion(); - } - } - - break; - - case VerticalHeightSensor::GPS: - - // NOTE: emergency fallback due to extended loss of currently selected sensor data or failure - // to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts. - // Do switching between GPS and rangefinder if using range finder as a height source when close - // to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers. - if (do_range_aid) { - if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) { - startRngAidHgtFusion(); - } - - } else { - if (!_control_status.flags.gps_hgt) { - if (!_gps_intermittent && _gps_checks_passed) { - // In fallback mode and GPS has recovered so start using it - startGpsHgtFusion(); - - } else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) { - // Use baro as a fallback - startBaroHgtFusion(); - } - } - } - - break; - - case VerticalHeightSensor::EV: - - // don't start using EV data unless data is arriving frequently - if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { - startEvHgtFusion(); - } - - break; - } - - updateBaroHgtBias(); - updateBaroHgtOffset(); updateGroundEffect(); - if (_baro_data_ready) { - updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt); + controlBaroHeightFusion(); + controlGnssHeightFusion(); + controlRangeHeightFusion(); + controlEvHeightFusion(); - if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) { - fuseBaroHgt(_aid_src_baro_hgt); - } + checkHeightSensorRefFallback(); +} + +void Ekf::checkHeightSensorRefFallback() +{ + if (_height_sensor_ref != HeightSensor::UNKNOWN) { + // The reference sensor is running, all good + return; } - if (_rng_data_ready) { - updateRngHgt(_aid_src_rng_hgt); + HeightSensor fallback_list[4]; - if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) { - fuseRngHgt(_aid_src_rng_hgt); - } + switch (_params.height_sensor_ref) { + default: + /* FALLTHROUGH */ + case HeightSensor::UNKNOWN: + fallback_list[0] = HeightSensor::GNSS; + fallback_list[1] = HeightSensor::BARO; + fallback_list[2] = HeightSensor::EV; + fallback_list[3] = HeightSensor::RANGE; + break; + + case HeightSensor::BARO: + fallback_list[0] = HeightSensor::BARO; + fallback_list[1] = HeightSensor::GNSS; + fallback_list[2] = HeightSensor::EV; + fallback_list[3] = HeightSensor::RANGE; + break; + + case HeightSensor::GNSS: + fallback_list[0] = HeightSensor::GNSS; + fallback_list[1] = HeightSensor::BARO; + fallback_list[2] = HeightSensor::EV; + fallback_list[3] = HeightSensor::RANGE; + break; + + case HeightSensor::RANGE: + fallback_list[0] = HeightSensor::RANGE; + fallback_list[1] = HeightSensor::EV; + fallback_list[2] = HeightSensor::BARO; + fallback_list[3] = HeightSensor::GNSS; + break; + + case HeightSensor::EV: + fallback_list[0] = HeightSensor::EV; + fallback_list[1] = HeightSensor::RANGE; + fallback_list[2] = HeightSensor::BARO; + fallback_list[3] = HeightSensor::GNSS; + break; } - if (_control_status.flags.ev_hgt) { - - if (_control_status.flags.ev_hgt && _ev_data_ready) { - fuseEvHgt(); + for (unsigned i = 0; i < 4; i++) { + if (((fallback_list[i] == HeightSensor::BARO) && _control_status.flags.baro_hgt) + || ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt) + || ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt) + || ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) { + ECL_INFO("fallback to secondary height reference"); + _height_sensor_ref = fallback_list[i]; + break; } } } @@ -1024,6 +856,199 @@ void Ekf::checkRangeAidSuitability() } } +void Ekf::controlBaroHeightFusion() +{ + if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) { + stopBaroHgtFusion(); + return; + } + + _baro_b_est.predict(_dt_ekf_avg); + + if (_baro_data_ready) { + _baro_lpf.update(_baro_sample_delayed.hgt); + updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt); + + const bool continuing_conditions_passing = !_baro_hgt_faulty && !_baro_hgt_intermittent; + const bool starting_conditions_passing = continuing_conditions_passing; + + if (_control_status.flags.baro_hgt) { + if (continuing_conditions_passing) { + fuseBaroHgt(_aid_src_baro_hgt); + + const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, (uint64_t)5e6); + + if (isHeightResetRequired()) { + // All height sources are failing + resetHeightToBaro(); + resetVerticalVelocityToZero(); + + } else if (is_fusion_failing) { + // Some other height source is still working + stopBaroHgtFusion(); + _baro_hgt_faulty = true; + } + + } else { + stopBaroHgtFusion(); + } + } else { + if (starting_conditions_passing) { + startBaroHgtFusion(); + } + } + + } else if (_control_status.flags.baro_hgt && _baro_hgt_intermittent) { + // No baro data anymore. Stop until it comes back. + stopBaroHgtFusion(); + } +} + +void Ekf::controlGnssHeightFusion() +{ + if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) { + stopGpsHgtFusion(); + return; + } + + _gps_hgt_b_est.predict(_dt_ekf_avg); + + if (_gps_data_ready) { + const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised; + const bool starting_conditions_passing = continuing_conditions_passing; + + if (_control_status.flags.gps_hgt) { + if (continuing_conditions_passing) { + /* fuseGpsHgt(); */ // Done in fuseGpsPos + + const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.reset_timeout_max); + + if (isHeightResetRequired()) { + // All height sources are failing + resetHeightToGps(); + resetVerticalVelocityToGps(_gps_sample_delayed); + + } else if (is_fusion_failing) { + // Some other height source is still working + stopGpsHgtFusion(); + } + + } else { + stopGpsHgtFusion(); + } + } else { + if (starting_conditions_passing) { + startGpsHgtFusion(); + } + } + + } else if (_control_status.flags.gps_hgt && _gps_intermittent) { + stopGpsHgtFusion(); + } +} + +void Ekf::controlRangeHeightFusion() +{ + if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) { + stopRngHgtFusion(); + return; + } + + _rng_hgt_b_est.predict(_dt_ekf_avg); + + // If we are supposed to be using range finder data as the primary height sensor, have bad range measurements + // and are on the ground, then synthesise a measurement at the expected on ground value + if (!_control_status.flags.in_air + && !_range_sensor.isDataHealthy() + && _range_sensor.isRegularlySendingData() + && _range_sensor.isDataReady()) { + + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setValidity(true); // bypass the checks + } + + if (_rng_data_ready) { + + // TODO: primary height source: + // run a bias estimator for all other sources + updateRngHgt(_aid_src_rng_hgt); + + const bool continuing_conditions_passing = _range_sensor.isDataHealthy(); + const bool starting_conditions_passing = continuing_conditions_passing + && _range_sensor.isRegularlySendingData(); + + if (_control_status.flags.rng_hgt) { + if (continuing_conditions_passing) { + fuseRngHgt(_aid_src_rng_hgt); + + const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6); + + if (isHeightResetRequired()) { + // All height sources are failing + resetHeightToRng(); + resetVerticalVelocityToZero(); + + } else if (is_fusion_failing) { + // Some other height source is still working + stopRngHgtFusion(); + _control_status.flags.rng_fault = true; + _range_sensor.setFaulty(); + } + + } else { + stopRngHgtFusion(); + } + + } else { + if (starting_conditions_passing) { + startRngHgtFusion(); + } + } + + } else if (_control_status.flags.rng_hgt && isTimedOut(_time_last_range, (uint64_t)5e6)) { + stopRngHgtFusion(); + } +} + +void Ekf::controlEvHeightFusion() +{ + if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) { + stopEvHgtFusion(); + return; + } + + _ev_hgt_b_est.predict(_dt_ekf_avg); + + if (_ev_data_ready) { + const bool continuing_conditions_passing = true; + const bool starting_conditions_passing = continuing_conditions_passing + && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL); + + if (_control_status.flags.ev_hgt) { + if (continuing_conditions_passing) { + fuseEvHgt(); + + if (isHeightResetRequired()) { + // All height sources are failing + resetHeightToRng(); + resetVerticalVelocityToZero(); + } + + } else { + stopEvHgtFusion(); + } + + } else { + if (starting_conditions_passing) { + startEvHgtFusion(); + } + } + + } else if (_control_status.flags.ev_hgt && isTimedOut(_time_last_ext_vision, (uint64_t)5e6)) { + stopEvHgtFusion(); + } +} + void Ekf::controlAirDataFusion() { // control activation and initialisation/reset of wind states required for airspeed fusion diff --git a/src/modules/ekf2/EKF/ekf.cpp b/src/modules/ekf2/EKF/ekf.cpp index 2aaa41ebc6..15faaf1ad1 100644 --- a/src/modules/ekf2/EKF/ekf.cpp +++ b/src/modules/ekf2/EKF/ekf.cpp @@ -164,10 +164,12 @@ bool Ekf::initialiseFilter() if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) { if (_baro_sample_delayed.time_us != 0) { if (_baro_counter == 0) { - _baro_hgt_offset = _baro_sample_delayed.hgt; + _baro_lpf.reset(_baro_sample_delayed.hgt); } else { - _baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt; + _baro_lpf.update(_baro_sample_delayed.hgt); + + _baro_b_est.setBias(_baro_lpf.getState()); } _baro_counter++; @@ -187,7 +189,7 @@ bool Ekf::initialiseFilter() } // we use baro height initially and switch to GPS/range/EV finder later when it passes checks. - setControlBaroHeight(); + _control_status.flags.baro_hgt = false; if (!initialiseTilt()) { return false; diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 8898d98eba..0c311ed785 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -47,6 +47,7 @@ #include "EKFGSF_yaw.h" #include "bias_estimator.hpp" +#include "height_bias_estimator.hpp" #include #include @@ -586,14 +587,7 @@ private: // Variables used to perform in flight resets and switch between height sources AlphaFilter _mag_lpf{0.1f}; ///< filtered magnetometer measurement for instant reset (Gauss) - - float _baro_hgt_offset{0.0f}; ///< baro height reading at the local NED origin (m) - float _gps_hgt_offset{0.0f}; ///< GPS height reading at the local NED origin (m) - float _rng_hgt_offset{0.0f}; ///< Range height reading at the local NED origin (m) - float _ev_hgt_offset{0.0f}; ///< EV height reading at the local NED origin (m) - - float _baro_hgt_bias{0.0f}; - float _baro_hgt_bias_var{1.f}; + AlphaFilter _baro_lpf{0.1f}; ///< filtered barometric height measurement (m) // Variables used to control activation of post takeoff functionality float _last_on_ground_posD{0.0f}; ///< last vertical position when the in_air status was false (m) @@ -709,6 +703,8 @@ private: void resetHorizontalPositionToLastKnown(); void resetHorizontalPositionTo(const Vector2f &new_horz_pos); + bool isHeightResetRequired() const; + void resetVerticalPositionTo(float new_vert_pos); void resetHeightToBaro(); @@ -923,9 +919,6 @@ private: void runMagAndMagDeclFusions(const Vector3f &mag); void run3DMagAndDeclFusions(const Vector3f &mag); - // control fusion of range finder observations - void controlRangeFinderFusion(); - // control fusion of air data observations void controlAirDataFusion(); @@ -935,9 +928,6 @@ private: // control fusion of multi-rotor drag specific force observations void controlDragFusion(); - // control fusion of pressure altitude observations - void controlBaroFusion(); - // control fusion of fake position observations to constrain drift void controlFakePosFusion(); @@ -948,29 +938,19 @@ private: // control fusion of auxiliary velocity observations void controlAuxVelFusion(); - // control for height sensor timeouts, sensor changes and state resets - void controlHeightSensorTimeouts(); - void checkVerticalAccelerationHealth(); // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); + void checkHeightSensorRefFallback(); + void controlBaroHeightFusion(); + void controlGnssHeightFusion(); + void controlRangeHeightFusion(); + void controlEvHeightFusion(); // determine if flight condition is suitable to use range finder instead of the primary height sensor void checkRangeAidSuitability(); - // set control flags to use baro height - void setControlBaroHeight(); - - // set control flags to use range height - void setControlRangeHeight(); - - // set control flags to use GPS height - void setControlGPSHeight(); - - // set control flags to use external vision height - void setControlEVHeight(); - void stopMagFusion(); void stopMag3DFusion(); void stopMagHdgFusion(); @@ -978,13 +958,16 @@ private: void startMag3DFusion(); void startBaroHgtFusion(); + void stopBaroHgtFusion(); + void startGpsHgtFusion(); + void stopGpsHgtFusion(); + void startRngHgtFusion(); + void stopRngHgtFusion(); void startRngAidHgtFusion(); void startEvHgtFusion(); - - void updateBaroHgtOffset(); - void updateBaroHgtBias(); + void stopEvHgtFusion(); void updateGroundEffect(); @@ -1091,7 +1074,12 @@ private: // yaw estimator instance EKFGSF_yaw _yawEstimator{}; - BiasEstimator _baro_b_est{}; + uint8_t _height_sensor_ref{HeightSensor::UNKNOWN}; + + HeightBiasEstimator _baro_b_est{HeightSensor::BARO, _height_sensor_ref}; + HeightBiasEstimator _gps_hgt_b_est{HeightSensor::GNSS, _height_sensor_ref}; + HeightBiasEstimator _rng_hgt_b_est{HeightSensor::RANGE, _height_sensor_ref}; + HeightBiasEstimator _ev_hgt_b_est{HeightSensor::EV, _height_sensor_ref}; int64_t _ekfgsf_yaw_reset_time{0}; ///< timestamp of last emergency yaw reset (uSec) uint8_t _ekfgsf_yaw_reset_count{0}; // number of times the yaw has been reset to the EKF-GSF estimate diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 95461113fd..906bebe35d 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -213,6 +213,18 @@ void Ekf::resetHorizontalPositionTo(const Vector2f &new_horz_pos) _time_last_hor_pos_fuse = _time_last_imu; } +bool Ekf::isHeightResetRequired() const +{ + // check if height is continuously failing because of accel errors + const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us); + + // check if height has been inertial deadreckoning for too long + const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6); + + return (continuous_bad_accel_hgt || hgt_fusion_timeout); +} + + void Ekf::resetVerticalPositionTo(const float new_vert_pos) { const float old_vert_pos = _state.pos(2); @@ -244,10 +256,14 @@ void Ekf::resetHeightToBaro() ECL_INFO("reset height to baro"); _information_events.flags.reset_hgt_to_baro = true; - resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_hgt_offset)); + resetVerticalPositionTo(-(_baro_sample_delayed.hgt - _baro_b_est.getBias())); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise)); + + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); } void Ekf::resetHeightToGps() @@ -255,14 +271,16 @@ void Ekf::resetHeightToGps() ECL_INFO("reset height to GPS"); _information_events.flags.reset_hgt_to_gps = true; - const float z_pos_before_reset = _state.pos(2); - resetVerticalPositionTo(-_gps_sample_delayed.hgt + getEkfGlobalOriginAltitude()); + resetVerticalPositionTo(-(_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias())); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance()); - // adjust the baro offset - _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; + _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); + + _aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu; } void Ekf::resetHeightToRng() @@ -281,14 +299,14 @@ void Ekf::resetHeightToRng() } // update the state and associated variance - const float z_pos_before_reset = _state.pos(2); - resetVerticalPositionTo(-dist_bottom + _rng_hgt_offset); + resetVerticalPositionTo(-(dist_bottom - _rng_hgt_b_est.getBias())); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise)); - // adjust the baro offset - _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; + _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); } void Ekf::resetHeightToEv() @@ -296,14 +314,14 @@ void Ekf::resetHeightToEv() ECL_INFO("reset height to EV"); _information_events.flags.reset_hgt_to_ev = true; - const float z_pos_before_reset = _state.pos(2); - resetVerticalPositionTo(_ev_sample_delayed.pos(2)); + resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias()); // the state variance is the same as the observation P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f))); - // adjust the baro offset - _baro_hgt_offset += _state.pos(2) - z_pos_before_reset; + _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); + _gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); } void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample_delayed) @@ -733,10 +751,9 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons resetVerticalPositionTo(_gps_alt_ref - current_alt); - _baro_hgt_offset += _state_reset_status.posD_change; - - _rng_hgt_offset += _state_reset_status.posD_change; - _ev_hgt_offset -= _state_reset_status.posD_change; + _baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change); + _rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change); + _ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change); } return true; @@ -1232,42 +1249,6 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) } } -void Ekf::setControlBaroHeight() -{ - _control_status.flags.baro_hgt = true; - - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; -} - -void Ekf::setControlRangeHeight() -{ - _control_status.flags.rng_hgt = true; - - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.ev_hgt = false; -} - -void Ekf::setControlGPSHeight() -{ - _control_status.flags.gps_hgt = true; - - _control_status.flags.baro_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; -} - -void Ekf::setControlEVHeight() -{ - _control_status.flags.ev_hgt = true; - - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; -} - void Ekf::stopMagFusion() { stopMag3DFusion(); @@ -1325,96 +1306,130 @@ void Ekf::startMag3DFusion() void Ekf::startBaroHgtFusion() { if (!_control_status.flags.baro_hgt) { - if (!_control_status.flags.rng_hgt) { + if (_params.height_sensor_ref == HeightSensor::BARO) { + _height_sensor_ref = HeightSensor::BARO; resetHeightToBaro(); + + } else { + _baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState()); } - setControlBaroHeight(); - - // We don't need to set a height sensor offset - // since we track a separate _baro_hgt_offset - + _control_status.flags.baro_hgt = true; + _baro_b_est.setFusionActive(); ECL_INFO("starting baro height fusion"); } } +void Ekf::stopBaroHgtFusion() +{ + if (_control_status.flags.baro_hgt) { + if (_height_sensor_ref == HeightSensor::BARO) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + + _control_status.flags.baro_hgt = false; + _baro_b_est.setFusionInactive(); + ECL_INFO("stopping baro height fusion"); + } +} + void Ekf::startGpsHgtFusion() { if (!_control_status.flags.gps_hgt) { - if (_control_status.flags.rng_hgt) { - // switch out of range aid - // calculate height sensor offset such that current - // measurement matches our current height estimate - _gps_hgt_offset = _gps_sample_delayed.hgt - getEkfGlobalOriginAltitude() + _state.pos(2); + + if (_params.height_sensor_ref == HeightSensor::GNSS) { + _gps_hgt_b_est.reset(); + _height_sensor_ref = HeightSensor::GNSS; + resetHeightToGps(); } else { - _gps_hgt_offset = 0.f; - resetHeightToGps(); + _gps_hgt_b_est.setBias(_state.pos(2) + (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude())); + + // Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout + _aid_src_gnss_pos.time_last_fuse[2] = _time_last_imu; } - setControlGPSHeight(); - + _control_status.flags.gps_hgt = true; + _gps_hgt_b_est.setFusionActive(); ECL_INFO("starting GPS height fusion"); } } +void Ekf::stopGpsHgtFusion() +{ + if (_control_status.flags.gps_hgt) { + + if (_height_sensor_ref == HeightSensor::GNSS) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } + + _control_status.flags.gps_hgt = false; + _gps_hgt_b_est.setFusionInactive(); + ECL_INFO("stopping GPS height fusion"); + } +} + void Ekf::startRngHgtFusion() { if (!_control_status.flags.rng_hgt) { - setControlRangeHeight(); - - // Range finder is the primary height source, the ground is now the datum used - // to compute the local vertical position - _rng_hgt_offset = 0.f; - - if (!_control_status_prev.flags.ev_hgt) { - // EV and range finders are using the same height datum + if (_params.height_sensor_ref == HeightSensor::RANGE) { + // Range finder is the primary height source, the ground is now the datum used + // to compute the local vertical position + _rng_hgt_b_est.reset(); + _height_sensor_ref = HeightSensor::RANGE; resetHeightToRng(); + + } else { + _rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom()); } + _control_status.flags.rng_hgt = true; + _rng_hgt_b_est.setFusionActive(); ECL_INFO("starting RNG height fusion"); } } -void Ekf::startRngAidHgtFusion() +void Ekf::stopRngHgtFusion() { - if (!_control_status.flags.rng_hgt) { - setControlRangeHeight(); + if (_control_status.flags.rng_hgt) { + if (_height_sensor_ref == HeightSensor::RANGE) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } - // calculate height sensor offset such that current - // measurement matches our current height estimate - _rng_hgt_offset = _terrain_vpos; - - ECL_INFO("starting RNG aid height fusion"); + _control_status.flags.rng_hgt = false; + _rng_hgt_b_est.setFusionInactive(); + ECL_INFO("stopping range height fusion"); } } void Ekf::startEvHgtFusion() { if (!_control_status.flags.ev_hgt) { - setControlEVHeight(); - - if (!_control_status_prev.flags.rng_hgt) { - // EV and range finders are using the same height datum + if (_params.height_sensor_ref == HeightSensor::EV) { + _rng_hgt_b_est.reset(); + _height_sensor_ref = HeightSensor::EV; resetHeightToEv(); + + } else { + _ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2)); } + _control_status.flags.ev_hgt = true; + _ev_hgt_b_est.setFusionActive(); ECL_INFO("starting EV height fusion"); } } -void Ekf::updateBaroHgtOffset() +void Ekf::stopEvHgtFusion() { - // calculate a filtered offset between the baro origin and local NED origin if we are not - // using the baro as a height reference - if (!_control_status.flags.baro_hgt && _baro_data_ready && (_delta_time_baro_us != 0)) { - const float local_time_step = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); + if (_control_status.flags.ev_hgt) { + if (_height_sensor_ref == HeightSensor::EV) { + _height_sensor_ref = HeightSensor::UNKNOWN; + } - // apply a 10 second first order low pass filter to baro offset - const float unbiased_baro = _baro_sample_delayed.hgt - _baro_b_est.getBias(); - - const float offset_rate_correction = 0.1f * (unbiased_baro + _state.pos(2) - _baro_hgt_offset); - _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); + _control_status.flags.ev_hgt = false; + _ev_hgt_b_est.setFusionInactive(); + ECL_INFO("stopping EV height fusion"); } } @@ -1428,27 +1443,6 @@ float Ekf::getGpsHeightVariance() return gps_alt_var; } -void Ekf::updateBaroHgtBias() -{ - // Baro bias estimation using GPS altitude - if (_baro_data_ready && (_delta_time_baro_us != 0)) { - const float dt = math::constrain(1e-6f * _delta_time_baro_us, 0.0f, 1.0f); - _baro_b_est.setMaxStateNoise(_params.baro_noise); - _baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate); - _baro_b_est.predict(dt); - } - - if (_gps_data_ready && !_gps_intermittent - && _gps_checks_passed && _NED_origin_initialised - && !_baro_hgt_faulty) { - // Use GPS altitude as a reference to compute the baro bias measurement - const float baro_bias = (_baro_sample_delayed.hgt - _baro_hgt_offset) - - (_gps_sample_delayed.hgt - getEkfGlobalOriginAltitude()); - const float baro_bias_var = getGpsHeightVariance() + sq(_params.baro_noise); - _baro_b_est.fuseBias(baro_bias, baro_bias_var); - } -} - float Ekf::getRngHeightVariance() const { const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom()); @@ -1660,14 +1654,13 @@ void Ekf::stopGpsFusion() void Ekf::stopGpsPosFusion() { - ECL_INFO("stopping GPS position fusion"); + if (_control_status.flags.gps) { + ECL_INFO("stopping GPS position fusion"); + _control_status.flags.gps = false; + stopGpsHgtFusion(); - if (_control_status.flags.gps_hgt) { - ECL_INFO("stopping GPS height fusion"); - startBaroHgtFusion(); + resetEstimatorAidStatus(_aid_src_gnss_pos); } - - resetEstimatorAidStatus(_aid_src_gnss_pos); } void Ekf::stopGpsVelFusion() diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 326390cfae..7547e9d906 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -437,7 +437,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) float max_time_delay_ms = math::max((float)_params.sensor_interval_max_ms, _params.auxvel_delay_ms); // using baro - if (_params.vdist_sensor_type == 0) { + if (_params.fusion_mode & SensorFusionMask::USE_BARO_HGT) { max_time_delay_ms = math::max(_params.baro_delay_ms, max_time_delay_ms); } @@ -452,7 +452,7 @@ bool EstimatorInterface::initialise_interface(uint64_t timestamp) } // range aid or range height - if (_params.range_aid || (_params.vdist_sensor_type == VerticalHeightSensor::RANGE)) { + if (_params.range_aid || (_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) { max_time_delay_ms = math::max(_params.range_delay_ms, max_time_delay_ms); } diff --git a/src/modules/ekf2/EKF/gps_fusion.cpp b/src/modules/ekf2/EKF/gps_fusion.cpp index 19828df5dc..33538096b0 100644 --- a/src/modules/ekf2/EKF/gps_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_fusion.cpp @@ -102,12 +102,23 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) auto &gps_pos = _aid_src_gnss_pos; resetEstimatorAidStatus(gps_pos); + const float height_measurement = gps_sample.hgt - getEkfGlobalOriginAltitude(); + const float height_measurement_var = getGpsHeightVariance(); + + // save current bias and update bias estimator + const float bias = _gps_hgt_b_est.getBias(); + const float bias_var = _gps_hgt_b_est.getBiasVar(); + + _gps_hgt_b_est.setMaxStateNoise(height_measurement_var); + _gps_hgt_b_est.setProcessNoiseStdDev(height_measurement_var); //TODO: update this + _gps_hgt_b_est.fuseBias(height_measurement - (-_state.pos(2)), height_measurement_var + P(9, 9)); + Vector3f position; position(0) = gps_sample.pos(0); position(1) = gps_sample.pos(1); // vertical position - gps measurement has opposite sign to earth z axis - position(2) = -(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_offset); + position(2) = -(height_measurement - bias); const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f); @@ -125,7 +136,7 @@ void Ekf::updateGpsPos(const gpsSample &gps_sample) obs_var(0) = obs_var(1) = sq(math::constrain(gps_sample.hacc, lower_limit, upper_limit)); } - obs_var(2) = getGpsHeightVariance(); + obs_var(2) = height_measurement_var + bias_var; // innovation gate size float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f); diff --git a/src/modules/ekf2/EKF/height_fusion.cpp b/src/modules/ekf2/EKF/height_fusion.cpp index 5c80466e31..40dcfc8e99 100644 --- a/src/modules/ekf2/EKF/height_fusion.cpp +++ b/src/modules/ekf2/EKF/height_fusion.cpp @@ -44,17 +44,18 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s resetEstimatorAidStatusFlags(baro_hgt); // innovation gate size - float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); + const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f); - // observation variance - user parameter defined - float obs_var = sq(fmaxf(_params.baro_noise, 0.01f)); + // measurement variance - user parameter defined + const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f)); + const float measurement = _baro_sample_delayed.hgt; // vertical position innovation - baro measurement has opposite sign to earth z axis - baro_hgt.observation = -(_baro_sample_delayed.hgt - _baro_b_est.getBias() - _baro_hgt_offset); - baro_hgt.observation_variance = obs_var; + baro_hgt.observation = -(measurement - _baro_b_est.getBias()); + baro_hgt.observation_variance = measurement_var + _baro_b_est.getBiasVar(); baro_hgt.innovation = _state.pos(2) - baro_hgt.observation; - baro_hgt.innovation_variance = P(9, 9) + obs_var; + baro_hgt.innovation_variance = P(9, 9) + baro_hgt.observation_variance; // Compensate for positive static pressure transients (negative vertical position innovations) // caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations. @@ -86,6 +87,12 @@ void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s baro_hgt.fusion_enabled = _control_status.flags.baro_hgt; baro_hgt.timestamp_sample = baro_sample.time_us; + + // update the bias estimator before updating the main filter but after + // using its current state to compute the vertical position innovation + _baro_b_est.setMaxStateNoise(_params.baro_noise); + _baro_b_est.setProcessNoiseStdDev(_params.baro_drift_rate); + _baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9)); } void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt) @@ -104,18 +111,19 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt) // reset flags resetEstimatorAidStatusFlags(rng_hgt); - // observation variance - user parameter defined - float obs_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f); + // measurement variance - user parameter defined + const float measurement_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f); + const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance); // innovation gate size - float innov_gate = fmaxf(_params.range_innov_gate, 1.f); + const float innov_gate = fmaxf(_params.range_innov_gate, 1.f); // vertical position innovation, use range finder with tilt correction - rng_hgt.observation = (-math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance)) + _rng_hgt_offset; - rng_hgt.observation_variance = obs_var; + rng_hgt.observation = -(measurement - _rng_hgt_b_est.getBias()); + rng_hgt.observation_variance = measurement_var + _rng_hgt_b_est.getBiasVar(); rng_hgt.innovation = _state.pos(2) - rng_hgt.observation; - rng_hgt.innovation_variance = P(9, 9) + obs_var; + rng_hgt.innovation_variance = P(9, 9) + rng_hgt.observation_variance; setEstimatorAidStatusTestRatio(rng_hgt, innov_gate); @@ -130,6 +138,14 @@ void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt) rng_hgt.fusion_enabled = _control_status.flags.rng_hgt; rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us; + + // update the bias estimator before updating the main filter but after + // using its current state to compute the vertical position innovation + const float rng_noise = sqrtf(measurement_var); + _rng_hgt_b_est.setMaxStateNoise(rng_noise); + _rng_hgt_b_est.setProcessNoiseStdDev(rng_noise); // TODO: fix + + _rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9)); } void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt) @@ -145,15 +161,25 @@ void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt) void Ekf::fuseEvHgt() { + const float measurement = _ev_sample_delayed.pos(2); + const float measurement_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)); + + const float bias = _ev_hgt_b_est.getBias(); + const float bias_var = _ev_hgt_b_est.getBiasVar(); + + const float ev_noise = sqrtf(measurement_var); + _ev_hgt_b_est.setMaxStateNoise(ev_noise); + _ev_hgt_b_est.setProcessNoiseStdDev(ev_noise); // TODO: fix + _ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9)); + // calculate the innovation assuming the external vision observation is in local NED frame - _ev_pos_innov(2) = _state.pos(2) - _ev_sample_delayed.pos(2); + const float obs = measurement - bias; + const float obs_var = measurement_var + bias_var; + _ev_pos_innov(2) = _state.pos(2) - obs; // innovation gate size float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f); - // observation variance - defined externally - float obs_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)); - // _ev_pos_test_ratio(1) is the vertical test ratio fuseVerticalPosition(_ev_pos_innov(2), innov_gate, obs_var, _ev_pos_innov_var(2), _ev_pos_test_ratio(1)); diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 3063e0dafd..4b8e4bd61a 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -106,7 +106,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode): _param_ekf2_req_hdrift(_params->req_hdrift), _param_ekf2_req_vdrift(_params->req_vdrift), _param_ekf2_aid_mask(_params->fusion_mode), - _param_ekf2_hgt_mode(_params->vdist_sensor_type), + _param_ekf2_hgt_mode(_params->height_sensor_ref), _param_ekf2_terr_mask(_params->terrain_fusion_mode), _param_ekf2_noaid_tout(_params->valid_timeout_max), _param_ekf2_rng_noise(_params->range_noise), @@ -292,7 +292,7 @@ void EKF2::Run() } // if using baro ensure sensor interval minimum is sufficient to accommodate system averaged baro output - if (_params->vdist_sensor_type == 0) { + if (_params->fusion_mode & SensorFusionMask::USE_BARO_HGT) { float sens_baro_rate = 0.f; if (param_get(param_find("SENS_BARO_RATE"), &sens_baro_rate) == PX4_OK) { diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 8a4fddcc8b..2bc7fdc9cf 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -608,22 +608,31 @@ PARAM_DEFINE_FLOAT(EKF2_TAS_GATE, 3.0f); * 5 : Set to true to enable multi-rotor drag specific force fusion * 6 : set to true if the EV observations are in a non NED reference frame and need to be rotated before being used * 7 : Set to true to enable GPS yaw fusion. Cannot be used if bit position 4 is true. + * 8 : Set to true to enable vision velocity fusion + * 9 : Set to true to enable barometer height fusion. + * 10 : Set to true to enable GPS height fusion. + * 11 : Set to true to enable range finder height fusion. + * 12 : Set to true to enable vision height fusion. * * @group EKF2 * @min 0 - * @max 511 - * @bit 0 use GPS - * @bit 1 use optical flow + * @max 8091 + * @bit 0 position: GPS + * @bit 1 velocity: optical flow * @bit 2 inhibit IMU bias estimation - * @bit 3 vision position fusion - * @bit 4 vision yaw fusion - * @bit 5 multi-rotor drag fusion + * @bit 3 position: vision + * @bit 4 yaw: vision + * @bit 5 wind: multi-rotor drag fusion * @bit 6 rotate external vision - * @bit 7 GPS yaw fusion - * @bit 8 vision velocity fusion + * @bit 7 yaw: GPS + * @bit 8 velocity: vision + * @bit 9 height: barometer + * @bit 10 height: GPS + * @bit 11 height: range finder + * @bit 12 height: vision * @reboot_required true */ -PARAM_DEFINE_INT32(EKF2_AID_MASK, 1); +PARAM_DEFINE_INT32(EKF2_AID_MASK, 513); /** * Determines the primary source of height data used by the EKF. diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp index b077c7a5db..d4f32aacb1 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.cpp @@ -10,9 +10,19 @@ EkfWrapper::~EkfWrapper() { } -void EkfWrapper::setBaroHeight() +void EkfWrapper::setBaroHeightRef() { - _ekf_params->vdist_sensor_type = VerticalHeightSensor::BARO; + _ekf_params->height_sensor_ref |= HeightSensorRef::BARO; +} + +void EkfWrapper::enableBaroHeightFusion() +{ + _ekf_params->fusion_mode |= SensorFusionMask::USE_BARO_HGT; +} + +void EkfWrapper::disableBaroHeightFusion() +{ + _ekf_params->fusion_mode &= ~SensorFusionMask::USE_BARO_HGT; } bool EkfWrapper::isIntendingBaroHeightFusion() const @@ -20,9 +30,19 @@ bool EkfWrapper::isIntendingBaroHeightFusion() const return _ekf->control_status_flags().baro_hgt; } -void EkfWrapper::setGpsHeight() +void EkfWrapper::setGpsHeightRef() { - _ekf_params->vdist_sensor_type = VerticalHeightSensor::GPS; + _ekf_params->height_sensor_ref |= HeightSensorRef::GPS; +} + +void EkfWrapper::enableGpsHeightFusion() +{ + _ekf_params->fusion_mode |= SensorFusionMask::USE_GPS_HGT; +} + +void EkfWrapper::disableGpsHeightFusion() +{ + _ekf_params->fusion_mode &= ~SensorFusionMask::USE_GPS_HGT; } bool EkfWrapper::isIntendingGpsHeightFusion() const @@ -30,9 +50,19 @@ bool EkfWrapper::isIntendingGpsHeightFusion() const return _ekf->control_status_flags().gps_hgt; } -void EkfWrapper::setRangeHeight() +void EkfWrapper::setRangeHeightRef() { - _ekf_params->vdist_sensor_type = VerticalHeightSensor::RANGE; + _ekf_params->height_sensor_ref |= HeightSensorRef::RANGE; +} + +void EkfWrapper::enableRangeHeightFusion() +{ + _ekf_params->fusion_mode |= SensorFusionMask::USE_RNG_HGT; +} + +void EkfWrapper::disableRangeHeightFusion() +{ + _ekf_params->fusion_mode &= ~SensorFusionMask::USE_RNG_HGT; } bool EkfWrapper::isIntendingRangeHeightFusion() const @@ -40,9 +70,19 @@ bool EkfWrapper::isIntendingRangeHeightFusion() const return _ekf->control_status_flags().rng_hgt; } -void EkfWrapper::setVisionHeight() +void EkfWrapper::setVisionHeightRef() { - _ekf_params->vdist_sensor_type = VerticalHeightSensor::EV; + _ekf_params->height_sensor_ref |= HeightSensorRef::EV; +} + +void EkfWrapper::enableVisionHeightFusion() +{ + _ekf_params->fusion_mode |= SensorFusionMask::USE_EXT_VIS_HGT; +} + +void EkfWrapper::disableVisionHeightFusion() +{ + _ekf_params->fusion_mode &= ~SensorFusionMask::USE_EXT_VIS_HGT; } bool EkfWrapper::isIntendingVisionHeightFusion() const diff --git a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h index 80b4cc0644..633fd0a0ce 100644 --- a/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h +++ b/src/modules/ekf2/test/sensor_simulator/ekf_wrapper.h @@ -49,16 +49,24 @@ public: ~EkfWrapper(); - void setBaroHeight(); + void setBaroHeightRef(); + void enableBaroHeightFusion(); + void disableBaroHeightFusion(); bool isIntendingBaroHeightFusion() const; - void setGpsHeight(); + void setGpsHeightRef(); + void enableGpsHeightFusion(); + void disableGpsHeightFusion(); bool isIntendingGpsHeightFusion() const; - void setRangeHeight(); + void setRangeHeightRef(); + void enableRangeHeightFusion(); + void disableRangeHeightFusion(); bool isIntendingRangeHeightFusion() const; - void setVisionHeight(); + void setVisionHeightRef(); + void enableVisionHeightFusion(); + void disableVisionHeightFusion(); bool isIntendingVisionHeightFusion() const; void enableGpsFusion(); diff --git a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp index c9b503331d..c0327abe92 100644 --- a/src/modules/ekf2/test/test_EKF_fusionLogic.cpp +++ b/src/modules/ekf2/test/test_EKF_fusionLogic.cpp @@ -358,6 +358,7 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusion) // GIVEN: EKF that receives baro and GPS data _sensor_simulator.startGps(); _sensor_simulator.runSeconds(11); + _ekf_wrapper.enableGpsHeightFusion(); // THEN: EKF should intend to fuse baro by default EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); @@ -396,26 +397,28 @@ TEST_F(EkfFusionLogicTest, doBaroHeightFusionTimeout) // BUT WHEN: GPS height data is also available _sensor_simulator.startGps(); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.runSeconds(11); reset_logging_checker.capturePostResetState(); EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); // AND: the baro data jumps by a lot _sensor_simulator._baro.setData(800.f); _sensor_simulator.runSeconds(20); reset_logging_checker.capturePostResetState(); - // THEN: EKF should fallback to GPS height + // THEN: EKF should fallback to GPS height (without reset) EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); - EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(3)); - EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isVerticalVelocityResetCounterIncreasedBy(2)); + EXPECT_TRUE(reset_logging_checker.isVerticalPositionResetCounterIncreasedBy(1)); } TEST_F(EkfFusionLogicTest, doGpsHeightFusion) { // WHEN: commanding GPS height and sending GPS data - _ekf_wrapper.setGpsHeight(); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.startGps(); _sensor_simulator.runSeconds(11); @@ -434,7 +437,7 @@ TEST_F(EkfFusionLogicTest, doGpsHeightFusion) TEST_F(EkfFusionLogicTest, doRangeHeightFusion) { // WHEN: commanding range height and sending range data - _ekf_wrapper.setRangeHeight(); + _ekf_wrapper.enableRangeHeightFusion(); _sensor_simulator.startRangeFinder(); _sensor_simulator.runSeconds(2.5f); // THEN: EKF should intend to fuse range height @@ -462,7 +465,7 @@ TEST_F(EkfFusionLogicTest, doRangeHeightFusion) TEST_F(EkfFusionLogicTest, doVisionHeightFusion) { // WHEN: commanding vision height and sending vision data - _ekf_wrapper.setVisionHeight(); + _ekf_wrapper.enableVisionHeightFusion(); _sensor_simulator.startExternalVision(); _sensor_simulator.runSeconds(2); diff --git a/src/modules/ekf2/test/test_EKF_gps.cpp b/src/modules/ekf2/test/test_EKF_gps.cpp index 436b52b3da..28befd646d 100644 --- a/src/modules/ekf2/test/test_EKF_gps.cpp +++ b/src/modules/ekf2/test/test_EKF_gps.cpp @@ -157,12 +157,12 @@ TEST_F(EkfGpsTest, gpsHgtToBaroFallback) _ekf_wrapper.enableFlowFusion(); _sensor_simulator.startFlow(); - _ekf_wrapper.setGpsHeight(); + _ekf_wrapper.enableGpsHeightFusion(); _sensor_simulator.runSeconds(1); EXPECT_TRUE(_ekf_wrapper.isIntendingGpsHeightFusion()); EXPECT_TRUE(_ekf_wrapper.isIntendingFlowFusion()); - EXPECT_FALSE(_ekf_wrapper.isIntendingBaroHeightFusion()); + EXPECT_TRUE(_ekf_wrapper.isIntendingBaroHeightFusion()); // WHEN: stopping GPS fusion _sensor_simulator.stopGps(); diff --git a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp index 7fc0518852..4220d6559f 100644 --- a/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp +++ b/src/modules/ekf2/test/test_EKF_terrain_estimator.cpp @@ -80,7 +80,6 @@ public: _ekf->set_vehicle_at_rest(true); _ekf_wrapper.enableGpsFusion(); - _ekf_wrapper.setBaroHeight(); _sensor_simulator.runSeconds(2); // Run to pass the GPS checks _sensor_simulator.runSeconds(3.5); // And a bit more to start the GPS fusion TODO: this shouldn't be necessary EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());