mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-08 21:51:37 +08:00
ekf2: enable multiple height sources fusion
Instead of having a single height source fused into the EKF and the other ones "waiting" for a failure or the primary sensor, fuse all sources in EKF2 at the same time. To prevent the sources from fighting against each other, the "primary" source is set as reference and the other ones are running a bias estimator in order to make all the secondary height sources converge to the primary one. If the reference isn't available, another one is automatically selected from a priority list. This secondary reference keeps its current bias estimate but stops updating it in order to be the new reference as close as possible to the primary one.
This commit is contained in:
bresch
+256
-231
@@ -172,9 +172,6 @@ void Ekf::controlFusionModes()
|
||||
// run EKF-GSF yaw estimator once per _imu_sample_delayed update after all main EKF data samples available
|
||||
runYawEKFGSF();
|
||||
|
||||
// check for height sensor timeouts and reset and change sensor if necessary
|
||||
controlHeightSensorTimeouts();
|
||||
|
||||
// control use of observations for aiding
|
||||
controlMagFusion();
|
||||
controlOpticalFlowFusion();
|
||||
@@ -660,134 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlHeightSensorTimeouts()
|
||||
{
|
||||
/*
|
||||
* Handle the case where we have not fused height measurements recently and
|
||||
* uncertainty exceeds the max allowable. Reset using the best available height
|
||||
* measurement source, continue using it after the reset and declare the current
|
||||
* source failed if we have switched.
|
||||
*/
|
||||
|
||||
checkVerticalAccelerationHealth();
|
||||
|
||||
// check if height is continuously failing because of accel errors
|
||||
const bool continuous_bad_accel_hgt = isTimedOut(_time_good_vert_accel, (uint64_t)_params.bad_acc_reset_delay_us);
|
||||
|
||||
// check if height has been inertial deadreckoning for too long
|
||||
// in vision hgt mode check for vision data
|
||||
const bool hgt_fusion_timeout = isTimedOut(_time_last_hgt_fuse, (uint64_t)5e6);
|
||||
|
||||
if (hgt_fusion_timeout || continuous_bad_accel_hgt) {
|
||||
|
||||
const char *failing_height_source = nullptr;
|
||||
const char *new_height_source = nullptr;
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
bool reset_to_gps = false;
|
||||
|
||||
// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
|
||||
if (!_gps_intermittent) {
|
||||
reset_to_gps = (_gps_checks_passed && !_fault_status.flags.bad_acc_vertical) || _baro_hgt_faulty || _baro_hgt_intermittent;
|
||||
}
|
||||
|
||||
if (reset_to_gps) {
|
||||
// set height sensor health
|
||||
_baro_hgt_faulty = true;
|
||||
|
||||
startGpsHgtFusion();
|
||||
|
||||
failing_height_source = "baro";
|
||||
new_height_source = "gps";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
resetHeightToBaro();
|
||||
|
||||
failing_height_source = "baro";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt) {
|
||||
bool reset_to_baro = false;
|
||||
|
||||
// if baro data is available and GPS data is inaccurate and the timeout cannot be blamed on IMU data, reset height to baro
|
||||
if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
reset_to_baro = (!_fault_status.flags.bad_acc_vertical && !_gps_checks_passed) || _gps_intermittent;
|
||||
}
|
||||
|
||||
if (reset_to_baro) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "gps";
|
||||
new_height_source = "baro";
|
||||
|
||||
} else if (!_gps_intermittent) {
|
||||
resetHeightToGps();
|
||||
|
||||
failing_height_source = "gps";
|
||||
new_height_source = "gps";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt) {
|
||||
|
||||
if (_range_sensor.isHealthy()) {
|
||||
resetHeightToRng();
|
||||
|
||||
failing_height_source = "rng";
|
||||
new_height_source = "rng";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "rng";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt) {
|
||||
// check if vision data is available
|
||||
bool ev_data_available = false;
|
||||
|
||||
if (_ext_vision_buffer) {
|
||||
const extVisionSample &ev_init = _ext_vision_buffer->get_newest();
|
||||
ev_data_available = isRecent(ev_init.time_us, 2 * EV_MAX_INTERVAL);
|
||||
}
|
||||
|
||||
if (ev_data_available) {
|
||||
resetHeightToEv();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "ev";
|
||||
|
||||
} else if (_range_sensor.isHealthy()) {
|
||||
// Fallback to rangefinder data if available
|
||||
startRngHgtFusion();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "rng";
|
||||
|
||||
} else if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
failing_height_source = "ev";
|
||||
new_height_source = "baro";
|
||||
}
|
||||
}
|
||||
|
||||
if (failing_height_source && new_height_source) {
|
||||
_warning_events.flags.height_sensor_timeout = true;
|
||||
ECL_WARN("%s hgt timeout - reset to %s", failing_height_source, new_height_source);
|
||||
}
|
||||
|
||||
// Also reset the vertical velocity
|
||||
if (_control_status.flags.gps && !_gps_intermittent && _gps_checks_passed) {
|
||||
resetVerticalVelocityToGps(_gps_sample_delayed);
|
||||
|
||||
} else {
|
||||
resetVerticalVelocityToZero();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::checkVerticalAccelerationHealth()
|
||||
{
|
||||
// Check for IMU accelerometer vibration induced clipping as evidenced by the vertical
|
||||
@@ -797,6 +666,7 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
// Don't use stale innovation data.
|
||||
bool is_inertial_nav_falling = false;
|
||||
bool are_vertical_pos_and_vel_independant = false;
|
||||
bool is_using_multiple_hgt_sources = false;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
// GNSS velocity
|
||||
@@ -827,6 +697,8 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
}
|
||||
|
||||
if (isRecent(_vert_pos_fuse_attempt_time_us, 1000000)) {
|
||||
is_using_multiple_hgt_sources = _control_status.flags.gps_hgt && _control_status.flags.baro_hgt;
|
||||
|
||||
if (isRecent(_vert_vel_fuse_time_us, 1000000)) {
|
||||
// If vertical position and velocity come from independent sensors then we can
|
||||
// trust them more if they disagree with the IMU, but need to check that they agree
|
||||
@@ -861,7 +733,7 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
|
||||
// if vertical velocity and position are independent and agree, then do not require evidence of clipping if
|
||||
// innovations are large
|
||||
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_clipping_frequently) && is_inertial_nav_falling;
|
||||
const bool bad_vert_accel = (are_vertical_pos_and_vel_independant || is_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling;
|
||||
|
||||
if (bad_vert_accel) {
|
||||
_time_bad_vert_accel = _time_last_imu;
|
||||
@@ -882,115 +754,75 @@ void Ekf::checkVerticalAccelerationHealth()
|
||||
|
||||
void Ekf::controlHeightFusion()
|
||||
{
|
||||
checkVerticalAccelerationHealth();
|
||||
checkRangeAidSuitability();
|
||||
const bool do_range_aid = (_params.range_aid == 1) && _is_range_aid_suitable;
|
||||
|
||||
switch (_params.vdist_sensor_type) {
|
||||
default:
|
||||
ECL_ERR("Invalid hgt mode: %" PRIi32, _params.vdist_sensor_type);
|
||||
|
||||
// FALLTHROUGH
|
||||
case VerticalHeightSensor::BARO:
|
||||
if (do_range_aid) {
|
||||
if (!_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
startRngAidHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!_control_status.flags.baro_hgt) {
|
||||
if (!_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
startBaroHgtFusion();
|
||||
|
||||
} else if (!_control_status.flags.gps_hgt && !_gps_intermittent && _gps_checks_passed) {
|
||||
// Use GPS as a fallback
|
||||
startGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::RANGE:
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
if (!_control_status.flags.in_air
|
||||
&& !_range_sensor.isDataHealthy()
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& _range_sensor.isDataReady()) {
|
||||
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
}
|
||||
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
if (_range_sensor.isDataHealthy()) {
|
||||
startRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::GPS:
|
||||
|
||||
// NOTE: emergency fallback due to extended loss of currently selected sensor data or failure
|
||||
// to pass innovation cinsistency checks is handled elsewhere in Ekf::controlHeightSensorTimeouts.
|
||||
// Do switching between GPS and rangefinder if using range finder as a height source when close
|
||||
// to ground and moving slowly. Also handle switch back from emergency Baro sensor when GPS recovers.
|
||||
if (do_range_aid) {
|
||||
if (!_control_status_prev.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
startRngAidHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (!_control_status.flags.gps_hgt) {
|
||||
if (!_gps_intermittent && _gps_checks_passed) {
|
||||
// In fallback mode and GPS has recovered so start using it
|
||||
startGpsHgtFusion();
|
||||
|
||||
} else if (!_control_status.flags.baro_hgt && !_baro_hgt_faulty && !_baro_hgt_intermittent) {
|
||||
// Use baro as a fallback
|
||||
startBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case VerticalHeightSensor::EV:
|
||||
|
||||
// don't start using EV data unless data is arriving frequently
|
||||
if (!_control_status.flags.ev_hgt && isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) {
|
||||
startEvHgtFusion();
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
updateBaroHgtBias();
|
||||
updateBaroHgtOffset();
|
||||
updateGroundEffect();
|
||||
|
||||
if (_baro_data_ready) {
|
||||
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
|
||||
controlBaroHeightFusion();
|
||||
controlGnssHeightFusion();
|
||||
controlRangeHeightFusion();
|
||||
controlEvHeightFusion();
|
||||
|
||||
if (_control_status.flags.baro_hgt && !_baro_hgt_faulty) {
|
||||
fuseBaroHgt(_aid_src_baro_hgt);
|
||||
}
|
||||
checkHeightSensorRefFallback();
|
||||
}
|
||||
|
||||
void Ekf::checkHeightSensorRefFallback()
|
||||
{
|
||||
if (_height_sensor_ref != HeightSensor::UNKNOWN) {
|
||||
// The reference sensor is running, all good
|
||||
return;
|
||||
}
|
||||
|
||||
if (_rng_data_ready) {
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
HeightSensor fallback_list[4];
|
||||
|
||||
if (_control_status.flags.rng_hgt && _range_sensor.isDataHealthy()) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
}
|
||||
switch (_params.height_sensor_ref) {
|
||||
default:
|
||||
/* FALLTHROUGH */
|
||||
case HeightSensor::UNKNOWN:
|
||||
fallback_list[0] = HeightSensor::GNSS;
|
||||
fallback_list[1] = HeightSensor::BARO;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::BARO:
|
||||
fallback_list[0] = HeightSensor::BARO;
|
||||
fallback_list[1] = HeightSensor::GNSS;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::GNSS:
|
||||
fallback_list[0] = HeightSensor::GNSS;
|
||||
fallback_list[1] = HeightSensor::BARO;
|
||||
fallback_list[2] = HeightSensor::EV;
|
||||
fallback_list[3] = HeightSensor::RANGE;
|
||||
break;
|
||||
|
||||
case HeightSensor::RANGE:
|
||||
fallback_list[0] = HeightSensor::RANGE;
|
||||
fallback_list[1] = HeightSensor::EV;
|
||||
fallback_list[2] = HeightSensor::BARO;
|
||||
fallback_list[3] = HeightSensor::GNSS;
|
||||
break;
|
||||
|
||||
case HeightSensor::EV:
|
||||
fallback_list[0] = HeightSensor::EV;
|
||||
fallback_list[1] = HeightSensor::RANGE;
|
||||
fallback_list[2] = HeightSensor::BARO;
|
||||
fallback_list[3] = HeightSensor::GNSS;
|
||||
break;
|
||||
}
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
|
||||
if (_control_status.flags.ev_hgt && _ev_data_ready) {
|
||||
fuseEvHgt();
|
||||
for (unsigned i = 0; i < 4; i++) {
|
||||
if (((fallback_list[i] == HeightSensor::BARO) && _control_status.flags.baro_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::GNSS) && _control_status.flags.gps_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::RANGE) && _control_status.flags.rng_hgt)
|
||||
|| ((fallback_list[i] == HeightSensor::EV) && _control_status.flags.ev_hgt)) {
|
||||
ECL_INFO("fallback to secondary height reference");
|
||||
_height_sensor_ref = fallback_list[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1024,6 +856,199 @@ void Ekf::checkRangeAidSuitability()
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlBaroHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_BARO_HGT)) {
|
||||
stopBaroHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_baro_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_baro_data_ready) {
|
||||
_baro_lpf.update(_baro_sample_delayed.hgt);
|
||||
updateBaroHgt(_baro_sample_delayed, _aid_src_baro_hgt);
|
||||
|
||||
const bool continuing_conditions_passing = !_baro_hgt_faulty && !_baro_hgt_intermittent;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_control_status.flags.baro_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseBaroHgt(_aid_src_baro_hgt);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, (uint64_t)5e6);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToBaro();
|
||||
resetVerticalVelocityToZero();
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopBaroHgtFusion();
|
||||
_baro_hgt_faulty = true;
|
||||
}
|
||||
|
||||
} else {
|
||||
stopBaroHgtFusion();
|
||||
}
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.baro_hgt && _baro_hgt_intermittent) {
|
||||
// No baro data anymore. Stop until it comes back.
|
||||
stopBaroHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlGnssHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_GPS_HGT)) {
|
||||
stopGpsHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_gps_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_gps_data_ready) {
|
||||
const bool continuing_conditions_passing = !_gps_intermittent && _gps_checks_passed && _NED_origin_initialised;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing;
|
||||
|
||||
if (_control_status.flags.gps_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
/* fuseGpsHgt(); */ // Done in fuseGpsPos
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_pos.time_last_fuse[2], _params.reset_timeout_max);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToGps();
|
||||
resetVerticalVelocityToGps(_gps_sample_delayed);
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps_hgt && _gps_intermittent) {
|
||||
stopGpsHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlRangeHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_RNG_HGT)) {
|
||||
stopRngHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_rng_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
// If we are supposed to be using range finder data as the primary height sensor, have bad range measurements
|
||||
// and are on the ground, then synthesise a measurement at the expected on ground value
|
||||
if (!_control_status.flags.in_air
|
||||
&& !_range_sensor.isDataHealthy()
|
||||
&& _range_sensor.isRegularlySendingData()
|
||||
&& _range_sensor.isDataReady()) {
|
||||
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
}
|
||||
|
||||
if (_rng_data_ready) {
|
||||
|
||||
// TODO: primary height source:
|
||||
// run a bias estimator for all other sources
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
const bool continuing_conditions_passing = _range_sensor.isDataHealthy();
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_rng_hgt.time_last_fuse, (uint64_t)5e6);
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToRng();
|
||||
resetVerticalVelocityToZero();
|
||||
|
||||
} else if (is_fusion_failing) {
|
||||
// Some other height source is still working
|
||||
stopRngHgtFusion();
|
||||
_control_status.flags.rng_fault = true;
|
||||
_range_sensor.setFaulty();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt && isTimedOut(_time_last_range, (uint64_t)5e6)) {
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlEvHeightFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_HGT)) {
|
||||
stopEvHgtFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
_ev_hgt_b_est.predict(_dt_ekf_avg);
|
||||
|
||||
if (_ev_data_ready) {
|
||||
const bool continuing_conditions_passing = true;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL);
|
||||
|
||||
if (_control_status.flags.ev_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseEvHgt();
|
||||
|
||||
if (isHeightResetRequired()) {
|
||||
// All height sources are failing
|
||||
resetHeightToRng();
|
||||
resetVerticalVelocityToZero();
|
||||
}
|
||||
|
||||
} else {
|
||||
stopEvHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startEvHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.ev_hgt && isTimedOut(_time_last_ext_vision, (uint64_t)5e6)) {
|
||||
stopEvHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::controlAirDataFusion()
|
||||
{
|
||||
// control activation and initialisation/reset of wind states required for airspeed fusion
|
||||
|
||||
Reference in New Issue
Block a user