fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 02:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2: height aid source consistency (#20405)

Browse Source 
- working towards keeping all height source (baro/ev/gnss/rng) handling as consistent as possible, possibly refactoring these out into separate classes later
This commit is contained in:
Daniel Agar
2022-10-19 15:35:07 -04:00
committed by GitHub
parent 870229ef49
commit f9509b442c
21 changed files with 890 additions and 1083 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/baro_height_control.cpp
+105 -68
View File
@@ -40,117 +40,154 @@
void Ekf::controlBaroHeightFusion()
{
if (!_baro_buffer) {
return;
}
static constexpr const char *HGT_SRC_NAME = "baro";
auto &aid_src = _aid_src_baro_hgt;
HeightBiasEstimator &bias_est = _baro_b_est;
bias_est.predict(_dt_ekf_avg);
baroSample baro_sample;
const bool baro_data_ready = _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample);
if (baro_data_ready) {
if (_baro_counter == 0) {
_baro_lpf.reset(baro_sample.hgt);
if (_baro_buffer && _baro_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &baro_sample)) {
} else {
_baro_lpf.update(baro_sample.hgt);
const float measurement = baro_sample.hgt;
const float measurement_var = sq(_params.baro_noise);
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
if (measurement_valid) {
if (_baro_counter == 0) {
_baro_lpf.reset(baro_sample.hgt);
} else {
_baro_lpf.update(baro_sample.hgt);
}
if (_baro_counter <= _obs_buffer_length) {
// Initialize the pressure offset (included in the baro bias)
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
_baro_counter++;
}
}
if (_baro_counter < _obs_buffer_length) {
// Initialize the pressure offset (included in the baro bias)
_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
_baro_counter++;
// vertical position innovation - baro measurement has opposite sign to earth z axis
updateVerticalPositionAidSrcStatus(baro_sample.time_us,
-(measurement - bias_est.getBias()),
measurement_var + bias_est.getBiasVar(),
innov_gate,
aid_src);
// Compensate for positive static pressure transients (negative vertical position innovations)
// caused by rotor wash ground interaction by applying a temporary deadzone to baro innovations.
if (_control_status.flags.gnd_effect && (_params.gnd_effect_deadzone > 0.f)) {
const float deadzone_start = 0.0f;
const float deadzone_end = deadzone_start + _params.gnd_effect_deadzone;
if (aid_src.innovation < -deadzone_start) {
if (aid_src.innovation <= -deadzone_end) {
aid_src.innovation += deadzone_end;
} else {
aid_src.innovation = -deadzone_start;
}
}
}
}
if (!(_params.baro_ctrl == 1)) {
stopBaroHgtFusion();
return;
}
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (measurement_valid) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
}
_baro_b_est.predict(_dt_ekf_avg);
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.baro_ctrl == 1)
&& measurement_valid
&& !_baro_hgt_faulty;
// check for intermittent data
const bool baro_hgt_intermittent = !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
if (baro_data_ready) {
updateBaroHgt(baro_sample, _aid_src_baro_hgt);
const bool continuing_conditions_passing = !_baro_hgt_faulty && !baro_hgt_intermittent;
const bool starting_conditions_passing = continuing_conditions_passing && (_baro_counter >= _obs_buffer_length);
const bool starting_conditions_passing = continuing_conditions_passing
&& (_baro_counter > _obs_buffer_length)
&& isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL);
if (_control_status.flags.baro_hgt) {
if (continuing_conditions_passing) {
fuseBaroHgt(_aid_src_baro_hgt);
aid_src.fusion_enabled = true;
const bool is_fusion_failing = isTimedOut(_aid_src_baro_hgt.time_last_fuse, _params.hgt_fusion_timeout_max);
if (continuing_conditions_passing) {
fuseVerticalPosition(aid_src);
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, _params.hgt_fusion_timeout_max);
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToBaro(baro_sample);
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_baro = true;
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
// reset vertical velocity
resetVerticalVelocityToZero();
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else if (is_fusion_failing) {
// Some other height source is still working
ECL_WARN("stopping %s height fusion, fusion failing", HGT_SRC_NAME);
stopBaroHgtFusion();
_baro_hgt_faulty = true;
}
} else {
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
stopBaroHgtFusion();
}
} else {
if (starting_conditions_passing) {
startBaroHgtFusion(baro_sample);
if (_params.height_sensor_ref == HeightSensor::BARO) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::BARO;
_information_events.flags.reset_hgt_to_baro = true;
resetVerticalPositionTo(-(_baro_lpf.getState() - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
} else {
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
bias_est.setBias(_state.pos(2) + _baro_lpf.getState());
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
bias_est.setFusionActive();
_control_status.flags.baro_hgt = true;
}
}
} else if (_control_status.flags.baro_hgt && baro_hgt_intermittent) {
// No baro data anymore. Stop until it comes back.
} else if (_control_status.flags.baro_hgt
&& !isNewestSampleRecent(_time_last_baro_buffer_push, 2 * BARO_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopBaroHgtFusion();
}
}
void Ekf::startBaroHgtFusion(const baroSample &baro_sample)
{
if (!_control_status.flags.baro_hgt) {
if (_params.height_sensor_ref == HeightSensor::BARO) {
_height_sensor_ref = HeightSensor::BARO;
resetHeightToBaro(baro_sample);
} else {
_baro_b_est.setBias(_state.pos(2) + _baro_lpf.getState());
}
_control_status.flags.baro_hgt = true;
_baro_b_est.setFusionActive();
ECL_INFO("starting baro height fusion");
}
}
void Ekf::resetHeightToBaro(const baroSample &baro_sample)
{
ECL_INFO("reset height to baro");
_information_events.flags.reset_hgt_to_baro = true;
resetVerticalPositionTo(-(baro_sample.hgt - _baro_b_est.getBias()));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.baro_noise));
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
}
void Ekf::stopBaroHgtFusion()
{
if (_control_status.flags.baro_hgt) {
if (_height_sensor_ref == HeightSensor::BARO) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.baro_hgt = false;
_baro_b_est.setFusionInactive();
ECL_INFO("stopping baro height fusion");
resetEstimatorAidStatus(_aid_src_baro_hgt);
_control_status.flags.baro_hgt = false;
}
}