fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-18 16:29:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: improve optical flow GPS quality checking

Browse Source
This commit is contained in:
Paul Riseborough 2018-05-18 13:57:11 +10:00 committed by Lorenz Meier
parent 7f36add241
commit 99a80387ed
3 changed files with 42 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
EKF/control.cpp
View File

@ -353,35 +353,38 @@ void Ekf::controlOpticalFlowFusion()
}
} else {
bool good_gps_aiding = _control_status.flags.gps && ((_time_last_imu - _last_gps_fail_us) > (uint64_t)6e6);
if (good_gps_aiding && !_range_aid_mode_enabled) {
// Detect the special case where we are in flight, are using good quality GPS and speed and range has exceeded
// limits for use of range finder for height
_time_bad_motion_us = _imu_sample_delayed.time_us;
} else {
_time_good_motion_us = _imu_sample_delayed.time_us;
}
_time_good_motion_us = _imu_sample_delayed.time_us;
}
// Inhibit flow use if motion is un-suitable
// 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;
// Inhibit flow use if motion is un-suitable or we have good quality GPS
// Apply hysteresis to prevent rapid mode switching
float gps_err_norm_lim;
if (_control_status.flags.opt_flow) {
gps_err_norm_lim = 0.7f;
} else {
gps_err_norm_lim = 1.0f;
}
if (!_inhibit_flow_use) {
bool movement_not_ok = (_imu_sample_delayed.time_us - _time_good_motion_us) > (uint64_t)1E5;
bool good_gps_aiding = _control_status.flags.gps && _gps_error_norm < gps_err_norm_lim;
if (movement_not_ok || good_gps_aiding || !_range_aid_mode_enabled) {
_inhibit_flow_use = true;
}
} else {
if ((_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6) {
_inhibit_gndobs_use = false;
bool movement_ok = (_imu_sample_delayed.time_us - _time_bad_motion_us) > (uint64_t)5E6;
bool bad_gps_aiding = _control_status.flags.gps && _gps_error_norm > gps_err_norm_lim;
if (movement_ok || bad_gps_aiding) {
_inhibit_flow_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;
if (_inhibit_flow_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;
@ -407,8 +410,8 @@ void Ekf::controlOpticalFlowFusion()
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
}
// If the heading is valid and use is no tinhibited , start using optical flow aiding
if (_control_status.flags.yaw_align && !_inhibit_gndobs_use) {
// If the heading is valid and use is not inhibited , start using optical flow aiding
if (_control_status.flags.yaw_align && !_inhibit_flow_use) {
// set the flag and reset the fusion timeout
_control_status.flags.opt_flow = true;
_time_last_of_fuse = _time_last_imu;
@ -1048,15 +1051,15 @@ void Ekf::rangeAidConditionsMet()
// 3) Our terrain estimate is stable (needs better checks)
// 4) We are in-air
if (_control_status.flags.in_air) {
// check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool use_range_finder;
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
bool can_use_range_finder;
if (_range_aid_mode_enabled) {
use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
can_use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid();
} else {
// if we were not using range aid in the previous iteration then require the current height above terrain to be
// smaller than 70 % of the maximum allowed ground distance for range aid
use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid();
can_use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid();
}
bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)
@ -1066,29 +1069,29 @@ void Ekf::rangeAidConditionsMet()
float ground_vel = sqrtf(_state.vel(0) * _state.vel(0) + _state.vel(1) * _state.vel(1));
if (_range_aid_mode_enabled) {
use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
can_use_range_finder &= ground_vel < _params.max_vel_for_range_aid;
} else {
// if we were not using range aid in the previous iteration then require the ground velocity to be
// smaller than 70 % of the maximum allowed ground velocity for range aid
use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid;
can_use_range_finder &= ground_vel < 0.7f * _params.max_vel_for_range_aid;
}
} else {
use_range_finder = false;
can_use_range_finder = false;
}
// use hysteresis to check for hagl
if (_range_aid_mode_enabled) {
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
can_use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 1.0f);
} else {
// if we were not using range aid in the previous iteration then use a much lower (1/100) threshold to avoid
// switching to range finder too soon (wait for terrain to update).
use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
can_use_range_finder &= ((_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var)) < 0.01f);
}
_range_aid_mode_enabled = use_range_finder;
_range_aid_mode_enabled = can_use_range_finder;
} else {
_range_aid_mode_enabled = false;

3
EKF/ekf.h
View File

@ -358,7 +358,7 @@ private:
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
bool _inhibit_flow_use{false}; ///< true when use of optical flow and range finder is being inhibited
float _mag_declination{0.0f}; ///< magnetic declination used by reset and fusion functions (rad)
@ -379,6 +379,7 @@ private:
float _gps_velE_filt{0.0f}; ///< GPS filtered East velocity (m/sec)
uint64_t _last_gps_fail_us{0}; ///< last system time in usec that the GPS failed it's checks
uint64_t _last_gps_pass_us{0}; ///< last system time in usec that the GPS passed it's checks
float _gps_error_norm{1.0f}; ///< normalised gps error
// Variables used to publish the WGS-84 location of the EKF local NED origin
uint64_t _last_gps_origin_time_us{0}; ///< time the origin was last set (uSec)

25
EKF/gps_checks.cpp
View File

@ -119,26 +119,15 @@ bool Ekf::gps_is_good(struct gps_message *gps)
_gps_check_fail_status.flags.gdop = (gps->gdop > _params.req_gdop);
// Check the reported horizontal and vertical position accuracy
if (_control_status.flags.opt_flow) {
// optical flow is used when low and slow and often in a confined environment
// so tighten required GPS accuracy that affects position and altitude hold
// This also applies some hysteresis to the logic used to activate optical flow
_gps_check_fail_status.flags.hacc = (gps->eph > 0.7f * _params.req_hacc);
_gps_check_fail_status.flags.vacc = (gps->epv > 0.7f * _params.req_vacc);
} else {
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
}
_gps_check_fail_status.flags.hacc = (gps->eph > _params.req_hacc);
_gps_check_fail_status.flags.vacc = (gps->epv > _params.req_vacc);
// Check the reported speed accuracy
if (_control_status.flags.opt_flow) {
// Optical flow is used when low and slow and often in a confined environment
// so tighten required GPS accuracy that affects position hold
// This also applies some hysteresis to the logic used to activate optical flow
_gps_check_fail_status.flags.sacc = (gps->sacc > 0.7f * _params.req_sacc);
} else {
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
}
_gps_check_fail_status.flags.sacc = (gps->sacc > _params.req_sacc);
// check if GPS quality is degraded
_gps_error_norm = fmaxf((gps->eph / _params.req_hacc) , (gps->epv / _params.req_vacc));
_gps_error_norm = fmaxf(_gps_error_norm , (gps->sacc / _params.req_sacc));
// Calculate position movement since last measurement
float delta_posN = 0.0f;