fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-24 05:43:34 +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/CMakeLists.txt
-1
View File
@@ -51,7 +51,6 @@ add_library(ecl_EKF
gps_fusion.cpp
gps_yaw_fusion.cpp
height_control.cpp
height_fusion.cpp
imu_down_sampler.cpp
mag_control.cpp
mag_fusion.cpp
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;
}
}
src/modules/ekf2/EKF/bias_estimator.cpp
+1 -1
View File
@@ -69,7 +69,7 @@ void BiasEstimator::constrainStateVar()
void BiasEstimator::fuseBias(const float measurement, const float measurement_var)
{
const float innov_var = _state_var + measurement_var;
const float innov_var = _state_var + math::max(sq(0.01f), measurement_var);
const float innov = measurement - _state;
const float K = _state_var / innov_var;
const float innov_test_ratio = computeInnovTestRatio(innov, innov_var);
src/modules/ekf2/EKF/bias_estimator.hpp
+1 -1
View File
@@ -87,7 +87,7 @@ public:
void setBiasStdDev(float state_noise) { _state_var = state_noise * state_noise; }
void setInnovGate(float gate_size) { _gate_size = gate_size; }
void setMaxStateNoise(float max_noise) { _state_var_max = max_noise * max_noise; }
void setMaxStateNoise(float max_noise) { _state_var_max = math::max(sq(0.01f), max_noise * max_noise); }
float getBias() const { return _state; }
float getBiasVar() const { return _state_var; }
src/modules/ekf2/EKF/control.cpp
+16 -1
View File
@@ -129,8 +129,23 @@ void Ekf::controlFusionModes()
// Run the kinematic consistency check when not moving horizontally
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing
&& (sq(_state.vel(0)) + sq(_state.vel(1)) < fmaxf(P(4, 4) + P(5, 5), 0.1f))) {
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
_rng_consistency_check.setGate(_params.range_kin_consistency_gate);
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us);
_rng_consistency_check.update(_range_sensor.getDistBottom(), math::max(var, 0.001f), _state.vel(2), P(6, 6), _imu_sample_delayed.time_us);
}
} else {
// 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.isRegularlySendingData()
&& _range_sensor.isDataReady()) {
_range_sensor.setRange(_params.rng_gnd_clearance);
_range_sensor.setValidity(true); // bypass the checks
}
}
src/modules/ekf2/EKF/ekf.h
+3 -32
View File
@@ -693,19 +693,12 @@ private:
// fuse body frame drag specific forces for multi-rotor wind estimation
void fuseDrag(const dragSample &drag_sample);
void fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt);
void fuseRngHgt(estimator_aid_source_1d_s &range_hgt);
void fuseEvHgt();
void updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt);
void updateRngHgt(estimator_aid_source_1d_s &rng_hgt);
// fuse single velocity and position measurement
bool fuseVelPosHeight(const float innov, const float innov_var, const int obs_index);
void resetVelocityTo(const Vector3f &vel);
void resetHorizontalVelocityTo(const Vector2f &new_horz_vel);
void resetVerticalVelocityTo(float new_vert_vel);
void resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var = NAN);
void resetVelocityToGps(const gpsSample &gps_sample);
void resetHorizontalVelocityToOpticalFlow(const flowSample &flow_sample);
@@ -720,15 +713,8 @@ private:
bool isHeightResetRequired() const;
void resetVerticalPositionTo(float new_vert_pos);
void resetVerticalPositionTo(float new_vert_pos, float new_vert_pos_var = NAN);
void resetHeightToBaro(const baroSample &baro_sample);
void resetHeightToGps(const gpsSample &gps_sample);
void resetHeightToRng();
void resetHeightToEv();
void resetVerticalVelocityToGps(const gpsSample &gps_sample);
void resetVerticalVelocityToEv(const extVisionSample &ev_sample);
void resetVerticalVelocityToZero();
// fuse optical flow line of sight rate measurements
@@ -942,11 +928,9 @@ private:
void controlFakePosFusion();
void controlFakeHgtFusion();
void startFakeHgtFusion();
void resetFakeHgtFusion();
void resetHeightToLastKnown();
void stopFakeHgtFusion();
void fuseFakeHgt();
void controlZeroVelocityUpdate();
@@ -964,7 +948,7 @@ private:
void controlBaroHeightFusion();
void controlGnssHeightFusion(const gpsSample &gps_sample);
void controlRangeHeightFusion();
void controlEvHeightFusion();
void controlEvHeightFusion(const extVisionSample &ev_sample);
bool isConditionalRangeAidSuitable();
@@ -974,24 +958,13 @@ private:
void startMagHdgFusion();
void startMag3DFusion();
void startBaroHgtFusion(const baroSample &baro_sample);
void stopBaroHgtFusion();
void startGpsHgtFusion(const gpsSample &gps_sample);
void stopGpsHgtFusion();
void startRngHgtFusion();
void stopRngHgtFusion();
void startRngAidHgtFusion();
void startEvHgtFusion();
void stopEvHgtFusion();
void updateGroundEffect();
// return an estimation of the sensor altitude variance
float getGpsHeightVariance(const gpsSample &gps_sample);
float getRngHeightVariance() const;
// calculate the measurement variance for the optical flow sensor
float calcOptFlowMeasVar(const flowSample &flow_sample);
@@ -1078,10 +1051,8 @@ private:
void stopFlowFusion();
void startFakePosFusion();
void resetFakePosFusion();
void stopFakePosFusion();
void fuseFakePosition();
void setVelPosStatus(const int index, const bool healthy);
src/modules/ekf2/EKF/ekf_helper.cpp
+20 -42
View File
@@ -124,11 +124,15 @@ void Ekf::resetHorizontalVelocityTo(const Vector2f &new_horz_vel)
_time_last_hor_vel_fuse = _imu_sample_delayed.time_us;
}
void Ekf::resetVerticalVelocityTo(float new_vert_vel)
void Ekf::resetVerticalVelocityTo(float new_vert_vel, float new_vert_vel_var)
{
const float delta_vert_vel = new_vert_vel - _state.vel(2);
_state.vel(2) = new_vert_vel;
if (PX4_ISFINITE(new_vert_vel_var)) {
P.uncorrelateCovarianceSetVariance<1>(6, math::max(sq(0.01f), new_vert_vel_var));
}
for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {
_output_buffer[index].vel(2) += delta_vert_vel;
_output_vert_buffer[index].vert_vel += delta_vert_vel;
@@ -226,11 +230,16 @@ bool Ekf::isHeightResetRequired() const
}
void Ekf::resetVerticalPositionTo(const float new_vert_pos)
void Ekf::resetVerticalPositionTo(const float new_vert_pos, float new_vert_pos_var)
{
const float old_vert_pos = _state.pos(2);
_state.pos(2) = new_vert_pos;
if (PX4_ISFINITE(new_vert_pos_var)) {
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, math::max(sq(0.01f), new_vert_pos_var));
}
// store the reset amount and time to be published
_state_reset_status.posD_change = new_vert_pos - old_vert_pos;
_state_reset_status.posD_counter++;
@@ -248,34 +257,21 @@ void Ekf::resetVerticalPositionTo(const float new_vert_pos)
// add the reset amount to the output observer vertical position state
_output_vert_new.vert_vel_integ = _state.pos(2);
_baro_b_est.setBias(_baro_b_est.getBias() + _state_reset_status.posD_change);
_ev_hgt_b_est.setBias(_ev_hgt_b_est.getBias() - _state_reset_status.posD_change);
_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);
// Reset the timout timer
_time_last_hgt_fuse = _imu_sample_delayed.time_us;
}
void Ekf::resetVerticalVelocityToGps(const gpsSample &gps_sample)
{
resetVerticalVelocityTo(gps_sample.vel(2));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(6, sq(1.5f * gps_sample.sacc));
}
void Ekf::resetVerticalVelocityToEv(const extVisionSample &ev_sample)
{
resetVerticalVelocityTo(ev_sample.vel(2));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(6, ev_sample.velVar(2));
}
void Ekf::resetVerticalVelocityToZero()
{
// we don't know what the vertical velocity is, so set it to zero
resetVerticalVelocityTo(0.0f);
// Set the variance to a value large enough to allow the state to converge quickly
// that does not destabilise the filter
P.uncorrelateCovarianceSetVariance<1>(6, 10.0f);
resetVerticalVelocityTo(0.0f, 10.f);
}
// align output filter states to match EKF states at the fusion time horizon
@@ -700,11 +696,11 @@ bool Ekf::setEkfGlobalOrigin(const double latitude, const double longitude, cons
// determine current z
float current_alt = -_state.pos(2) + gps_alt_ref_prev;
const float gps_hgt_bias = _gps_hgt_b_est.getBias();
resetVerticalPositionTo(_gps_alt_ref - current_alt);
_baro_b_est.setBias(_baro_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);
// preserve GPS height bias
_gps_hgt_b_est.setBias(gps_hgt_bias);
}
return true;
@@ -1299,24 +1295,6 @@ void Ekf::startMag3DFusion()
}
}
float Ekf::getGpsHeightVariance(const gpsSample &gps_sample)
{
// observation variance - receiver defined and parameter limited
// use 1.5 as a typical ratio of vacc/hacc
const float lower_limit = fmaxf(1.5f * _params.gps_pos_noise, 0.01f);
const float upper_limit = fmaxf(1.5f * _params.pos_noaid_noise, lower_limit);
const float gps_alt_var = sq(math::constrain(gps_sample.vacc, lower_limit, upper_limit));
return gps_alt_var;
}
float Ekf::getRngHeightVariance() const
{
const float dist_dependant_var = sq(_params.range_noise_scaler * _range_sensor.getDistBottom());
const float var = sq(_params.range_noise) + dist_dependant_var;
const float var_sat = fmaxf(var, 0.001f);
return var_sat;
}
void Ekf::updateGroundEffect()
{
if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) {
src/modules/ekf2/EKF/ev_height_control.cpp
+96 -54
View File
@@ -38,95 +38,137 @@
#include "ekf.h"
void Ekf::controlEvHeightFusion()
void Ekf::controlEvHeightFusion(const extVisionSample &ev_sample)
{
if (!(_params.height_sensor_ref == HeightSensor::EV)) { // TODO: replace by EV control parameter
stopEvHgtFusion();
return;
}
static constexpr const char *HGT_SRC_NAME = "EV";
_ev_hgt_b_est.predict(_dt_ekf_avg);
auto &aid_src = _aid_src_ev_hgt;
HeightBiasEstimator &bias_est = _ev_hgt_b_est;
const bool ev_intermittent = !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
bias_est.predict(_dt_ekf_avg);
if (_ev_data_ready) {
const bool position_valid = PX4_ISFINITE(_ev_sample_delayed.pos(2));
const bool continuing_conditions_passing = !ev_intermittent && position_valid;
const bool starting_conditions_passing = continuing_conditions_passing;
const float measurement = ev_sample.pos(2);
const float measurement_var = ev_sample.posVar(2);
const float innov_gate = math::max(_params.ev_pos_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
updateVerticalPositionAidSrcStatus(ev_sample.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) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
bias_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
}
const bool continuing_conditions_passing = ((_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS) || (_params.height_sensor_ref == HeightSensor::EV)) // TODO: (_params.ev_ctrl & EvCtrl::VPOS)
&& measurement_valid;
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL);
if (_control_status.flags.ev_hgt) {
aid_src.fusion_enabled = true;
if (continuing_conditions_passing) {
fuseEvHgt();
const bool reset = (_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter);
if (isHeightResetRequired() || reset ) {
resetHeightToEv();
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
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement - bias_est.getBias(), measurement_var);
bias_est.setBias(-_state.pos(2) + measurement);
// reset vertical velocity
if (PX4_ISFINITE(ev_sample.vel(2)) && (_params.fusion_mode & SensorFusionMask::USE_EXT_VIS_POS)) {
resetVerticalVelocityTo(ev_sample.vel(2), ev_sample.velVar(2));
// If the sample has a valid vertical velocity estimate, use it
if (PX4_ISFINITE(_ev_sample_delayed.vel(2))) {
resetVerticalVelocityToEv(_ev_sample_delayed);
} else {
resetVerticalVelocityToZero();
}
aid_src.time_last_fuse = _imu_sample_delayed.time_us;
} else if ((_ev_sample_delayed.reset_counter != _ev_sample_delayed_prev.reset_counter) && !aid_src.fused) {
// fusion failed and EV sample indicates reset
ECL_INFO("%s height reset", HGT_SRC_NAME);
if (_height_sensor_ref == HeightSensor::EV) {
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(measurement, measurement_var);
bias_est.reset();
} else {
bias_est.setBias(-_state.pos(2) + measurement);
}
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);
stopEvHgtFusion();
}
} else {
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
stopEvHgtFusion();
}
} else {
if (starting_conditions_passing) {
startEvHgtFusion();
if (_params.height_sensor_ref == HeightSensor::EV) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::EV;
_information_events.flags.reset_hgt_to_ev = 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.ev_hgt = true;
}
}
} else if (_control_status.flags.ev_hgt && ev_intermittent) {
} else if (_control_status.flags.ev_hgt
&& !isNewestSampleRecent(_time_last_ext_vision_buffer_push, 2 * EV_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopEvHgtFusion();
}
}
void Ekf::startEvHgtFusion()
{
if (!_control_status.flags.ev_hgt) {
if (_params.height_sensor_ref == HeightSensor::EV) {
_ev_hgt_b_est.reset();
_height_sensor_ref = HeightSensor::EV;
resetHeightToEv();
} else {
_ev_hgt_b_est.setBias(-_state.pos(2) + _ev_sample_delayed.pos(2));
}
_control_status.flags.ev_hgt = true;
_ev_hgt_b_est.setFusionActive();
ECL_INFO("starting EV height fusion");
}
}
void Ekf::resetHeightToEv()
{
ECL_INFO("reset height to EV");
_information_events.flags.reset_hgt_to_ev = true;
resetVerticalPositionTo(_ev_sample_delayed.pos(2) - _ev_hgt_b_est.getBias());
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f)));
_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);
_rng_hgt_b_est.setBias(_rng_hgt_b_est.getBias() + _state_reset_status.posD_change);
}
void Ekf::stopEvHgtFusion()
{
if (_control_status.flags.ev_hgt) {
if (_height_sensor_ref == HeightSensor::EV) {
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.ev_hgt = false;
_ev_hgt_b_est.setFusionInactive();
ECL_INFO("stopping EV height fusion");
resetEstimatorAidStatus(_aid_src_ev_hgt);
_control_status.flags.ev_hgt = false;
}
}
src/modules/ekf2/EKF/fake_height_control.cpp
+24 -50
View File
@@ -40,25 +40,34 @@
void Ekf::controlFakeHgtFusion()
{
auto &fake_hgt = _aid_src_fake_hgt;
// clear
resetEstimatorAidStatus(fake_hgt);
auto &aid_src = _aid_src_fake_hgt;
// If we aren't doing any aiding, fake position measurements at the last known vertical position to constrain drift
const bool fake_hgt_data_ready = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)2e5); // Fuse fake height at a limited rate
const bool fake_hgt_data_ready = !isVerticalAidingActive()
&& isTimedOut(aid_src.time_last_fuse, (uint64_t)2e5); // Fuse fake height at a limited rate
if (fake_hgt_data_ready) {
const float obs_var = sq(_params.pos_noaid_noise);
const float innov_gate = 3.f;
updateVerticalPositionAidSrcStatus(_imu_sample_delayed.time_us, _last_known_pos(2), obs_var, innov_gate, aid_src);
const bool continuing_conditions_passing = !isVerticalAidingActive();
const bool starting_conditions_passing = continuing_conditions_passing
&& _vertical_velocity_deadreckon_time_exceeded
&& _vertical_position_deadreckon_time_exceeded;
&& _vertical_velocity_deadreckon_time_exceeded
&& _vertical_position_deadreckon_time_exceeded;
if (_control_status.flags.fake_hgt) {
if (continuing_conditions_passing) {
fuseFakeHgt();
const bool is_fusion_failing = isTimedOut(fake_hgt.time_last_fuse, (uint64_t)4e5);
// always protect against extreme values that could result in a NaN
aid_src.fusion_enabled = aid_src.test_ratio < sq(100.0f / innov_gate);
fuseVerticalPosition(aid_src);
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
if (is_fusion_failing) {
resetFakeHgtFusion();
@@ -70,18 +79,14 @@ void Ekf::controlFakeHgtFusion()
} else {
if (starting_conditions_passing) {
startFakeHgtFusion();
ECL_INFO("start fake height fusion");
_control_status.flags.fake_hgt = true;
resetFakeHgtFusion();
}
}
}
}
void Ekf::startFakeHgtFusion()
{
if (!_control_status.flags.fake_hgt) {
ECL_INFO("start fake height fusion");
_control_status.flags.fake_hgt = true;
resetFakeHgtFusion();
} else if (_control_status.flags.fake_hgt && isVerticalAidingActive()) {
stopFakeHgtFusion();
}
}
@@ -100,8 +105,7 @@ void Ekf::resetHeightToLastKnown()
{
_information_events.flags.reset_pos_to_last_known = true;
ECL_INFO("reset height to last known");
resetVerticalPositionTo(_last_known_pos(2));
P.uncorrelateCovarianceSetVariance<1>(9, sq(_params.pos_noaid_noise));
resetVerticalPositionTo(_last_known_pos(2), sq(_params.pos_noaid_noise));
}
void Ekf::stopFakeHgtFusion()
@@ -113,33 +117,3 @@ void Ekf::stopFakeHgtFusion()
resetEstimatorAidStatus(_aid_src_fake_hgt);
}
}
void Ekf::fuseFakeHgt()
{
const float obs_var = sq(_params.pos_noaid_noise);
const float innov_gate = 3.f;
auto &fake_hgt = _aid_src_fake_hgt;
fake_hgt.observation = _last_known_pos(2);
fake_hgt.observation_variance = obs_var;
fake_hgt.innovation = _state.pos(2) - _last_known_pos(2);
fake_hgt.innovation_variance = P(9, 9) + obs_var;
setEstimatorAidStatusTestRatio(fake_hgt, innov_gate);
// always protect against extreme values that could result in a NaN
fake_hgt.fusion_enabled = fake_hgt.test_ratio < sq(100.0f / innov_gate);
// fuse
if (fake_hgt.fusion_enabled && !fake_hgt.innovation_rejected) {
if (fuseVelPosHeight(fake_hgt.innovation, fake_hgt.innovation_variance, 5)) {
fake_hgt.fused = true;
fake_hgt.time_last_fuse = _imu_sample_delayed.time_us;
}
}
fake_hgt.timestamp_sample = _imu_sample_delayed.time_us;
}
src/modules/ekf2/EKF/fake_pos_control.cpp
+35 -74
View File
@@ -40,25 +40,48 @@
void Ekf::controlFakePosFusion()
{
auto &fake_pos = _aid_src_fake_pos;
// clear
resetEstimatorAidStatus(fake_pos);
auto &aid_src = _aid_src_fake_pos;
// If we aren't doing any aiding, fake position measurements at the last known position to constrain drift
// During intial tilt aligment, fake position is used to perform a "quasi-stationary" leveling of the EKF
const bool fake_pos_data_ready = isTimedOut(fake_pos.time_last_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
const bool fake_pos_data_ready = !isHorizontalAidingActive()
&& isTimedOut(aid_src.time_last_fuse, (uint64_t)2e5); // Fuse fake position at a limited rate
if (fake_pos_data_ready) {
Vector2f obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more
// aggressively when the vehicle is at rest
obs_var(0) = obs_var(1) = sq(0.01f);
} else {
obs_var(0) = obs_var(1) = sq(0.5f);
}
const float innov_gate = 3.f;
updateHorizontalPositionAidSrcStatus(_imu_sample_delayed.time_us, Vector2f(_last_known_pos), obs_var, innov_gate, aid_src);
const bool continuing_conditions_passing = !isHorizontalAidingActive();
const bool starting_conditions_passing = continuing_conditions_passing
&& _horizontal_deadreckon_time_exceeded;
&& _horizontal_deadreckon_time_exceeded;
if (_control_status.flags.fake_pos) {
if (continuing_conditions_passing) {
fuseFakePosition();
const bool is_fusion_failing = isTimedOut(fake_pos.time_last_fuse, (uint64_t)4e5);
// always protect against extreme values that could result in a NaN
aid_src.fusion_enabled = (aid_src.test_ratio[0] < sq(100.0f / innov_gate))
&& (aid_src.test_ratio[1] < sq(100.0f / innov_gate));
fuseHorizontalPosition(aid_src);
const bool is_fusion_failing = isTimedOut(aid_src.time_last_fuse, (uint64_t)4e5);
if (is_fusion_failing) {
resetFakePosFusion();
@@ -70,7 +93,10 @@ void Ekf::controlFakePosFusion()
} else {
if (starting_conditions_passing) {
startFakePosFusion();
ECL_INFO("start fake position fusion");
_control_status.flags.fake_pos = true;
_fuse_hpos_as_odom = false; // TODO: needed?
resetFakePosFusion();
if (_control_status.flags.tilt_align) {
// The fake position fusion is not started for initial alignement
@@ -85,16 +111,6 @@ void Ekf::controlFakePosFusion()
}
}
void Ekf::startFakePosFusion()
{
if (!_control_status.flags.fake_pos) {
ECL_INFO("start fake position fusion");
_control_status.flags.fake_pos = true;
_fuse_hpos_as_odom = false; // TODO: needed?
resetFakePosFusion();
}
}
void Ekf::resetFakePosFusion()
{
ECL_INFO("reset fake position fusion");
@@ -115,58 +131,3 @@ void Ekf::stopFakePosFusion()
resetEstimatorAidStatus(_aid_src_fake_pos);
}
}
void Ekf::fuseFakePosition()
{
Vector2f obs_var;
if (_control_status.flags.in_air && _control_status.flags.tilt_align) {
obs_var(0) = obs_var(1) = sq(fmaxf(_params.pos_noaid_noise, _params.gps_pos_noise));
} else if (!_control_status.flags.in_air && _control_status.flags.vehicle_at_rest) {
// Accelerate tilt fine alignment by fusing more
// aggressively when the vehicle is at rest
obs_var(0) = obs_var(1) = sq(0.01f);
} else {
obs_var(0) = obs_var(1) = sq(0.5f);
}
const float innov_gate = 3.f;
auto &fake_pos = _aid_src_fake_pos;
for (int i = 0; i < 2; i++) {
fake_pos.observation[i] = _last_known_pos(i);
fake_pos.observation_variance[i] = obs_var(i);
fake_pos.innovation[i] = _state.pos(i) - _last_known_pos(i);
fake_pos.innovation_variance[i] = P(7 + i, 7 + i) + obs_var(i);
}
setEstimatorAidStatusTestRatio(fake_pos, innov_gate);
fake_pos.fusion_enabled = true;
// always protect against extreme values that could result in a NaN
if (!fake_pos.innovation_rejected) {
if ((fake_pos.test_ratio[0] > sq(100.0f / innov_gate)) || (fake_pos.test_ratio[1] > sq(100.0f / innov_gate))) {
fake_pos.innovation_rejected = true;
}
}
// fuse
if (fake_pos.fusion_enabled && !fake_pos.innovation_rejected) {
if (fuseVelPosHeight(fake_pos.innovation[0], fake_pos.innovation_variance[0], 3)
&& fuseVelPosHeight(fake_pos.innovation[1], fake_pos.innovation_variance[1], 4)
) {
fake_pos.fused = true;
fake_pos.time_last_fuse = _imu_sample_delayed.time_us;
} else {
fake_pos.fused = false;
}
}
fake_pos.timestamp_sample = _imu_sample_delayed.time_us;
}
src/modules/ekf2/EKF/gnss_height_control.cpp
+65 -62
View File
@@ -40,10 +40,7 @@
void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
{
if (!(_params.gnss_ctrl & GnssCtrl::VPOS)) {
stopGpsHgtFusion();
return;
}
static constexpr const char *HGT_SRC_NAME = "GNSS";
auto &aid_src = _aid_src_gnss_hgt;
HeightBiasEstimator &bias_est = _gps_hgt_b_est;
@@ -52,13 +49,23 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
if (_gps_data_ready) {
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float noise = math::max(gps_sample.vacc, 1.5f * _params.gps_pos_noise); // use 1.5 as a typical ratio of vacc/hacc
const float innov_gate = fmaxf(_params.gps_pos_innov_gate, 1.f);
if (!isOnlyActiveSourceOfVerticalPositionAiding(_control_status.flags.gps_hgt)) {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
if (noise > _params.pos_noaid_noise) {
noise = _params.pos_noaid_noise;
}
}
const float measurement = gps_sample.hgt - getEkfGlobalOriginAltitude();
const float measurement_var = getGpsHeightVariance(gps_sample);
const float measurement_var = sq(noise);
const float innov_gate = math::max(_params.gps_pos_innov_gate, 1.f);
const bool measurement_valid = PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var);
// GNSS position, vertical position GNSS measurement has opposite sign to earth z axis
updateVerticalPositionAidSrcStatus(gps_sample.time_us,
@@ -67,28 +74,28 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
innov_gate,
aid_src);
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
if (gps_checks_passing && !gps_checks_failing
&& PX4_ISFINITE(measurement) && PX4_ISFINITE(measurement_var)
) {
const float noise = sqrtf(measurement_var);
bias_est.setMaxStateNoise(noise);
if (measurement_valid && gps_checks_passing && !gps_checks_failing) {
bias_est.setMaxStateNoise(sqrtf(measurement_var));
bias_est.setProcessNoiseSpectralDensity(_params.gps_hgt_bias_nsd);
bias_est.fuseBias(measurement - (-_state.pos(2)), measurement_var + P(9, 9));
}
// determine if we should use GNSS height aiding
// determine if we should use height aiding
const bool continuing_conditions_passing = (_params.gnss_ctrl & GnssCtrl::VPOS)
&& PX4_ISFINITE(gps_sample.hgt)
&& measurement_valid
&& _NED_origin_initialised
&& _gps_checks_passed;
const bool starting_conditions_passing = continuing_conditions_passing
&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)
&& _gps_checks_passed
&& gps_checks_passing
&& !gps_checks_failing
&& isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL);
&& !gps_checks_failing;
if (_control_status.flags.gps_hgt) {
aid_src.fusion_enabled = true;
@@ -101,68 +108,63 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
if (isHeightResetRequired()) {
// All height sources are failing
resetHeightToGps(gps_sample);
resetVerticalVelocityToGps(gps_sample);
ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME);
_information_events.flags.reset_hgt_to_gps = true;
resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
bias_est.setBias(_state.pos(2) + measurement);
// reset vertical velocity
if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) {
// use 1.5 as a typical ratio of vacc/hacc
resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc));
} else {
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);
stopGpsHgtFusion();
}
} else {
ECL_WARN("stopping %s height fusion, continuing conditions failing", HGT_SRC_NAME);
stopGpsHgtFusion();
}
} else {
if (starting_conditions_passing) {
startGpsHgtFusion(gps_sample);
if (_params.height_sensor_ref == HeightSensor::GNSS) {
ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME);
_height_sensor_ref = HeightSensor::GNSS;
_information_events.flags.reset_hgt_to_gps = 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.gps_hgt = true;
}
}
} else if (_control_status.flags.gps_hgt && _gps_intermittent) {
} else if (_control_status.flags.gps_hgt
&& !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL)) {
// No data anymore. Stop until it comes back.
ECL_WARN("stopping %s height fusion, no data", HGT_SRC_NAME);
stopGpsHgtFusion();
}
}
void Ekf::startGpsHgtFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps_hgt) {
if (_params.height_sensor_ref == HeightSensor::GNSS) {
_gps_hgt_b_est.reset();
_height_sensor_ref = HeightSensor::GNSS;
resetHeightToGps(gps_sample);
} else {
_gps_hgt_b_est.setBias(_state.pos(2) + (gps_sample.hgt - getEkfGlobalOriginAltitude()));
// Reset the timeout value here because the fusion isn't done at the same place and would immediately trigger a timeout
_aid_src_gnss_hgt.time_last_fuse = _imu_sample_delayed.time_us;
}
_control_status.flags.gps_hgt = true;
_gps_hgt_b_est.setFusionActive();
ECL_INFO("starting GPS height fusion");
}
}
void Ekf::resetHeightToGps(const gpsSample &gps_sample)
{
ECL_INFO("reset height to GPS");
_information_events.flags.reset_hgt_to_gps = true;
resetVerticalPositionTo(-(gps_sample.hgt - getEkfGlobalOriginAltitude() - _gps_hgt_b_est.getBias()));
// the state variance is the same as the observation
P.uncorrelateCovarianceSetVariance<1>(9, getGpsHeightVariance(gps_sample));
_baro_b_est.setBias(_baro_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);
_aid_src_gnss_hgt.time_last_fuse = _imu_sample_delayed.time_us;
}
void Ekf::stopGpsHgtFusion()
{
if (_control_status.flags.gps_hgt) {
@@ -171,8 +173,9 @@ void Ekf::stopGpsHgtFusion()
_height_sensor_ref = HeightSensor::UNKNOWN;
}
_control_status.flags.gps_hgt = false;
_gps_hgt_b_est.setFusionInactive();
ECL_INFO("stopping GPS height fusion");
resetEstimatorAidStatus(_aid_src_gnss_hgt);
_control_status.flags.gps_hgt = false;
}
}
src/modules/ekf2/EKF/gps_control.cpp
+18 -10
View File
@@ -59,29 +59,37 @@ void Ekf::controlGpsFusion()
controlGpsYawFusion(gps_sample, gps_checks_passing, gps_checks_failing);
// GNSS velocity
const Vector3f velocity{gps_sample.vel};
const float vel_var = sq(gps_sample.sacc);
const Vector3f vel_obs_var{vel_var, vel_var, vel_var * sq(1.5f)};
updateVelocityAidSrcStatus(gps_sample.time_us, velocity, vel_obs_var, fmaxf(_params.gps_vel_innov_gate, 1.f), _aid_src_gnss_vel);
const float vel_obs_var = sq(math::max(gps_sample.sacc, _params.gps_vel_noise));
updateVelocityAidSrcStatus(gps_sample.time_us,
gps_sample.vel, // observation
Vector3f(vel_obs_var, vel_obs_var, vel_obs_var * sq(1.5f)), // observation variance
math::max(_params.gps_vel_innov_gate, 1.f), // innovation gate
_aid_src_gnss_vel);
_aid_src_gnss_vel.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::VEL);
// GNSS position
const float pos_var_lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
float pos_var = sq(fmaxf(gps_sample.hacc, pos_var_lower_limit));
// relax the upper observation noise limit which prevents bad GPS perturbing the position estimate
float pos_noise = math::max(gps_sample.hacc, _params.gps_pos_noise);
if (!isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
// if we are not using another source of aiding, then we are reliant on the GPS
// observations to constrain attitude errors and must limit the observation noise value.
float upper_limit = fmaxf(_params.pos_noaid_noise, pos_var_lower_limit);
pos_var = fminf(pos_var, upper_limit);
if (pos_noise > _params.pos_noaid_noise) {
pos_noise = _params.pos_noaid_noise;
}
}
updateHorizontalPositionAidSrcStatus(gps_sample.time_us, gps_sample.pos, Vector2f(pos_var, pos_var), fmaxf(_params.gps_pos_innov_gate, 1.f), _aid_src_gnss_pos);
const float pos_obs_var = sq(pos_noise);
updateHorizontalPositionAidSrcStatus(gps_sample.time_us,
gps_sample.pos, // observation
Vector2f(pos_obs_var, pos_obs_var), // observation variance
math::max(_params.gps_pos_innov_gate, 1.f), // innovation gate
_aid_src_gnss_pos);
_aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS);
// update GSF yaw estimator velocity (basic sanity check on GNSS velocity data)
if (gps_checks_passing && !gps_checks_failing) {
_yawEstimator.setVelocity(velocity.xy(), gps_sample.sacc);
_yawEstimator.setVelocity(gps_sample.vel.xy(), gps_sample.sacc);
}
// Determine if we should use GPS aiding for velocity and horizontal position
src/modules/ekf2/EKF/height_control.cpp
+3 -3
View File
@@ -46,7 +46,7 @@ void Ekf::controlHeightFusion()
controlBaroHeightFusion();
controlGnssHeightFusion(_gps_sample_delayed);
controlRangeHeightFusion();
controlEvHeightFusion();
controlEvHeightFusion(_ev_sample_delayed);
checkHeightSensorRefFallback();
}
@@ -211,14 +211,14 @@ Likelihood Ekf::estimateInertialNavFallingLikelihood() const
// Check all the sources agains each other
for (unsigned i = 0; i < 6; i++) {
if (checks[i].failed_lim) {
// There is a chance that the interial nav is falling if one source is failing the test
// There is a chance that the inertial nav is falling if one source is failing the test
likelihood_medium = true;
}
for (unsigned j = 0; j < 6; j++) {
if ((checks[i].ref_type != checks[j].ref_type) && checks[i].failed_lim && checks[j].failed_min) {
// There is a high chance that the interial nav is falling if two sources are failing the test
// There is a high chance that the inertial nav is failing if two sources are failing the test
likelihood_high = true;
}
}
src/modules/ekf2/EKF/height_fusion.cpp
-186
View File
@@ -1,186 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2021 PX4. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file height_fusion.cpp
* Function for fusing height (range, baro, GNSS alt, ...) measurements
*/
#include "ekf.h"
void Ekf::updateBaroHgt(const baroSample &baro_sample, estimator_aid_source_1d_s &baro_hgt)
{
// reset flags
resetEstimatorAidStatus(baro_hgt);
// innovation gate size
const float innov_gate = fmaxf(_params.baro_innov_gate, 1.f);
// measurement variance - user parameter defined
const float measurement_var = sq(fmaxf(_params.baro_noise, 0.01f));
const float measurement = baro_sample.hgt;
// vertical position innovation - baro measurement has opposite sign to earth z axis
baro_hgt.observation = -(measurement - _baro_b_est.getBias());
baro_hgt.observation_variance = measurement_var + _baro_b_est.getBiasVar();
baro_hgt.innovation = _state.pos(2) - baro_hgt.observation;
baro_hgt.innovation_variance = P(9, 9) + baro_hgt.observation_variance;
// 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 (baro_hgt.innovation < -deadzone_start) {
if (baro_hgt.innovation <= -deadzone_end) {
baro_hgt.innovation += deadzone_end;
} else {
baro_hgt.innovation = -deadzone_start;
}
}
}
setEstimatorAidStatusTestRatio(baro_hgt, innov_gate);
// special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && baro_hgt.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(baro_hgt.innovation_variance);
baro_hgt.innovation = math::constrain(baro_hgt.innovation, -innov_limit, innov_limit);
baro_hgt.innovation_rejected = false;
}
baro_hgt.fusion_enabled = _control_status.flags.baro_hgt;
baro_hgt.timestamp_sample = baro_sample.time_us;
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
_baro_b_est.setMaxStateNoise(_params.baro_noise);
_baro_b_est.setProcessNoiseSpectralDensity(_params.baro_bias_nsd);
_baro_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
}
void Ekf::fuseBaroHgt(estimator_aid_source_1d_s &baro_hgt)
{
if (baro_hgt.fusion_enabled
&& !baro_hgt.innovation_rejected
&& fuseVelPosHeight(baro_hgt.innovation, baro_hgt.innovation_variance, 5)) {
baro_hgt.fused = true;
baro_hgt.time_last_fuse = _imu_sample_delayed.time_us;
}
}
void Ekf::updateRngHgt(estimator_aid_source_1d_s &rng_hgt)
{
// reset flags
resetEstimatorAidStatus(rng_hgt);
// measurement variance - user parameter defined
const float measurement_var = fmaxf(sq(_params.range_noise) + sq(_params.range_noise_scaler * _range_sensor.getDistBottom()), 0.01f);
const float measurement = math::max(_range_sensor.getDistBottom(), _params.rng_gnd_clearance);
// innovation gate size
const float innov_gate = fmaxf(_params.range_innov_gate, 1.f);
// vertical position innovation, use range finder with tilt correction
rng_hgt.observation = -(measurement - _rng_hgt_b_est.getBias());
rng_hgt.observation_variance = measurement_var + _rng_hgt_b_est.getBiasVar();
rng_hgt.innovation = _state.pos(2) - rng_hgt.observation;
rng_hgt.innovation_variance = P(9, 9) + rng_hgt.observation_variance;
setEstimatorAidStatusTestRatio(rng_hgt, innov_gate);
// special case if there is bad vertical acceleration data, then don't reject measurement,
// but limit innovation to prevent spikes that could destabilise the filter
if (_fault_status.flags.bad_acc_vertical && rng_hgt.innovation_rejected) {
const float innov_limit = innov_gate * sqrtf(rng_hgt.innovation_variance);
rng_hgt.innovation = math::constrain(rng_hgt.innovation, -innov_limit, innov_limit);
rng_hgt.innovation_rejected = false;
}
rng_hgt.fusion_enabled = _control_status.flags.rng_hgt;
rng_hgt.timestamp_sample = _range_sensor.getSampleAddress()->time_us;
// update the bias estimator before updating the main filter but after
// using its current state to compute the vertical position innovation
const float rng_noise = sqrtf(measurement_var);
_rng_hgt_b_est.setMaxStateNoise(rng_noise);
_rng_hgt_b_est.setProcessNoiseSpectralDensity(_params.rng_hgt_bias_nsd);
_rng_hgt_b_est.fuseBias(measurement - (-_state.pos(2)) , measurement_var + P(9, 9));
}
void Ekf::fuseRngHgt(estimator_aid_source_1d_s &rng_hgt)
{
if (rng_hgt.fusion_enabled
&& !rng_hgt.innovation_rejected
&& fuseVelPosHeight(rng_hgt.innovation, rng_hgt.innovation_variance, 5)) {
rng_hgt.fused = true;
rng_hgt.time_last_fuse = _imu_sample_delayed.time_us;
}
}
void Ekf::fuseEvHgt()
{
const float measurement = _ev_sample_delayed.pos(2);
const float measurement_var = fmaxf(_ev_sample_delayed.posVar(2), sq(0.01f));
const float bias = _ev_hgt_b_est.getBias();
const float bias_var = _ev_hgt_b_est.getBiasVar();
_ev_hgt_b_est.setMaxStateNoise(sqrtf(measurement_var));
_ev_hgt_b_est.setProcessNoiseSpectralDensity(_params.ev_hgt_bias_nsd);
_ev_hgt_b_est.fuseBias(measurement - _state.pos(2), measurement_var + P(9, 9));
// calculate the innovation assuming the external vision observation is in local NED frame
const float obs = measurement - bias;
const float obs_var = measurement_var + bias_var;
// innovation gate size
float innov_gate = fmaxf(_params.ev_pos_innov_gate, 1.f);
updateVerticalPositionAidSrcStatus(_ev_sample_delayed.time_us, obs, obs_var, innov_gate, _aid_src_ev_hgt);
_aid_src_ev_hgt.fusion_enabled = _control_status.flags.ev_hgt;
fuseVerticalPosition(_aid_src_ev_hgt);
}
src/modules/ekf2/EKF/range_height_control.cpp
+100 -94
View File
@@ -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;
}
}
src/modules/ekf2/EKF/sensor_range_finder.hpp
+1 -1
View File
@@ -106,7 +106,7 @@ public:
float getValidMinVal() const { return _rng_valid_min_val; }
float getValidMaxVal() const { return _rng_valid_max_val; }
void setFaulty() { _is_faulty = true; }
void setFaulty(bool faulty = true) { _is_faulty = faulty; }
private:
void updateSensorToEarthRotation(const matrix::Dcmf &R_to_earth);
src/modules/ekf2/EKF/terrain_estimator.cpp
+1 -1
View File
@@ -118,7 +118,7 @@ void Ekf::controlHaglRngFusion()
// The sensor can probably not detect the ground properly
// declare the sensor faulty and stop the fusion
_control_status.flags.rng_fault = true;
_range_sensor.setFaulty();
_range_sensor.setFaulty(true);
stopHaglRngFusion();
} else {
src/modules/ekf2/EKF/vel_pos_fusion.cpp
+4 -4
View File
@@ -52,7 +52,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector2f &ob
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
aid_src.observation_variance[i] = obs_var(i);
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i];
}
@@ -70,7 +70,7 @@ void Ekf::updateVelocityAidSrcStatus(const uint64_t &time_us, const Vector3f &ob
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.vel(i) - aid_src.observation[i];
aid_src.observation_variance[i] = obs_var(i);
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
aid_src.innovation_variance[i] = P(4 + i, 4 + i) + aid_src.observation_variance[i];
}
@@ -95,7 +95,7 @@ void Ekf::updateVerticalPositionAidSrcStatus(const uint64_t &time_us, const floa
aid_src.observation = obs;
aid_src.innovation = _state.pos(2) - aid_src.observation;
aid_src.observation_variance = obs_var;
aid_src.observation_variance = math::max(sq(0.01f), obs_var);
aid_src.innovation_variance = P(9, 9) + aid_src.observation_variance;
setEstimatorAidStatusTestRatio(aid_src, innov_gate);
@@ -120,7 +120,7 @@ void Ekf::updateHorizontalPositionAidSrcStatus(const uint64_t &time_us, const Ve
aid_src.observation[i] = obs(i);
aid_src.innovation[i] = _state.pos(i) - aid_src.observation[i];
aid_src.observation_variance[i] = obs_var(i);
aid_src.observation_variance[i] = math::max(sq(0.01f), obs_var(i));
aid_src.innovation_variance[i] = P(7 + i, 7 + i) + aid_src.observation_variance[i];
}