From a1e67396f4f933c96a41909eac23e51741ef0f2d Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:42:52 +0100 Subject: [PATCH 01/11] ekf: added methods for setting control height flags Signed-off-by: Roman --- EKF/ekf.h | 12 ++++++++++++ EKF/ekf_helper.cpp | 36 ++++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) diff --git a/EKF/ekf.h b/EKF/ekf.h index 017ae3a449..37b4c6ed52 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -507,6 +507,18 @@ private: return var * var; } + // 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(); + // zero the specified range of rows in the state covariance matrix void zeroRows(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first, uint8_t last); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 47122d2e81..49989e30b0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1162,3 +1162,39 @@ 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; +} From a0ab5cf0d7311322e2ba34d41307cd3a97b90fb7 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:44:55 +0100 Subject: [PATCH 02/11] ekf vel_pos_fusion: added height sensor offset to range innovation calculation Signed-off-by: Roman --- EKF/vel_pos_fusion.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index 7a20ff4caa..93981fc023 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -159,7 +159,7 @@ void Ekf::fuseVelPosHeight() fuse_map[5] = true; // use range finder with tilt correction _vel_pos_innov[5] = _state.pos(2) - (-math::max(_range_sample_delayed.rng * _R_rng_to_earth_2_2, - _params.rng_gnd_clearance)); + _params.rng_gnd_clearance)) - _hgt_sensor_offset; // observation variance - user parameter defined R[5] = fmaxf((sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sample_delayed.rng)) * sq(_R_rng_to_earth_2_2), 0.01f); // innovation gate size From 36bffd2571f4fa44ad9caccef6df7acb0645bb62 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:49:29 +0100 Subject: [PATCH 03/11] ekf: calculate the delta time between consecutive baro measurments (used for calculating filtered baro offset when primary height source is not baro) Signed-off-by: Roman --- EKF/control.cpp | 8 ++++++++ EKF/ekf.h | 1 + 2 files changed, 9 insertions(+) diff --git a/EKF/control.cpp b/EKF/control.cpp index 1204ec3edd..dc1bcd3a82 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -82,8 +82,16 @@ void Ekf::controlFusionModes() // check for arrival of new sensor data at the fusion time horizon _gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); _mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed); + + _delta_time_baro_us = _baro_sample_delayed.time_us; _baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed); + // if we have a new baro sample save the delta time between this sample and the last sample which is + // used below for baro offset calculations + if (_baro_data_ready) { + _delta_time_baro_us = _baro_sample_delayed.time_us - _delta_time_baro_us; + } + // calculate 2,2 element of rotation matrix from sensor frame to earth frame _R_rng_to_earth_2_2 = _R_to_earth(2, 0) * _sin_tilt_rng + _R_to_earth(2, 2) * _cos_tilt_rng; _range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed) diff --git a/EKF/ekf.h b/EKF/ekf.h index 37b4c6ed52..10d721523b 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -261,6 +261,7 @@ private: float _imu_collection_time_adj{0.0f}; // the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) uint64_t _time_acc_bias_check{0}; // last time the accel bias check passed (usec) + uint64_t _delta_time_baro_us{0}; // delta time between two consecutive delayed baro samples from the buffer (usec) Vector3f _earth_rate_NED; // earth rotation vector (NED) in rad/s From 3778f0921a05cf4207b4b45034dd8344683c007c Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 10:56:39 +0100 Subject: [PATCH 04/11] ekf: enable range finder to be used for estimating height even if it's not the primary height source - moved height control into single function in order to decide which sensor should be used for estimating height - under certain conditions allow to use the range finder to estimate height even if it's not the primary height source - fixed a bug where the delta time for the baro offset calculation was always zero - use methods to set height control flags to reduce code duplication and to prevent bugs Signed-off-by: Roman --- EKF/common.h | 4 + EKF/control.cpp | 206 +++++++++++++++++++++++++++--------------------- EKF/ekf.h | 5 ++ 3 files changed, 123 insertions(+), 92 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index ac9b7af5db..4b841df072 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -256,6 +256,10 @@ struct parameters { float rng_gnd_clearance{0.1f}; // minimum valid value for range when on ground (m) 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) + 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 range_aid == 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 range_aid == 1) + int range_aid{0}; // allow switching primary height source to range finder if certian conditions are met // vision position fusion float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index dc1bcd3a82..5441291696 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -110,11 +110,10 @@ void Ekf::controlFusionModes() controlExternalVisionFusion(); controlOpticalFlowFusion(); controlGpsFusion(); - controlBaroFusion(); - controlRangeFinderFusion(); controlAirDataFusion(); controlBetaFusion(); controlDragFusion(); + controlHeightFusion(); // for efficiency, fusion of direct state observations for position and velocity is performed sequentially // in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc) @@ -137,12 +136,8 @@ void Ekf::controlExternalVisionFusion() // check for a exernal vision measurement that has fallen behind the fusion time horizon if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) { // turn on use of external vision measurements for position and height - _control_status.flags.ev_pos = true; + setControlEVHeight(); ECL_INFO("EKF commencing external vision position fusion"); - // turn off other forms of height aiding - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; // reset the position, height and velocity resetPosition(); resetVelocity(); @@ -206,10 +201,7 @@ void Ekf::controlExternalVisionFusion() // determine if we should use the height observation if (_params.vdist_sensor_type == VDIST_SENSOR_EV) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = true; + setControlEVHeight(); _fuse_height = true; } @@ -432,15 +424,6 @@ void Ekf::controlGpsFusion() } - // Determine if GPS should be used as the height source - if (((_params.vdist_sensor_type == VDIST_SENSOR_GPS)) && !_gps_hgt_faulty) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; - _fuse_height = true; - - } } else { // handle the case where we do not have GPS and have not been using it for an extended period, but are still relying on it if ((_time_last_imu - _time_last_gps > 10e6) && (_time_last_imu - _time_last_airspeed > 1e6) && (_time_last_imu - _time_last_optflow > 1e6) && _control_status.flags.gps) { @@ -531,10 +514,7 @@ void Ekf::controlHeightSensorTimeouts() _gps_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlGPSHeight(); // request a reset reset_height = true; @@ -545,10 +525,7 @@ void Ekf::controlHeightSensorTimeouts() _baro_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlBaroHeight(); // request a reset reset_height = true; @@ -590,10 +567,7 @@ void Ekf::controlHeightSensorTimeouts() _baro_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlBaroHeight(); // request a reset reset_height = true; @@ -604,10 +578,7 @@ void Ekf::controlHeightSensorTimeouts() _gps_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = true; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlGPSHeight(); // request a reset reset_height = true; @@ -642,10 +613,7 @@ void Ekf::controlHeightSensorTimeouts() _baro_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlBaroHeight(); // request a reset reset_height = true; @@ -656,10 +624,7 @@ void Ekf::controlHeightSensorTimeouts() _rng_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = true; - _control_status.flags.ev_hgt = false; + setControlRangeHeight(); // request a reset reset_height = true; @@ -694,10 +659,7 @@ void Ekf::controlHeightSensorTimeouts() _baro_hgt_faulty = false; // reset the height mode - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; + setControlBaroHeight(); // request a reset reset_height = true; @@ -705,10 +667,7 @@ void Ekf::controlHeightSensorTimeouts() } else if (reset_to_ev) { // reset the height mode - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = true; + setControlEVHeight(); // request a reset reset_height = true; @@ -732,57 +691,87 @@ void Ekf::controlHeightSensorTimeouts() } } -void Ekf::controlBaroFusion() +void Ekf::controlHeightFusion() { - if (_baro_data_ready) { - // determine if we should use the baro as our height source - uint64_t last_baro_time_us = _baro_sample_delayed.time_us; - if (((_params.vdist_sensor_type == VDIST_SENSOR_BARO) || _control_status.flags.baro_hgt) && !_baro_hgt_faulty) { - _control_status.flags.baro_hgt = true; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = false; - _control_status.flags.ev_hgt = false; - _fuse_height = true; + // set control flags for the desired primary height source - } - - // 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) { - float local_time_step = 1e-6f*(float)(_baro_sample_delayed.time_us - last_baro_time_us); - local_time_step = math::constrain(local_time_step,0.0f,1.0f); - last_baro_time_us = _baro_sample_delayed.time_us; - float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset) + _state.pos(2) - _baro_hgt_offset; - _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); - - } - } -} - -void Ekf::controlRangeFinderFusion() -{ - // determine if we should use range finder data for height if (_range_data_ready) { - // set the height data source to range if requested - if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) { - _control_status.flags.baro_hgt = false; - _control_status.flags.gps_hgt = false; - _control_status.flags.rng_hgt = true; - _control_status.flags.ev_hgt = false; - - } - // correct the range data for position offset relative to the IMU Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; + } - // only use range finder as a height observation in the main filter if specifically enabled - if (_control_status.flags.rng_hgt) { + + if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { + bool can_use_range = rangeAidConditionsMet(); + + if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + setControlRangeHeight(); _fuse_height = true; - } + // we have just switched to using range finder, calculate height sensor offset such that current + // measurment matches our current height estimate + if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } - } else if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt) { + } else if (_baro_data_ready && !_baro_hgt_faulty && !can_use_range) { + setControlBaroHeight(); + _fuse_height = true; + + // we have just switched to using baro height, calculate height sensor offset such that current + // measurment matches our current height estimate + if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { + _hgt_sensor_offset = _baro_sample_delayed.hgt - _baro_hgt_offset + _state.pos(2); + } + } + } + + // set the height data source to range if requested + if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) && !_rng_hgt_faulty) { + setControlRangeHeight(); + _fuse_height = _range_data_ready; + } + + // Determine if GPS should be used as the height source + if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { + bool can_use_range = rangeAidConditionsMet(); + + if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + setControlRangeHeight(); + _fuse_height = true; + + // we have just switched to using range finder, calculate height sensor offset such that current + // measurment matches our current height estimate + if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } + + } else if (_gps_data_ready && !_gps_hgt_faulty && !can_use_range) { + setControlGPSHeight(); + _fuse_height = true; + + // we have just switched to using gps height, calculate height sensor offset such that current + // measurment matches our current height estimate + if (_control_status_prev.flags.gps_hgt != _control_status.flags.gps_hgt) { + _hgt_sensor_offset = _gps_sample_delayed.hgt - _gps_alt_ref + _state.pos(2); + } + } + } + + // 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) { + float local_time_step = 1e-6f * _delta_time_baro_us; + local_time_step = math::constrain(local_time_step, 0.0f, 1.0f); + + // apply a 10 second first order low pass filter to baro offset + float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset + _state.pos( + 2) - _baro_hgt_offset); + _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); + } + + if ((_time_last_imu - _time_last_hgt_fuse) > 2 * RNG_MAX_INTERVAL && _control_status.flags.rng_hgt && !_range_data_ready) { // If we are supposed to be using range finder data as the primary height sensor, have missed or rejected measurements // and are on the ground, then synthesise a measurement at the expected on ground value if (!_control_status.flags.in_air) { @@ -792,7 +781,40 @@ void Ekf::controlRangeFinderFusion() } _fuse_height = true; + } + +} + +bool Ekf::rangeAidConditionsMet() +{ + // if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source + // under the following conditions + // 1) we are not further than max_range_for_dual_fusion away from the ground + // 2) our ground speed is not higher than max_vel_for_dual_fusion + // 3) Our terrain estimate is stable (needs better checks) + if (_params.range_aid) { + // check if we should use range finder measurements to estimate height + bool use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + + bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) + && (_fault_status.value == 0); + + if (horz_vel_valid) { + float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); + + use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + + } else { + use_range_finder = false; + } + + // XXX need a way to tell if terrain is stable + use_range_finder &= fabsf(_hagl_innov) < 0.3f; + + return use_range_finder; + } else { + return false; } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 10d721523b..6b0efa4e41 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -502,6 +502,11 @@ private: // control for height sensor timeouts, sensor changes and state resets void controlHeightSensorTimeouts(); + // control for combined height fusion mode (implemented for switching between baro and range height) + void controlHeightFusion(); + + bool rangeAidConditionsMet(); + // return the square of two floating point numbers - used in auto coded sections inline float sq(float var) { From 1b92c9b5b357059e76346827c63ebc50fd150513 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 28 Feb 2017 11:06:18 +0100 Subject: [PATCH 05/11] ekf_helper: fixed resetting height using range finder - take range sensor offset wrt to IMU into account - use projection to earth Z axis Signed-off-by: Roman --- EKF/ekf_helper.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 49989e30b0..5bc1747346 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -158,8 +158,12 @@ void Ekf::resetHeight() rangeSample range_newest = _range_buffer.get_newest(); if (_time_last_imu - range_newest.time_us < 2 * RNG_MAX_INTERVAL) { + // correct the range data for position offset relative to the IMU + Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + range_newest.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2; // calculate the new vertical position using range sensor - float new_pos_down = _hgt_sensor_offset - range_newest.rng; + float new_pos_down = _hgt_sensor_offset - range_newest.rng * _R_rng_to_earth_2_2; // update the state and assoicated variance _state.pos(2) = new_pos_down; From 39983a7d55ba29b2e801fa9fb451f35048a24b41 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 6 Apr 2017 10:07:55 +0200 Subject: [PATCH 06/11] range aid: added hysteresis for switching in and out of range aid - prevents rapid switching - added innovation consistency check for using range aid Signed-off-by: Roman --- EKF/common.h | 1 + EKF/control.cpp | 38 ++++++++++++++++++++++++++------------ EKF/ekf.cpp | 1 + EKF/ekf.h | 5 ++++- 4 files changed, 32 insertions(+), 13 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 4b841df072..9c467f4030 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -260,6 +260,7 @@ struct parameters { 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 range_aid == 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 range_aid == 1) int range_aid{0}; // allow switching primary height source to range finder if certian conditions are met + float range_aid_innov_gate{1.0f}; // gate size used for innovation consistency checks for range aid fusion // vision position fusion float ev_innov_gate{5.0f}; // vision estimator fusion innovation consistency gate size (STD) diff --git a/EKF/control.cpp b/EKF/control.cpp index 5441291696..c4907d720d 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -704,9 +704,9 @@ void Ekf::controlHeightFusion() if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - bool can_use_range = rangeAidConditionsMet(); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -716,7 +716,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } - } else if (_baro_data_ready && !_baro_hgt_faulty && !can_use_range) { + } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { setControlBaroHeight(); _fuse_height = true; @@ -736,9 +736,9 @@ void Ekf::controlHeightFusion() // Determine if GPS should be used as the height source if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - bool can_use_range = rangeAidConditionsMet(); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (can_use_range && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -748,7 +748,7 @@ void Ekf::controlHeightFusion() _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); } - } else if (_gps_data_ready && !_gps_hgt_faulty && !can_use_range) { + } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { setControlGPSHeight(); _fuse_height = true; @@ -786,7 +786,7 @@ void Ekf::controlHeightFusion() } -bool Ekf::rangeAidConditionsMet() +bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) { // if the parameter for range aid is enabled we allow to switch from using the primary height source to using range finder as height source // under the following conditions @@ -794,8 +794,15 @@ bool Ekf::rangeAidConditionsMet() // 2) our ground speed is not higher than max_vel_for_dual_fusion // 3) Our terrain estimate is stable (needs better checks) if (_params.range_aid) { - // check if we should use range finder measurements to estimate height - bool use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching + bool use_range_finder; + if (in_range_aid_mode) { + use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + } else { + // if we were not using range aid in the previous iteration then require the current height above terrain to be + // smaller than 70 % of the maximum allowed ground distance for range aid + use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; + } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); @@ -803,14 +810,21 @@ bool Ekf::rangeAidConditionsMet() if (horz_vel_valid) { float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); - use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + if (in_range_aid_mode) { + use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + + } else { + // if we were not using range aid in the previous iteration then require the ground velocity to be + // smaller than 70 % of the maximum allowed ground velocity for range aid + use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid; + + } } else { use_range_finder = false; } - // XXX need a way to tell if terrain is stable - use_range_finder &= fabsf(_hagl_innov) < 0.3f; + use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); return use_range_finder; } else { diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 2c35687f86..6a9e4b6b84 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -57,6 +57,7 @@ #define ISFINITE(x) __builtin_isfinite(x) #endif + bool Ekf::init(uint64_t timestamp) { bool ret = initialise_interface(timestamp); diff --git a/EKF/ekf.h b/EKF/ekf.h index 6b0efa4e41..9f9202aec8 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -375,6 +375,9 @@ private: uint64_t _time_good_vert_accel{0}; // last time a good vertical accel was detected (usec) bool _bad_vert_accel_detected{false}; // true when bad vertical accelerometer data has been detected + // variables used to control range aid functionality + bool _in_range_aid_mode; + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates(); @@ -505,7 +508,7 @@ private: // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); - bool rangeAidConditionsMet(); + bool rangeAidConditionsMet(bool in_range_aid_mode); // return the square of two floating point numbers - used in auto coded sections inline float sq(float var) From 182ea434450c0e2496e0509eaddcac526caee9c6 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 27 Apr 2017 17:19:31 +0200 Subject: [PATCH 07/11] fixed baro offset calculation - do not subtract the height sensor offset variable when computing the baro offset from the local origin. The baro height offset is calculated when baro is not fused and so the height sensor offset used in that case is associated to another sensor and has nothing to do with the baro. Signed-off-by: Roman --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index c4907d720d..8b292ca516 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -766,7 +766,7 @@ void Ekf::controlHeightFusion() local_time_step = math::constrain(local_time_step, 0.0f, 1.0f); // apply a 10 second first order low pass filter to baro offset - float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt - _hgt_sensor_offset + _state.pos( + float offset_rate_correction = 0.1f * (_baro_sample_delayed.hgt + _state.pos( 2) - _baro_hgt_offset); _baro_hgt_offset += local_time_step * math::constrain(offset_rate_correction, -0.1f, 0.1f); } From c5d464b821054490ace947ba2645cd70dbc27dbb Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 27 Apr 2017 17:24:04 +0200 Subject: [PATCH 08/11] make range sensor height offset computation more robust - when switching to range finder use the current terrain estimate as height sensor offset, otherwise spikes in the range measurements could lead to a wrong offset Signed-off-by: Roman --- EKF/control.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 8b292ca516..47b3f186d0 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -713,7 +713,11 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + if (_terrain_initialised) { + _hgt_sensor_offset = _terrain_vpos; + } else { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } } } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { @@ -745,7 +749,11 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + if (_terrain_initialised) { + _hgt_sensor_offset = _terrain_vpos; + } else { + _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); + } } } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { From 50369673437e1facc9166a4115e546bf71b600e6 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 27 Apr 2017 17:25:09 +0200 Subject: [PATCH 09/11] barometer does not special height sensor offset because a barometer offset from the local origin is calculated if the primary source for height is not baro Signed-off-by: Roman --- EKF/control.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 47b3f186d0..ec6461e6fe 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -724,10 +724,10 @@ void Ekf::controlHeightFusion() setControlBaroHeight(); _fuse_height = true; - // we have just switched to using baro height, calculate height sensor offset such that current - // measurment matches our current height estimate + // we have just switched to using baro height, we don't need to set a height sensor offset + // since we track a separate _baro_hgt_offset if (_control_status_prev.flags.baro_hgt != _control_status.flags.baro_hgt) { - _hgt_sensor_offset = _baro_sample_delayed.hgt - _baro_hgt_offset + _state.pos(2); + _hgt_sensor_offset = 0.0f; } } } From 1fed2098049018d8c30dbe079e0a932464441586 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 11 May 2017 10:15:43 +0200 Subject: [PATCH 10/11] fix formatting --- EKF/control.cpp | 70 ++++++++++++++++++++++++------------------------- 1 file changed, 35 insertions(+), 35 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index ec6461e6fe..960a43f4b9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -704,9 +704,9 @@ void Ekf::controlHeightFusion() if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) { - _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -720,7 +720,7 @@ void Ekf::controlHeightFusion() } } - } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { + } else if (_baro_data_ready && !_baro_hgt_faulty && !_in_range_aid_mode) { setControlBaroHeight(); _fuse_height = true; @@ -740,9 +740,9 @@ void Ekf::controlHeightFusion() // Determine if GPS should be used as the height source if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) { - _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); + _in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode); - if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { + if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) { setControlRangeHeight(); _fuse_height = true; @@ -756,7 +756,7 @@ void Ekf::controlHeightFusion() } } - } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { + } else if (_gps_data_ready && !_gps_hgt_faulty && !_in_range_aid_mode) { setControlGPSHeight(); _fuse_height = true; @@ -802,39 +802,40 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) // 2) our ground speed is not higher than max_vel_for_dual_fusion // 3) Our terrain estimate is stable (needs better checks) if (_params.range_aid) { - // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching - bool use_range_finder; - if (in_range_aid_mode) { - use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; - } else { - // if we were not using range aid in the previous iteration then require the current height above terrain to be - // smaller than 70 % of the maximum allowed ground distance for range aid - use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; - } + // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching + bool use_range_finder; + if (in_range_aid_mode) { + use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; + + } else { + // if we were not using range aid in the previous iteration then require the current height above terrain to be + // smaller than 70 % of the maximum allowed ground distance for range aid + use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; + } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) - && (_fault_status.value == 0); + && (_fault_status.value == 0); if (horz_vel_valid) { float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1)); - if (in_range_aid_mode) { - use_range_finder &= ground_vel < _params.max_vel_for_range_aid; + if (in_range_aid_mode) { + use_range_finder &= ground_vel < _params.max_vel_for_range_aid; - } else { - // if we were not using range aid in the previous iteration then require the ground velocity to be - // smaller than 70 % of the maximum allowed ground velocity for range aid - use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid; - - } + } else { + // if we were not using range aid in the previous iteration then require the ground velocity to be + // smaller than 70 % of the maximum allowed ground velocity for range aid + use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid; + } } else { use_range_finder = false; } - use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); + use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f); return use_range_finder; + } else { return false; } @@ -876,15 +877,14 @@ void Ekf::controlAirDataFusion() void Ekf::controlBetaFusion() { - // control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion + // control activation and initialisation/reset of wind states required for synthetic sideslip fusion fusion - // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates - bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; - bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; - if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)){ - _control_status.flags.wind = false; - - } + // If both airspeed and sideslip fusion have timed out then we no longer have valid wind estimates + bool sideslip_timed_out = _time_last_imu - _time_last_beta_fuse > 10e6; + bool airspeed_timed_out = _time_last_imu - _time_last_arsp_fuse > 10e6; + if(_control_status.flags.wind && airspeed_timed_out && sideslip_timed_out && !(_params.fusion_mode & MASK_USE_DRAG)) { + _control_status.flags.wind = false; + } // Perform synthetic sideslip fusion when in-air and sideslip fuson had been enabled externally in addition to the following criteria: @@ -907,10 +907,10 @@ void Ekf::controlBetaFusion() resetWindCovariance(); } - fuseSideslip(); + fuseSideslip(); } - + } From 6f5cffafece60a44a941124c2143d41a2d4f7fae Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 15 Jun 2017 10:14:59 +0200 Subject: [PATCH 11/11] fix type of range_aid parameter: int -> int32_t --- EKF/common.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 9c467f4030..40332488ec 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -259,7 +259,7 @@ struct parameters { 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 range_aid == 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 range_aid == 1) - int range_aid{0}; // allow switching primary height source to range finder if certian conditions are met + int32_t range_aid{0}; // allow switching primary height source to range finder if certian conditions are met float range_aid_innov_gate{1.0f}; // gate size used for innovation consistency checks for range aid fusion // vision position fusion