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; } }