mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 13:53:34 +08:00
ekf2: height aid source consistency (#20405)
- 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
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file gps_control.cpp
|
||||
* @file range_height_control.cpp
|
||||
* Control functions for ekf range finder height fusion
|
||||
*/
|
||||
|
||||
@@ -40,160 +40,166 @@
|
||||
|
||||
void Ekf::controlRangeHeightFusion()
|
||||
{
|
||||
if (!((_params.rng_ctrl == RngCtrl::CONDITIONAL) || (_params.rng_ctrl == RngCtrl::ENABLED))) {
|
||||
stopRngHgtFusion();
|
||||
return;
|
||||
}
|
||||
static constexpr const char *HGT_SRC_NAME = "RNG";
|
||||
|
||||
_rng_hgt_b_est.predict(_dt_ekf_avg);
|
||||
auto &aid_src = _aid_src_rng_hgt;
|
||||
HeightBiasEstimator &bias_est = _rng_hgt_b_est;
|
||||
|
||||
const bool rng_intermittent = !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL);
|
||||
bias_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()) {
|
||||
if (_rng_data_ready && _range_sensor.getSampleAddress()) {
|
||||
|
||||
_range_sensor.setRange(_params.rng_gnd_clearance);
|
||||
_range_sensor.setValidity(true); // bypass the checks
|
||||
}
|
||||
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
|
||||
const float measurement_var = sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
|
||||
|
||||
if (_rng_data_ready) {
|
||||
updateRngHgt(_aid_src_rng_hgt);
|
||||
const float innov_gate = math::max(_params.range_innov_gate, 1.f);
|
||||
|
||||
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
|
||||
|
||||
// vertical position innovation - baro measurement has opposite sign to earth z axis
|
||||
updateVerticalPositionAidSrcStatus(_range_sensor.getSampleAddress()->time_us,
|
||||
-(measurement - bias_est.getBias()),
|
||||
measurement_var + bias_est.getBiasVar(),
|
||||
innov_gate,
|
||||
aid_src);
|
||||
|
||||
// update the bias estimator before updating the main filter but after
|
||||
// using its current state to compute the vertical position innovation
|
||||
if (measurement_valid && _range_sensor.isDataHealthy()) {
|
||||
bias_est.setMaxStateNoise(sqrtf(measurement_var));
|
||||
bias_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
|
||||
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
|
||||
}
|
||||
|
||||
// determine if we should use height aiding
|
||||
const bool do_conditional_range_aid = (_params.rng_ctrl == RngCtrl::CONDITIONAL) && isConditionalRangeAidSuitable();
|
||||
const bool continuing_conditions_passing = ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid)
|
||||
&& measurement_valid
|
||||
&& _range_sensor.isDataHealthy();
|
||||
|
||||
const bool continuing_conditions_passing = _range_sensor.isDataHealthy() && !rng_intermittent
|
||||
&& ((_params.rng_ctrl == RngCtrl::ENABLED) || do_conditional_range_aid);
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
&& isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)
|
||||
&& _range_sensor.isRegularlySendingData();
|
||||
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
if (continuing_conditions_passing) {
|
||||
fuseRngHgt(_aid_src_rng_hgt);
|
||||
aid_src.fusion_enabled = true;
|
||||
|
||||
const bool is_fusion_failing = isTimedOut(_aid_src_rng_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
|
||||
resetHeightToRng();
|
||||
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-(measurement - bias_est.getBias()));
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
|
||||
// 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);
|
||||
stopRngHgtFusion();
|
||||
_control_status.flags.rng_fault = true;
|
||||
_range_sensor.setFaulty();
|
||||
}
|
||||
|
||||
} else {
|
||||
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
startRngHgtFusion();
|
||||
if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl == RngCtrl::CONDITIONAL)) {
|
||||
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
|
||||
ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
|
||||
} else if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl != RngCtrl::CONDITIONAL)) {
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
resetVerticalPositionTo(-measurement, measurement_var);
|
||||
bias_est.reset();
|
||||
|
||||
} else {
|
||||
ECL_INFO("starting %s height fusion", HGT_SRC_NAME);
|
||||
bias_est.setBias(_state.pos(2) + measurement);
|
||||
}
|
||||
|
||||
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
|
||||
bias_est.setFusionActive();
|
||||
_control_status.flags.rng_hgt = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.rng_hgt && rng_intermittent) {
|
||||
} else if (_control_status.flags.rng_hgt
|
||||
&& !isNewestSampleRecent(_time_last_range_buffer_push, 2 * RNG_MAX_INTERVAL)) {
|
||||
// No data anymore. Stop until it comes back.
|
||||
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
|
||||
stopRngHgtFusion();
|
||||
}
|
||||
}
|
||||
|
||||
bool Ekf::isConditionalRangeAidSuitable()
|
||||
{
|
||||
bool is_range_aid_suitable = false;
|
||||
|
||||
if (_control_status.flags.in_air
|
||||
&& _range_sensor.isHealthy()
|
||||
&& isTerrainEstimateValid()) {
|
||||
// check if we can use range finder measurements to estimate height, use hysteresis to avoid rapid switching
|
||||
// Note that the 0.7 coefficients and the innovation check are arbitrary values but work well in practice
|
||||
const float range_hagl = _terrain_vpos - _state.pos(2);
|
||||
const float range_hagl_max = _control_status.flags.rng_hgt ? _params.max_hagl_for_range_aid : (_params.max_hagl_for_range_aid * 0.7f);
|
||||
const bool is_in_range = range_hagl < range_hagl_max;
|
||||
float range_hagl_max = _params.max_hagl_for_range_aid;
|
||||
float max_vel_xy = _params.max_vel_for_range_aid;
|
||||
|
||||
const float hagl_test_ratio = (_hagl_innov * _hagl_innov / (sq(_params.range_aid_innov_gate) * _hagl_innov_var));
|
||||
const bool is_hagl_stable = _control_status.flags.rng_hgt ? (hagl_test_ratio < 1.f) : (hagl_test_ratio < 0.01f);
|
||||
|
||||
bool is_hagl_stable = (hagl_test_ratio < 1.f);
|
||||
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
range_hagl_max = 0.7f * _params.max_hagl_for_range_aid;
|
||||
max_vel_xy = 0.7f * _params.max_vel_for_range_aid;
|
||||
is_hagl_stable = (hagl_test_ratio < 0.01f);
|
||||
}
|
||||
|
||||
const float range_hagl = _terrain_vpos - _state.pos(2);
|
||||
|
||||
const bool is_in_range = (range_hagl < range_hagl_max);
|
||||
|
||||
bool is_below_max_speed = true;
|
||||
|
||||
if (isHorizontalAidingActive()) {
|
||||
const float max_vel = _control_status.flags.rng_hgt ? _params.max_vel_for_range_aid : (_params.max_vel_for_range_aid * 0.7f);
|
||||
const bool is_below_max_speed = !_state.vel.xy().longerThan(max_vel);
|
||||
|
||||
is_range_aid_suitable = is_in_range && is_hagl_stable && is_below_max_speed;
|
||||
|
||||
} else {
|
||||
is_range_aid_suitable = is_in_range && is_hagl_stable;
|
||||
is_below_max_speed = !_state.vel.xy().longerThan(max_vel_xy);
|
||||
}
|
||||
|
||||
return is_in_range && is_hagl_stable && is_below_max_speed;
|
||||
}
|
||||
|
||||
return is_range_aid_suitable;
|
||||
}
|
||||
|
||||
void Ekf::startRngHgtFusion()
|
||||
{
|
||||
if (!_control_status.flags.rng_hgt) {
|
||||
if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl == RngCtrl::CONDITIONAL)) {
|
||||
// Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference.
|
||||
_rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom());
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
|
||||
} else if ((_params.height_sensor_ref == HeightSensor::RANGE) && (_params.rng_ctrl != RngCtrl::CONDITIONAL)) {
|
||||
// Range finder is the primary height source, the ground is now the datum used
|
||||
// to compute the local vertical position
|
||||
_rng_hgt_b_est.reset();
|
||||
_height_sensor_ref = HeightSensor::RANGE;
|
||||
resetHeightToRng();
|
||||
|
||||
} else {
|
||||
_rng_hgt_b_est.setBias(_state.pos(2) + _range_sensor.getDistBottom());
|
||||
}
|
||||
|
||||
_control_status.flags.rng_hgt = true;
|
||||
_rng_hgt_b_est.setFusionActive();
|
||||
ECL_INFO("starting RNG height fusion");
|
||||
}
|
||||
}
|
||||
|
||||
void Ekf::resetHeightToRng()
|
||||
{
|
||||
ECL_INFO("reset height to RNG");
|
||||
_information_events.flags.reset_hgt_to_rng = true;
|
||||
|
||||
float dist_bottom;
|
||||
|
||||
if (_control_status.flags.in_air) {
|
||||
dist_bottom = _range_sensor.getDistBottom();
|
||||
|
||||
} else {
|
||||
// use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar)
|
||||
dist_bottom = _params.rng_gnd_clearance;
|
||||
}
|
||||
|
||||
// update the state and associated variance
|
||||
resetVerticalPositionTo(-(dist_bottom - _rng_hgt_b_est.getBias()));
|
||||
|
||||
// the state variance is the same as the observation
|
||||
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.range_noise));
|
||||
|
||||
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_gps_hgt_b_est.setBias(_gps_hgt_b_est.getBias() + _state_reset_status.posD_change);
|
||||
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
|
||||
return false;
|
||||
}
|
||||
|
||||
void Ekf::stopRngHgtFusion()
|
||||
{
|
||||
if (_control_status.flags.rng_hgt) {
|
||||
|
||||
if (_height_sensor_ref == HeightSensor::RANGE) {
|
||||
_height_sensor_ref = HeightSensor::UNKNOWN;
|
||||
}
|
||||
|
||||
_control_status.flags.rng_hgt = false;
|
||||
_rng_hgt_b_est.setFusionInactive();
|
||||
ECL_INFO("stopping range height fusion");
|
||||
resetEstimatorAidStatus(_aid_src_rng_hgt);
|
||||
|
||||
_control_status.flags.rng_hgt = false;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user