fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 07:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Fix non GPS aiding data reset logic (#418)

Browse Source 
* 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
This commit is contained in:
Paul Riseborough
2018-04-09 18:35:15 +10:00
committed by GitHub
parent ba2b9dfdd9
commit 02055acee2
6 changed files with 172 additions and 97 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/control.cpp
+72 -65
View File
@@ -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;
}
}