Skip to content

Commit

Permalink
EKF: Fix non GPS aiding data reset logic (PX4#418)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@priseborough
priseborough authored Apr 9, 2018
1 parent ba2b9df commit 02055ac
Show file tree
Hide file tree
Showing 6 changed files with 173 additions and 98 deletions.
8 changes: 4 additions & 4 deletions EKF/common.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
137 changes: 72 additions & 65 deletions EKF/control.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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");
Expand All @@ -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()) {
Expand All @@ -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;

}

}
Expand Down
10 changes: 6 additions & 4 deletions EKF/covariance.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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] = {};
Expand All @@ -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) {
Expand Down
8 changes: 7 additions & 1 deletion EKF/ekf.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -179,6 +179,9 @@ class Ekf : public EstimatorInterface
// 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);

Expand Down Expand Up @@ -298,7 +301,6 @@ class Ekf : public EstimatorInterface
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)
Expand Down Expand Up @@ -346,6 +348,9 @@ class Ekf : public EstimatorInterface
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)

Expand Down Expand Up @@ -410,6 +415,7 @@ class Ekf : public EstimatorInterface
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
Expand Down
56 changes: 50 additions & 6 deletions EKF/ekf_helper.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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);
Expand All @@ -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);
Expand Down
Loading

0 comments on commit 02055ac

Please sign in to comment.