From 02055acee27cd4b542a5858c3c266a99cf55513e Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 9 Apr 2018 18:35:15 +1000 Subject: [PATCH] EKF: Fix non GPS aiding data reset logic (#418) * EKF: Move optical flow specific state reset to helper functions * EKF: Ensure loss of optical flow aiding is handled correctly If data is only source of aiding and has been rejected for too long - reset using flow data as a velocity reference. If flow data is unavailable for too long - declare optical flow use stopped. Use consistent time periods for all resets * EKF: Ensure loss of external vision aiding is handled correctly If data is only source of aiding and has been rejected for too long - reset using data as a position. Don't reset velocity if there is another source of aiding constraining it. If data is unavailable for too long, declare external vision use stopped. Use consistent time periods for all resets. * EKF: Update parameter documentation Make the distinction between the no_gps_timeout_max and no_aid_timeout_max parameters clearer * EKF: make class variable units consistent with documentation * EKF: Don't reset states when optical flow use commences if using external vision * EKF: Stop optical flow fusion when on ground if excessive movement is detected. * EKF: fix terrain estimator vulnerabilities Reset estimate to sensor value if rejected for 10 seconds Protect against user motion when on ground. Fix unnecessary duplication of terrain validity check and separate validity update and reporting. * EKF: remove unnecessary Info console prints Optical flow use information can be obtained from the estimator_status.control_mode_flags message * EKF: fix inaccurate comment * EKF: remove duplicate calculation from terrain validity accessor function --- EKF/common.h | 8 +-- EKF/control.cpp | 137 ++++++++++++++++++++------------------ EKF/covariance.cpp | 10 +-- EKF/ekf.h | 8 ++- EKF/ekf_helper.cpp | 56 ++++++++++++++-- EKF/terrain_estimator.cpp | 50 +++++++++----- 6 files changed, 172 insertions(+), 97 deletions(-) diff --git a/EKF/common.h b/EKF/common.h index 29181cbb51..917e86566c 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -321,10 +321,10 @@ struct parameters { float acc_bias_learn_gyr_lim{3.0f}; ///< learning is disabled if the magnitude of the IMU angular rate vector is greater than this (rad/sec) float acc_bias_learn_tc{0.5f}; ///< time constant used to control the decaying envelope filters applied to the accel and gyro magnitudes (sec) - unsigned no_gps_timeout_max{7000000}; ///< maximum time we allow dead reckoning while both gps position and velocity measurements are being - ///< rejected before attempting to reset the states to the GPS measurement (uSec) - unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of measurements that constrain drift before - ///< the EKF will report that it is dead-reckoning (uSec) + unsigned no_gps_timeout_max{7000000}; ///< maximum time we allow horizontal inertial dead reckoning before attempting to reset the states to the measurement (uSec) + unsigned no_aid_timeout_max{1000000}; ///< maximum lapsed time from last fusion of measurements that constrain horizontal velocity drift before + ///< the EKF will report that it has been inertial dead-reckoning for too long and needs to revert to a + /// mode that doesn't privide horizontal vbelocity and position estimates (uSec) // multi-rotor drag specific force fusion float drag_noise{2.5f}; ///< observation noise variance for drag specific force measurements (m/sec**2)**2 diff --git a/EKF/control.cpp b/EKF/control.cpp index 02dc2c67c4..039899c769 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -289,6 +289,15 @@ void Ekf::controlExternalVisionFusion() // use the absolute position _vel_pos_innov[3] = _state.pos(0) - _ev_sample_delayed.posNED(0); _vel_pos_innov[4] = _state.pos(1) - _ev_sample_delayed.posNED(1); + + // check if we have been deadreckoning too long + if (_time_last_imu - _time_last_pos_fuse > _params.no_gps_timeout_max) { + // don't reset velocity if we have another source of aiding constraining it + if (_time_last_imu - _time_last_of_fuse > (uint64_t)1E6) { + resetVelocity(); + } + resetPosition(); + } } // observation 1-STD error @@ -311,7 +320,7 @@ void Ekf::controlExternalVisionFusion() fuseHeading(); } - } else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)5e6)) { + } else if (_control_status.flags.ev_pos && (_time_last_imu - _time_last_ext_vision > (uint64_t)_params.no_gps_timeout_max)) { // Turn off EV fusion mode if no data has been received _control_status.flags.ev_pos = false; ECL_INFO("EKF External Vision Data Stopped"); @@ -321,95 +330,97 @@ void Ekf::controlExternalVisionFusion() void Ekf::controlOpticalFlowFusion() { - // Check for new optical flow data that has fallen behind the fusion time horizon + // Detect if the vehicle is on the ground and is being excessively tilted, shaken or rotated. + 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)); + if (motion_is_excessive) { + _time_bad_motion_us = _imu_sample_delayed.time_us; + } else { + _time_good_motion_us = _imu_sample_delayed.time_us; + } + } else { + _time_bad_motion_us = 0; + _time_good_motion_us = _imu_sample_delayed.time_us; + } + + // Inhibit flow use if on ground and motion is excessive + // 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) { + _inhibit_gndobs_use = true; + } + } else { + if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) { + _inhibit_gndobs_use = false; + } + } + + // Handle cases where we are using optical flow but are no longer able to because data is old + // or its use has been inhibited. + if (_control_status.flags.opt_flow) { + if (_inhibit_gndobs_use) { + _control_status.flags.opt_flow = false; + _time_last_of_fuse = 0; + + } else if (_time_last_imu - _flow_sample_delayed.time_us > (uint64_t)_params.no_gps_timeout_max) { + _control_status.flags.opt_flow = false; + + } + } + if (_flow_data_ready) { + // New optical flow data has fallen behind the fusion time horizon and is ready to be fused + + // Accumulate autopilot gyro data across the same time interval as the flow sensor + _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias; + _delta_time_of += _imu_sample_delayed.delta_ang_dt; // optical flow fusion mode selection logic if ((_params.fusion_mode & MASK_USE_OF) // optical flow has been selected by the user && !_control_status.flags.opt_flow // we are not yet using flow data && _control_status.flags.tilt_align // we know our tilt attitude - && (_time_last_imu - _time_last_hagl_fuse) < (uint64_t)5e5) // we have a valid distance to ground estimate + && get_terrain_valid()) // we have a valid distance to ground estimate { - // If the heading is not aligned, reset the yaw and magnetic field states if (!_control_status.flags.yaw_align) { _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); } - // If the heading is valid, start using optical flow aiding - if (_control_status.flags.yaw_align) { + // If the heading is valid and use is no tinhibited , start using optical flow aiding + if (_control_status.flags.yaw_align && !_inhibit_gndobs_use) { // set the flag and reset the fusion timeout _control_status.flags.opt_flow = true; _time_last_of_fuse = _time_last_imu; ECL_INFO("EKF Starting Optical Flow Use"); // if we are not using GPS then the velocity and position states and covariances need to be set - if (!_control_status.flags.gps) { - // constrain height above ground to be above minimum possible - float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); - - // calculate absolute distance from focal point to centre of frame assuming a flat earth - float range = heightAboveGndEst / _R_rng_to_earth_2_2; - - if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { - // 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(2) = 0.0f; - - // rotate from body to earth frame - Vector3f vel_optflow_earth; - vel_optflow_earth = _R_to_earth * vel_optflow_body; - - // take x and Y components - _state.vel(0) = vel_optflow_earth(0); - _state.vel(1) = vel_optflow_earth(1); - - } else { - _state.vel(0) = 0.0f; - _state.vel(1) = 0.0f; - } - - // reset the velocity covariance terms - zeroRows(P,4,5); - zeroCols(P,4,5); - - // reset the horizontal velocity variance using the optical flow noise variance - P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); - - if (!_control_status.flags.in_air) { - // we are likely starting OF for the first time so reset the horizontal position and vertical velocity states - _state.pos(0) = 0.0f; - _state.pos(1) = 0.0f; - - } else { - // set to the last known position - _state.pos(0) = _last_known_posNE(0); - _state.pos(1) = _last_known_posNE(1); - - } - - // reset the corresponding covariances - // we are by definition at the origin at commencement so variances are also zeroed - zeroRows(P,7,8); - zeroCols(P,7,8); + if (!_control_status.flags.gps || !_control_status.flags.ev_pos) { + resetVelocity(); + resetPosition(); // align the output observer to the EKF states alignOutputFilter(); } } - } else if (!(_params.fusion_mode & MASK_USE_OF)) { _control_status.flags.opt_flow = false; } - // Accumulate autopilot gyro data across the same time interval as the flow sensor - _imu_del_ang_of += _imu_sample_delayed.delta_ang - _state.gyro_bias; - _delta_time_of += _imu_sample_delayed.delta_ang_dt; + // handle the case when we have optical flow, are reliant on it, but have not been using it for an extended period + if (_control_status.flags.opt_flow + && !_control_status.flags.gps + && !_control_status.flags.ev_pos) { + bool do_reset = _time_last_imu - _time_last_of_fuse > _params.no_gps_timeout_max; + if (do_reset) { + resetVelocity(); + resetPosition(); + } + } // fuse the data if the terrain/distance to bottom is valid if (_control_status.flags.opt_flow && get_terrain_valid()) { @@ -422,10 +433,6 @@ void Ekf::controlOpticalFlowFusion() _last_known_posNE(1) = _state.pos(1); } - } else if (_control_status.flags.opt_flow && (_time_last_imu - _time_last_optflow > (uint64_t)5e6)) { - ECL_INFO("EKF Optical Flow Data Stopped"); - _control_status.flags.opt_flow = false; - } } diff --git a/EKF/covariance.cpp b/EKF/covariance.cpp index 647aca9b7b..4f31fed811 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -148,6 +148,7 @@ void Ekf::predictCovariance() float dvz_b = _state.accel_bias(2); float dt = math::constrain(_imu_sample_delayed.delta_ang_dt, 0.0005f * FILTER_UPDATE_PERIOD_MS, 0.002f * FILTER_UPDATE_PERIOD_MS); + float dt_inv = 1.0f / dt; // compute noise variance for stationary processes float process_noise[_k_num_states] = {}; @@ -160,10 +161,11 @@ void Ekf::predictCovariance() // 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(_imu_sample_delayed.delta_ang.norm(), alpha * _ang_rate_mag_filt); - _accel_mag_filt = fmaxf(_imu_sample_delayed.delta_vel.norm(), alpha * _accel_mag_filt); - if (_ang_rate_mag_filt > dt * _params.acc_bias_learn_gyr_lim - || _accel_mag_filt > dt * _params.acc_bias_learn_acc_lim + _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); + + if (_ang_rate_mag_filt > _params.acc_bias_learn_gyr_lim + || _accel_mag_filt > _params.acc_bias_learn_acc_lim || _bad_vert_accel_detected) { // store the bias state variances to be reinstated later if (!_accel_bias_inhibit) { diff --git a/EKF/ekf.h b/EKF/ekf.h index 34979c3983..a88df3018c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -179,6 +179,9 @@ public: // return true if the terrain estimate is valid bool get_terrain_valid(); + // update terrain validity status + void update_terrain_valid(); + // get the estimated terrain vertical position relative to the NED origin void get_terrain_vert_pos(float *ret); @@ -298,7 +301,6 @@ private: uint64_t _time_last_beta_fuse{0}; ///< time the last fusion of synthetic sideslip measurements were performed (uSec) uint64_t _time_last_rng_ready{0}; ///< time the last range finder measurement was ready (uSec) Vector2f _last_known_posNE; ///< last known local NE position vector (m) - float _last_disarmed_posD{0.0f}; ///< vertical position recorded at arming (m) 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) @@ -346,6 +348,9 @@ private: Vector3f _flow_gyro_bias; ///< bias errors in optical flow sensor rate gyro outputs (rad/sec) Vector3f _imu_del_ang_of; ///< bias corrected delta angle measurements accumulated across the same time frame as the optical flow rates (rad) float _delta_time_of{0.0f}; ///< time in sec that _imu_del_ang_of was accumulated over (sec) + 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_gndobs_use{false}; ///< true when use of ground observations (optical flow and range finder) is being temporarily inhibited due to excessive on-ground motion float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad) @@ -410,6 +415,7 @@ private: float _R_rng_to_earth_2_2{0.0f}; ///< 2,2 element of the rotation matrix from sensor frame to earth frame bool _range_data_continuous{false}; ///< true when we are receiving range finder data faster than a 2Hz average float _dt_last_range_update_filt_us{0.0f}; ///< filtered value of the delta time elapsed since the last range measurement came into the filter (uSec) + bool _hagl_valid{false}; ///< true when the height above ground estimate is valid // height sensor fault status bool _baro_hgt_faulty{false}; ///< true if valid baro data is unavailable for use diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 716d0290af..e4c1556bc0 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -60,7 +60,42 @@ bool Ekf::resetVelocity() // use GPS accuracy to reset variances setDiag(P,4,6,sq(_gps_sample_delayed.sacc)); - } else if (_control_status.flags.opt_flow || _control_status.flags.ev_pos) { + } else if (_control_status.flags.opt_flow) { + // constrain height above ground to be above minimum possible + float heightAboveGndEst = fmaxf((_terrain_vpos - _state.pos(2)), _params.rng_gnd_clearance); + + // calculate absolute distance from focal point to centre of frame assuming a flat earth + float range = heightAboveGndEst / _R_rng_to_earth_2_2; + + if ((range - _params.rng_gnd_clearance) > 0.3f && _flow_sample_delayed.dt > 0.05f) { + // 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(2) = 0.0f; + + // rotate from body to earth frame + Vector3f vel_optflow_earth; + vel_optflow_earth = _R_to_earth * vel_optflow_body; + + // take x and Y components + _state.vel(0) = vel_optflow_earth(0); + _state.vel(1) = vel_optflow_earth(1); + + } else { + _state.vel(0) = 0.0f; + _state.vel(1) = 0.0f; + } + + // reset the velocity covariance terms + zeroRows(P,4,5); + zeroCols(P,4,5); + + // reset the horizontal velocity variance using the optical flow noise variance + P[5][5] = P[4][4] = sq(range) * calcOptFlowMeasVar(); + + } else if (_control_status.flags.ev_pos) { _state.vel.setZero(); zeroOffDiag(P,4,6); @@ -112,11 +147,6 @@ bool Ekf::resetPosition() // use GPS accuracy to reset variances setDiag(P,7,8,sq(_gps_sample_delayed.hacc)); - } else if (_control_status.flags.opt_flow) { - _state.pos(0) = 0.0f; - _state.pos(1) = 0.0f; - zeroOffDiag(P,7,8); - } else if (_control_status.flags.ev_pos) { // this reset is only called if we have new ev data at the fusion time horizon _state.pos(0) = _ev_sample_delayed.posNED(0); @@ -125,6 +155,20 @@ bool Ekf::resetPosition() // use EV accuracy to reset variances setDiag(P,7,8,sq(_ev_sample_delayed.posErr)); + } else if (_control_status.flags.opt_flow) { + if (!_control_status.flags.in_air) { + // we are likely starting OF for the first time so reset the horizontal position + _state.pos(0) = 0.0f; + _state.pos(1) = 0.0f; + + } else { + // set to the last known position + _state.pos(0) = _last_known_posNE(0); + _state.pos(1) = _last_known_posNE(1); + + } + setDiag(P,7,8,sq(_params.pos_noaid_noise)); + } else { // Used when falling back to non-aiding mode of operation _state.pos(0) = _last_known_posNE(0); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 82918eeea3..a119266494 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -109,6 +109,9 @@ void Ekf::runTerrainEstimator() _terrain_vpos = _params.rng_gnd_clearance + _state.pos(2); } } + + // Update terrain validity + update_terrain_valid(); } void Ekf::fuseHagl() @@ -134,37 +137,50 @@ void Ekf::fuseHagl() float gate_size = fmaxf(_params.range_innov_gate, 1.0f); _terr_test_ratio = sq(_hagl_innov) / (sq(gate_size) * _hagl_innov_var); - if (_terr_test_ratio <= 1.0f) { - // calculate the Kalman gain - float gain = _terrain_var / _hagl_innov_var; - // correct the state - _terrain_vpos -= gain * _hagl_innov; - // correct the variance - _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); - // record last successful fusion event - _time_last_hagl_fuse = _time_last_imu; - _innov_check_fail_status.flags.reject_hagl = false; - - } else { - _innov_check_fail_status.flags.reject_hagl = true; + if (!_inhibit_gndobs_use) { + if (_terr_test_ratio <= 1.0f) { + // calculate the Kalman gain + float gain = _terrain_var / _hagl_innov_var; + // correct the state + _terrain_vpos -= gain * _hagl_innov; + // correct the variance + _terrain_var = fmaxf(_terrain_var * (1.0f - gain), 0.0f); + // record last successful fusion event + _time_last_hagl_fuse = _time_last_imu; + _innov_check_fail_status.flags.reject_hagl = false; + } else { + // If we have been rejecting range data for too long, reset to measurement + if (_time_last_imu - _time_last_hagl_fuse > (uint64_t)10E6) { + _terrain_vpos = _state.pos(2) + meas_hagl; + _terrain_var = obs_variance; + } else { + _innov_check_fail_status.flags.reject_hagl = true; + } + } } + } else { _innov_check_fail_status.flags.reject_hagl = true; return; } } -// return true if the terrain estimate is valid +// return true if the terrain height estimate is valid bool Ekf::get_terrain_valid() +{ + return _hagl_valid; +} + +// determine terrain validity +void Ekf::update_terrain_valid() { if (_terrain_initialised && _range_data_continuous && !_control_status.flags.rng_stuck && (_time_last_imu - _time_last_hagl_fuse < (uint64_t)5e6)) { - return true; - + _hagl_valid = true; } else { - return false; + _hagl_valid = false; } }