Skip to content

Commit

Permalink
Merge branch 'pr-ekfOptFlowFixes'
Browse files Browse the repository at this point in the history 
* pr-ekfOptFlowFixes:
  EKF: fix bug causing height offset when GPS use stops
  EKF: Don't reject saturated flow data when it is the only aiding source
  EKF: Prevent flow motion check false positives
  EKF: Don't assume large position uncertainty when starting optical flow nav
  EKF: relax terrain update requirements for continuing optical flow use
  EKF: Relax minimum required range finder measurement rate
  EKF: relax optical flow on ground motion checks
  EKF: range finder aiding logic fixes
  EKF: Decouple range finder use criteria checking and selection
  EKF: Don't auto select range finder for height when on ground.
  EKF: Fix false triggering of optical flow bad motion checks
  EKF: update comments
  EKF: Don't use optical flow if GPS is good and the vehicle is not using range finder for height
  EKF: Stop using EV for yaw when GPS fusion starts
  EKF: Add persistence criteria to  GPS fail check
  EKF: allow GPS fallback if quality bad and alternative aiding available
  EKF: always run GPS checks
  • Loading branch information
@priseborough
priseborough committed May 10, 2018
2 parents 75e6590 + e8e9e34 commit 16976d3
Show file tree
Hide file tree
Showing 7 changed files with 121 additions and 102 deletions.
93 changes: 51 additions & 42 deletions EKF/control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -151,9 +151,7 @@ void Ekf::controlExternalVisionFusion()

// if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames
// needs to be calculated and the observations rotated into the EKF frame of reference
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {

if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) {
// rotate EV measurements into the EKF Navigation frame
calcExtVisRotMat();
}
Expand Down Expand Up @@ -182,7 +180,7 @@ void Ekf::controlExternalVisionFusion()
}

// external vision yaw aiding selection logic
if ((_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) {
// don't start using EV data unless daa is arriving frequently
if (_time_last_imu - _time_last_ext_vision < 2 * EV_MAX_INTERVAL) {
// reset the yaw angle to the value from the observaton quaternion
Expand Down Expand Up @@ -339,13 +337,14 @@ void Ekf::controlExternalVisionFusion()

void Ekf::controlOpticalFlowFusion()
{
// Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated.
// Check if motion is un-suitable for use of optical flow
if (!_control_status.flags.in_air) {
bool motion_is_excessive = ((_accel_mag_filt > 10.8f)
|| (_accel_mag_filt < 8.8f)
|| (_ang_rate_mag_filt > 0.5f)
|| (_R_to_earth(2, 2) < 0.866f));

// When on ground check if the vehicle is being shaken or moved in a way that could cause a loss of navigation
float accel_norm = _accel_vec_filt.norm();
bool motion_is_excessive = ((accel_norm > 14.7f) // accel greater than 1.5g
|| (accel_norm < 4.9f) // accel less than 0.5g
|| (_ang_rate_mag_filt > _params.flow_rate_max) // angular rate exceeds flow sensor limit
|| (_R_to_earth(2,2) < 0.866f)); // tilted more than 30 degrees
if (motion_is_excessive) {
_time_bad_motion_us = _imu_sample_delayed.time_us;

Expand All @@ -354,11 +353,17 @@ void Ekf::controlOpticalFlowFusion()
}

} else {
_time_bad_motion_us = 0;
_time_good_motion_us = _imu_sample_delayed.time_us;
bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6);
if (good_gps_aiding && !_range_aid_mode_enabled) {
// Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded
// limits for use of range finder for height
_time_bad_motion_us = _imu_sample_delayed.time_us;
} else {
_time_good_motion_us = _imu_sample_delayed.time_us;
}
}

// Inhibit flow use if on ground and motion is excessive
// Inhibit flow use if motion is un-suitable
// Apply a time based hysteresis to prevent rapid mode switching
if (!_inhibit_gndobs_use) {
if ((_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5) {
Expand Down Expand Up @@ -438,8 +443,8 @@ void Ekf::controlOpticalFlowFusion()
}
}

// fuse the data if the terrain/distance to bottom is valid
if (_control_status.flags.opt_flow && get_terrain_valid()) {
// fuse the data if the terrain/distance to bottom is valid but use a more relaxed check to enable it to survive bad range finder data
if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) {
// Update optical flow bias estimates
calcOptFlowBias();

Expand All @@ -460,14 +465,17 @@ void Ekf::controlGpsFusion()

// Determine if we should use GPS aiding for velocity and horizontal position
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
bool gps_checks_passing = (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6);
bool gps_checks_failing = (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6);
if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) {
if (_control_status.flags.tilt_align && _NED_origin_initialised
&& (_time_last_imu - _last_gps_fail_us > (uint64_t)5e6)) {

if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) {
// If the heading is not aligned, reset the yaw and magnetic field states
if (!_control_status.flags.yaw_align) {
// Do not use external vision for yaw if using GPS because yaw needs to be
// defined relative to an NED reference frame
if (!_control_status.flags.yaw_align || _control_status.flags.ev_yaw) {
_control_status.flags.yaw_align = false;
_control_status.flags.ev_yaw = false;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);

}

// If the heading is valid start using gps aiding
Expand All @@ -490,7 +498,6 @@ void Ekf::controlGpsFusion()
if (_control_status.flags.gps) {
ECL_INFO("EKF commencing GPS fusion");
_time_last_gps = _time_last_imu;

}
}
}
Expand All @@ -500,6 +507,12 @@ void Ekf::controlGpsFusion()

}

// Handle the case where we are using GPS and another source of aiding and GPS is failing checks
if (_control_status.flags.gps && gps_checks_failing && (_control_status.flags.opt_flow || _control_status.flags.ev_pos)) {
_control_status.flags.gps = false;
ECL_WARN("EKF GPS data quality poor - stopping use");
}

// handle the case when we now have GPS, but have not been using it for an extended period
if (_control_status.flags.gps) {
// We are relying on aiding to constrain drift so after a specified time
Expand Down Expand Up @@ -848,11 +861,13 @@ void Ekf::controlHeightFusion()
_range_sample_delayed.rng += pos_offset_earth(2) / _R_rng_to_earth_2_2;
}

rangeAidConditionsMet();

_range_aid_mode_selected = (_params.range_aid == 1) && _range_aid_mode_enabled;

if (_params.vdist_sensor_type == VDIST_SENSOR_BARO) {
_in_range_aid_mode = rangeAidConditionsMet(_in_range_aid_mode);

if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;

Expand All @@ -867,10 +882,9 @@ void Ekf::controlHeightFusion()
}
}

} else if (!_in_range_aid_mode && _baro_data_ready && !_baro_hgt_faulty) {
} else if (!_range_aid_mode_selected && _baro_data_ready && !_baro_hgt_faulty) {
setControlBaroHeight();
_fuse_height = true;
_in_range_aid_mode = false;

// 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
Expand All @@ -892,7 +906,6 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.gps_hgt && _gps_data_ready && !_gps_hgt_faulty) {
// switch to gps if there was a reset to gps
_fuse_height = true;
_in_range_aid_mode = false;

// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate
Expand Down Expand Up @@ -938,9 +951,8 @@ 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);

if (_in_range_aid_mode && _range_data_ready && !_rng_hgt_faulty) {
if (_range_aid_mode_selected && _range_data_ready && !_rng_hgt_faulty) {
setControlRangeHeight();
_fuse_height = true;

Expand All @@ -955,10 +967,9 @@ void Ekf::controlHeightFusion()
}
}

} else if (!_in_range_aid_mode && _gps_data_ready && !_gps_hgt_faulty) {
} else if (!_range_aid_mode_selected && _gps_data_ready && !_gps_hgt_faulty) {
setControlGPSHeight();
_fuse_height = true;
_in_range_aid_mode = false;

// we have just switched to using gps height, calculate height sensor offset such that current
// measurment matches our current height estimate
Expand All @@ -969,7 +980,6 @@ void Ekf::controlHeightFusion()
} else if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
_in_range_aid_mode = false;

// 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
Expand All @@ -984,7 +994,6 @@ void Ekf::controlHeightFusion()
if (_control_status.flags.baro_hgt && _baro_data_ready && !_baro_hgt_faulty) {
// switch to baro if there was a reset to baro
_fuse_height = true;
_in_range_aid_mode = false;

// 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
Expand Down Expand Up @@ -1022,18 +1031,18 @@ void Ekf::controlHeightFusion()

}

bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
void 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
// 1) we are not further than max_hagl_for_range_aid away from the ground
// 2) our ground speed is not higher than max_vel_for_range_aid
// 3) Our terrain estimate is stable (needs better checks)
if (_params.range_aid) {
// 4) We are in-air
if (_control_status.flags.in_air) {
// 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) {
if (_range_aid_mode_enabled) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();

} else {
Expand All @@ -1048,7 +1057,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
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) {
if (_range_aid_mode_enabled) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;

} else {
Expand All @@ -1062,7 +1071,7 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
}

// use hysteresis to check for hagl
if (in_range_aid_mode) {
if (_range_aid_mode_enabled) {
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);

} else {
Expand All @@ -1071,10 +1080,10 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode)
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
}

return use_range_finder;
_range_aid_mode_enabled = use_range_finder;

} else {
return false;
_range_aid_mode_enabled = false;
}
}

Expand Down
10 changes: 7 additions & 3 deletions EKF/covariance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,9 +163,13 @@ void Ekf::predictCovariance()
float d_vel_bias_sig = dt * dt * math::constrain(_params.accel_bias_p_noise, 0.0f, 1.0f);

// inhibit learning of imu acccel bias if the manoeuvre levels are too high to protect against the effect of sensor nonlinearities or bad accel data is detected
float alpha = 1.0f - math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt);
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt);
float alpha = math::constrain((dt / _params.acc_bias_learn_tc), 0.0f, 1.0f);
float beta = 1.0f - alpha;
_ang_rate_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_ang.norm(), beta * _ang_rate_mag_filt);
_accel_mag_filt = fmaxf(dt_inv * _imu_sample_delayed.delta_vel.norm(), beta * _accel_mag_filt);
_accel_vec_filt(0) = alpha * dt_inv * _imu_sample_delayed.delta_vel(0) + beta * _accel_vec_filt(0);
_accel_vec_filt(1) = alpha * dt_inv * _imu_sample_delayed.delta_vel(1) + beta * _accel_vec_filt(1);
_accel_vec_filt(2) = alpha * dt_inv * _imu_sample_delayed.delta_vel(2) + beta * _accel_vec_filt(2);

if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim
|| _accel_mag_filt > _params.acc_bias_learn_acc_lim
Expand Down
10 changes: 7 additions & 3 deletions EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -372,6 +372,7 @@ class Ekf : public EstimatorInterface
float _gps_velN_filt{0.0f}; ///< GPS filtered North velocity (m/sec)
float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec)
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks

// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec)
Expand Down Expand Up @@ -401,7 +402,8 @@ class Ekf : public EstimatorInterface

// variables used to inhibit accel bias learning
bool _accel_bias_inhibit{false}; ///< true when the accel bias learning is being inhibited
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (m/sec**2)
Vector3f _accel_vec_filt{}; ///< acceleration vector after application of a low pass filter (m/sec**2)
float _accel_mag_filt{0.0f}; ///< acceleration magnitude after application of a decaying envelope filter (rad/sec)
float _ang_rate_mag_filt{0.0f}; ///< angular rate magnitude after application of a decaying envelope filter (rad/sec)
Vector3f _prev_dvel_bias_var; ///< saved delta velocity XYZ bias variances (m/sec)**2

Expand Down Expand Up @@ -431,7 +433,8 @@ class Ekf : public EstimatorInterface
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{false}; ///< true when range finder is to be used as the height reference instead of the primary height sensor
bool _range_aid_mode_enabled{false}; ///< true when range finder can be used as the height reference instead of the primary height sensor
bool _range_aid_mode_selected{false}; ///< true when range finder is being used as the height reference instead of the primary height sensor

// variables used to check for "stuck" rng data
float _rng_check_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck
Expand Down Expand Up @@ -583,7 +586,8 @@ class Ekf : public EstimatorInterface
// control for combined height fusion mode (implemented for switching between baro and range height)
void controlHeightFusion();

bool rangeAidConditionsMet(bool in_range_aid_mode);
// determine if flight condition is suitable so use range finder instead of the primary height senor
void rangeAidConditionsMet();

// check for "stuck" range finder measurements when rng was not valid for certain period
void checkForStuckRange();
Expand Down
14 changes: 7 additions & 7 deletions EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -168,7 +168,9 @@ bool Ekf::resetPosition()

}

setDiag(P, 7, 8, sq(_params.pos_noaid_noise));
// estimate is relative to initial positon in this mode, so we start with zero error.
zeroCols(P,7,8);
zeroRows(P,7,8);

} else {
// Used when falling back to non-aiding mode of operation
Expand Down Expand Up @@ -567,7 +569,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
Dcmf R_to_earth(euler321);

// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
Expand Down Expand Up @@ -621,7 +623,7 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
R_to_earth(2, 1) = s1;

// calculate the observed yaw angle
if (_params.fusion_mode & MASK_USE_EVYAW) {
if (_control_status.flags.ev_yaw) {
// convert the observed quaternion to a rotation matrix
Dcmf R_to_earth_ev(_ev_sample_delayed.quat); // transformation matrix from body to world frame
// calculate the yaw angle for a 312 sequence
Expand Down Expand Up @@ -703,14 +705,12 @@ bool Ekf::resetMagHeading(Vector3f &mag_init)
_R_to_earth = quat_to_invrotmat(_state.quat_nominal);

// reset the rotation from the EV to EKF frame of reference if it is being used
if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS)
&& !(_params.fusion_mode & MASK_USE_EVYAW)) {

if ((_params.fusion_mode & MASK_ROTATE_EV) && (_params.fusion_mode & MASK_USE_EVPOS) && !_control_status.flags.ev_yaw) {
resetExtVisRotMat();
}

// update the yaw angle variance using the variance of the measurement
if (_params.fusion_mode & MASK_USE_EVYAW) {
if (!_control_status.flags.ev_yaw) {
// using error estimate from external vision data
angle_err_var_vec(2) = sq(fmaxf(_ev_sample_delayed.angErr, 1.0e-2f));

Expand Down
8 changes: 6 additions & 2 deletions EKF/estimator_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -364,20 +364,24 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl


// check magnitude is within sensor limits
// use this to prevent use of a saturated flow sensor when there are other aiding sources available
float flow_rate_magnitude;
bool flow_magnitude_good = true;

if (delta_time_good) {
flow_rate_magnitude = flow->flowdata.norm() / delta_time;
flow_magnitude_good = (flow_rate_magnitude <= _params.flow_rate_max);
}

bool relying_on_flow = _control_status.flags.opt_flow
&& !_control_status.flags.gps
&& !_control_status.flags.ev_pos;

// check quality metric
bool flow_quality_good = (flow->quality >= _params.flow_qual_min);

// Always use data when on ground to allow for bad quality due to unfocussed sensors and operator handling
// If flow quality fails checks on ground, assume zero flow rate after body rate compensation
if ((delta_time_good && flow_quality_good && flow_magnitude_good) || !_control_status.flags.in_air) {
if ((delta_time_good && flow_quality_good && (flow_magnitude_good || relying_on_flow)) || !_control_status.flags.in_air) {
flowSample optflow_sample_new;
// calculate the system time-stamp for the mid point of the integration period
optflow_sample_new.time_us = time_usec - _params.flow_delay_ms * 1000 - flow->dt / 2;
Expand Down
Loading

0 comments on commit 16976d3

Please sign in to comment.