From de7c9d85a4eb1de98095ab0ab7dbf3d71c2b5549 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 12 Jun 2018 15:52:34 +1000 Subject: [PATCH] EKF: Handle flow data without valid gyro data --- EKF/common.h | 1 - EKF/control.cpp | 28 +++++++++++++----- EKF/ekf.h | 6 ++-- EKF/ekf_helper.cpp | 4 +-- EKF/estimator_interface.cpp | 15 +--------- EKF/optflow_fusion.cpp | 58 ++++++++++++++++++++++--------------- 6 files changed, 62 insertions(+), 50 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index fad9a04a1433..3ddc5172cf03 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -139,7 +139,6 @@ struct airspeedSample { struct flowSample { uint8_t quality; ///< quality indicator between 0 and 255 Vector2f flowRadXY; ///< measured delta angle of the image about the X and Y body axes (rad), RH rotaton is positive - Vector2f flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive Vector3f gyroXYZ; ///< measured delta angle of the inertial frame about the body axes obtained from rate gyro measurements (rad), RH rotation is positive float dt; ///< amount of integration time (sec) uint64_t time_us; ///< timestamp of the integration period mid-point (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index 46de191a3a7a..b004a769228b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -455,16 +455,28 @@ void Ekf::controlOpticalFlowFusion() } } - // 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(); + // Only fuse optical flow if valid body rate compensation data is available + if (calcOptFlowBodyRateComp()) { - // Fuse optical flow LOS rate observations into the main filter - fuseOptFlow(); - _last_known_posNE(0) = _state.pos(0); - _last_known_posNE(1) = _state.pos(1); + bool flow_quality_good = (_flow_sample_delayed.quality >= _params.flow_qual_min); + if (!flow_quality_good && !_control_status.flags.in_air) { + // when on the ground with poor flow quality, assume zero ground relative velocity and LOS rate + _flowRadXYcomp.zero(); + + } else { + // compensate for body motion to give a LOS rate + _flowRadXYcomp(0) = _flow_sample_delayed.flowRadXY(0) - _flow_sample_delayed.gyroXYZ(0); + _flowRadXYcomp(1) = _flow_sample_delayed.flowRadXY(1) - _flow_sample_delayed.gyroXYZ(1); + } + + + // Fuse optical flow LOS rate observations into the main filter if height above ground has been updated recently but use a relaxed time criteria to enable it to coast through bad range finder data + if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)10e6)) { + fuseOptFlow(); + _last_known_posNE(0) = _state.pos(0); + _last_known_posNE(1) = _state.pos(1); + } } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 0b46088cfa9c..5f4eecf13851 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -355,6 +355,7 @@ class Ekf : public EstimatorInterface uint64_t _time_bad_motion_us{0}; ///< last system time that on-ground motion exceeded limits (uSec) uint64_t _time_good_motion_us{0}; ///< last system time that on-ground motion was within limits (uSec) bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited + Vector2f _flowRadXYcomp; ///< measured delta angle of the image about the X and Y body axes after removal of body rotation (rad), RH rotation is positive float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) @@ -486,8 +487,9 @@ class Ekf : public EstimatorInterface // fuse optical flow line of sight rate measurements void fuseOptFlow(); - // calculate optical flow bias errors - void calcOptFlowBias(); + // calculate optical flow body angular rate compensation + // returns false if bias corrected body rate data is unavailable + bool calcOptFlowBodyRateComp(); // initialise the terrain vertical position estimator // return true if the initialisation is successful diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 42465569e8a7..276af0b7f0b6 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -71,8 +71,8 @@ bool Ekf::resetVelocity() // we should have reliable OF measurements so // calculate X and Y body relative velocities from OF measurements Vector3f vel_optflow_body; - vel_optflow_body(0) = - range * _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt; - vel_optflow_body(1) = range * _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt; + vel_optflow_body(0) = - range * _flowRadXYcomp(1) / _flow_sample_delayed.dt; + vel_optflow_body(1) = range * _flowRadXYcomp(0) / _flow_sample_delayed.dt; vel_optflow_body(2) = 0.0f; // rotate from body to earth frame diff --git a/EKF/estimator_interface.cpp b/EKF/estimator_interface.cpp index 585bf4c6ebc1..5abf159ef223 100644 --- a/EKF/estimator_interface.cpp +++ b/EKF/estimator_interface.cpp @@ -405,20 +405,7 @@ void EstimatorInterface::setOpticalFlowData(uint64_t time_usec, flow_message *fl // NOTE: the EKF uses the reverse sign convention to the flow sensor. EKF assumes positive LOS rate is produced by a RH rotation of the image about the sensor axis. // copy the optical and gyro measured delta angles optflow_sample_new.gyroXYZ = - flow->gyrodata; - - if (flow_quality_good) { - optflow_sample_new.flowRadXY = - flow->flowdata; - - } else { - // when on the ground with poor flow quality, assume zero ground relative velocity - optflow_sample_new.flowRadXY(0) = - flow->gyrodata(0); - optflow_sample_new.flowRadXY(1) = - flow->gyrodata(1); - - } - - // compensate for body motion to give a LOS rate - optflow_sample_new.flowRadXYcomp(0) = optflow_sample_new.flowRadXY(0) - optflow_sample_new.gyroXYZ(0); - optflow_sample_new.flowRadXYcomp(1) = optflow_sample_new.flowRadXY(1) - optflow_sample_new.gyroXYZ(1); + optflow_sample_new.flowRadXY = - flow->flowdata; // convert integration interval to seconds optflow_sample_new.dt = delta_time; diff --git a/EKF/optflow_fusion.cpp b/EKF/optflow_fusion.cpp index 45342bc4f971..25961eef48ba 100644 --- a/EKF/optflow_fusion.cpp +++ b/EKF/optflow_fusion.cpp @@ -93,8 +93,8 @@ void Ekf::fuseOptFlow() // correct for gyro bias errors in the data used to do the motion compensation // Note the sign convention used: A positive LOS rate is a RH rotaton of the scene about that axis. Vector2f opt_flow_rate; - opt_flow_rate(0) = _flow_sample_delayed.flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); - opt_flow_rate(1) = _flow_sample_delayed.flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); + opt_flow_rate(0) = _flowRadXYcomp(0) / _flow_sample_delayed.dt + _flow_gyro_bias(0); + opt_flow_rate(1) = _flowRadXYcomp(1) / _flow_sample_delayed.dt + _flow_gyro_bias(1); if (opt_flow_rate.norm() < _flow_max_rate) { _flow_innov[0] = vel_body(1) / range - opt_flow_rate(0); // flow around the X axis @@ -527,39 +527,51 @@ void Ekf::get_drag_innov_var(float drag_innov_var[2]) memcpy(drag_innov_var, _drag_innov_var, sizeof(_drag_innov_var)); } -// calculate optical flow gyro bias errors -void Ekf::calcOptFlowBias() +// calculate optical flow body angular rate compensation +// returns false if bias corrected body rate data is unavailable +bool Ekf::calcOptFlowBodyRateComp() { // reset the accumulators if the time interval is too large if (_delta_time_of > 1.0f) { _imu_del_ang_of.setZero(); _delta_time_of = 0.0f; - return; + return false; } - // if accumulation time differences are not excessive and accumulation time is adequate - // compare the optical flow and and navigation rate data and calculate a bias error - if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { - // calculate a reference angular rate - Vector3f reference_body_rate; - reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); - - // calculate the optical flow sensor measured body rate - Vector3f of_body_rate; - of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); - - // calculate the bias estimate using a combined LPF and spike filter - _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), - -0.1f, 0.1f); - _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), - -0.1f, 0.1f); - _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), - -0.1f, 0.1f); + bool use_flow_sensor_gyro = ISFINITE(_flow_sample_delayed.gyroXYZ(0)) && ISFINITE(_flow_sample_delayed.gyroXYZ(1)) && ISFINITE(_flow_sample_delayed.gyroXYZ(2)); + + if (use_flow_sensor_gyro) { + + // if accumulation time differences are not excessive and accumulation time is adequate + // compare the optical flow and and navigation rate data and calculate a bias error + if ((fabsf(_delta_time_of - _flow_sample_delayed.dt) < 0.1f) && (_delta_time_of > FLT_EPSILON)) { + // calculate a reference angular rate + Vector3f reference_body_rate; + reference_body_rate = _imu_del_ang_of * (1.0f / _delta_time_of); + + // calculate the optical flow sensor measured body rate + Vector3f of_body_rate; + of_body_rate = _flow_sample_delayed.gyroXYZ * (1.0f / _flow_sample_delayed.dt); + + // calculate the bias estimate using a combined LPF and spike filter + _flow_gyro_bias(0) = 0.99f * _flow_gyro_bias(0) + 0.01f * math::constrain((of_body_rate(0) - reference_body_rate(0)), + -0.1f, 0.1f); + _flow_gyro_bias(1) = 0.99f * _flow_gyro_bias(1) + 0.01f * math::constrain((of_body_rate(1) - reference_body_rate(1)), + -0.1f, 0.1f); + _flow_gyro_bias(2) = 0.99f * _flow_gyro_bias(2) + 0.01f * math::constrain((of_body_rate(2) - reference_body_rate(2)), + -0.1f, 0.1f); + } + + } else { + // Use the EKF gyro data if optical flow sensor gyro data is not available + _flow_sample_delayed.gyroXYZ = _imu_del_ang_of; + _flow_gyro_bias.zero(); } // reset the accumulators _imu_del_ang_of.setZero(); _delta_time_of = 0.0f; + return true; } // calculate the measurement variance for the optical flow sensor (rad/sec)^2