diff --git a/src/modules/ekf2/CMakeLists.txt b/src/modules/ekf2/CMakeLists.txt index bfa188ee82..aca3f66b32 100644 --- a/src/modules/ekf2/CMakeLists.txt +++ b/src/modules/ekf2/CMakeLists.txt @@ -61,6 +61,7 @@ px4_add_module( EKF/gps_control.cpp EKF/gps_fusion.cpp EKF/gps_yaw_fusion.cpp + EKF/height_control.cpp EKF/height_fusion.cpp EKF/imu_down_sampler.cpp EKF/mag_control.cpp diff --git a/src/modules/ekf2/EKF/CMakeLists.txt b/src/modules/ekf2/EKF/CMakeLists.txt index 091f2a89e1..9043be2eb5 100644 --- a/src/modules/ekf2/EKF/CMakeLists.txt +++ b/src/modules/ekf2/EKF/CMakeLists.txt @@ -46,6 +46,7 @@ add_library(ecl_EKF gps_control.cpp gps_fusion.cpp gps_yaw_fusion.cpp + height_control.cpp height_fusion.cpp imu_down_sampler.cpp mag_control.cpp diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index ac7e73dc84..0d2d18e42a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -657,101 +657,6 @@ void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) } } -void Ekf::checkVerticalAccelerationHealth() -{ - // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical - // innovations being positive and not stale. - // Clipping usually causes the average accel reading to move towards zero which makes the INS - // think it is falling and produces positive vertical innovations. - // 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 - const auto &gps_vel = _aid_src_gnss_vel; - - if (gps_vel.time_last_fuse[2] > _vert_vel_fuse_time_us) { - _vert_vel_fuse_time_us = gps_vel.time_last_fuse[2]; - _vert_vel_innov_ratio = gps_vel.innovation[2] / sqrtf(gps_vel.innovation_variance[2]); - } - - // GNSS position - const auto &gps_pos = _aid_src_gnss_pos; - - if (gps_pos.time_last_fuse[2] > _vert_pos_fuse_attempt_time_us) { - _vert_pos_fuse_attempt_time_us = gps_pos.time_last_fuse[2]; - _vert_pos_innov_ratio = gps_pos.innovation[2] / sqrtf(gps_pos.innovation_variance[2]); - } - } - - if (_control_status.flags.baro_hgt) { - // baro height - const auto &baro_hgt = _aid_src_baro_hgt; - - if (baro_hgt.time_last_fuse > _vert_pos_fuse_attempt_time_us) { - _vert_pos_fuse_attempt_time_us = baro_hgt.time_last_fuse; - _vert_pos_innov_ratio = baro_hgt.innovation / sqrtf(baro_hgt.innovation_variance); - } - } - - 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 - const bool using_gps_for_both = _control_status.flags.gps_hgt && _control_status.flags.gps; - const bool using_ev_for_both = _control_status.flags.ev_hgt && _control_status.flags.ev_vel; - are_vertical_pos_and_vel_independant = !(using_gps_for_both || using_ev_for_both); - is_inertial_nav_falling |= _vert_vel_innov_ratio > _params.vert_innov_test_lim && _vert_pos_innov_ratio > _params.vert_innov_test_min; - is_inertial_nav_falling |= _vert_pos_innov_ratio > _params.vert_innov_test_lim && _vert_vel_innov_ratio > _params.vert_innov_test_min; - - } else { - // only height sensing available - is_inertial_nav_falling = _vert_pos_innov_ratio > _params.vert_innov_test_lim; - } - } - - // Check for more than 50% clipping affected IMU samples within the past 1 second - const uint16_t clip_count_limit = 1.f / _dt_ekf_avg; - const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] || - _imu_sample_delayed.delta_vel_clipping[1] || - _imu_sample_delayed.delta_vel_clipping[2]; - - if (is_clipping && _clip_counter < clip_count_limit) { - _clip_counter++; - - } else if (_clip_counter > 0) { - _clip_counter--; - } - - _fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2; - - const bool is_clipping_frequently = _clip_counter > 0; - - // 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_using_multiple_hgt_sources || is_clipping_frequently) && is_inertial_nav_falling; - - if (bad_vert_accel) { - _time_bad_vert_accel = _time_last_imu; - - } else { - _time_good_vert_accel = _time_last_imu; - } - - // declare a bad vertical acceleration measurement and make the declaration persist - // for a minimum of BADACC_PROBATION seconds - if (_fault_status.flags.bad_acc_vertical) { - _fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION); - - } else { - _fault_status.flags.bad_acc_vertical = bad_vert_accel; - } -} - void Ekf::controlHeightFusion() { checkVerticalAccelerationHealth(); diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 0c311ed785..ea0bc23959 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -53,6 +53,8 @@ #include #include +enum class Likelihood { LOW, MEDIUM, HIGH }; + class Ekf final : public EstimatorInterface { public: @@ -939,6 +941,7 @@ private: void controlAuxVelFusion(); void checkVerticalAccelerationHealth(); + Likelihood estimateInertialNavFallingLikelihood() const; // control for combined height fusion mode (implemented for switching between baro and range height) void controlHeightFusion(); diff --git a/src/modules/ekf2/EKF/estimator_interface.cpp b/src/modules/ekf2/EKF/estimator_interface.cpp index 7547e9d906..ea5eed0acd 100644 --- a/src/modules/ekf2/EKF/estimator_interface.cpp +++ b/src/modules/ekf2/EKF/estimator_interface.cpp @@ -528,6 +528,36 @@ bool EstimatorInterface::isHorizontalAidingActive() const return getNumberOfActiveHorizontalAidingSources() > 0; } +bool EstimatorInterface::isOtherSourceOfVerticalPositionAidingThan(const bool aiding_flag) const +{ + const int nb_sources = getNumberOfActiveVerticalPositionAidingSources(); + return aiding_flag ? nb_sources > 1 : nb_sources > 0; +} + +bool EstimatorInterface::isVerticalPositionAidingActive() const +{ + return getNumberOfActiveVerticalPositionAidingSources() > 0; +} + +int EstimatorInterface::getNumberOfActiveVerticalPositionAidingSources() const +{ + return int(_control_status.flags.gps_hgt) + + int(_control_status.flags.baro_hgt) + + int(_control_status.flags.rng_hgt) + + int(_control_status.flags.ev_hgt); +} + +bool EstimatorInterface::isVerticalVelocityAidingActive() const +{ + return getNumberOfActiveVerticalVelocityAidingSources() > 0; +} + +int EstimatorInterface::getNumberOfActiveVerticalVelocityAidingSources() const +{ + return int(_control_status.flags.gps) + + int(_control_status.flags.ev_vel); +} + void EstimatorInterface::printBufferAllocationFailed(const char *buffer_name) { if (buffer_name) { diff --git a/src/modules/ekf2/EKF/estimator_interface.h b/src/modules/ekf2/EKF/estimator_interface.h index ed99a2f5f4..b8bdc97f6b 100644 --- a/src/modules/ekf2/EKF/estimator_interface.h +++ b/src/modules/ekf2/EKF/estimator_interface.h @@ -181,6 +181,13 @@ public: int getNumberOfActiveHorizontalAidingSources() const; + bool isOtherSourceOfVerticalPositionAidingThan(bool aiding_flag) const; + bool isVerticalPositionAidingActive() const; + int getNumberOfActiveVerticalPositionAidingSources() const; + + bool isVerticalVelocityAidingActive() const; + int getNumberOfActiveVerticalVelocityAidingSources() const; + const matrix::Quatf &getQuaternion() const { return _output_new.quat_nominal; } // get the velocity of the body frame origin in local NED earth frame diff --git a/src/modules/ekf2/EKF/height_control.cpp b/src/modules/ekf2/EKF/height_control.cpp new file mode 100644 index 0000000000..e5783c519e --- /dev/null +++ b/src/modules/ekf2/EKF/height_control.cpp @@ -0,0 +1,188 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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_control.cpp + */ + +#include "ekf.h" + +void Ekf::checkVerticalAccelerationHealth() +{ + // Check for IMU accelerometer vibration induced clipping as evidenced by the vertical + // innovations being positive and not stale. + // Clipping usually causes the average accel reading to move towards zero which makes the INS + // think it is falling and produces positive vertical innovations. + + Likelihood inertial_nav_falling_likelihood = estimateInertialNavFallingLikelihood(); + + // Check for more than 50% clipping affected IMU samples within the past 1 second + const uint16_t clip_count_limit = 1.f / _dt_ekf_avg; + const bool is_clipping = _imu_sample_delayed.delta_vel_clipping[0] || + _imu_sample_delayed.delta_vel_clipping[1] || + _imu_sample_delayed.delta_vel_clipping[2]; + + if (is_clipping && _clip_counter < clip_count_limit) { + _clip_counter++; + + } else if (_clip_counter > 0) { + _clip_counter--; + } + + _fault_status.flags.bad_acc_clipping = _clip_counter > clip_count_limit / 2; + + const bool is_clipping_frequently = _clip_counter > 0; + + // Do not require evidence of clipping if the likelihood of having the INS falling is high + const bool bad_vert_accel = (is_clipping_frequently && (inertial_nav_falling_likelihood == Likelihood::MEDIUM)) + || (inertial_nav_falling_likelihood == Likelihood::HIGH); + + if (bad_vert_accel) { + _time_bad_vert_accel = _time_last_imu; + + } else { + _time_good_vert_accel = _time_last_imu; + } + + // declare a bad vertical acceleration measurement and make the declaration persist + // for a minimum of BADACC_PROBATION seconds + if (_fault_status.flags.bad_acc_vertical) { + _fault_status.flags.bad_acc_vertical = isRecent(_time_bad_vert_accel, BADACC_PROBATION); + + } else { + _fault_status.flags.bad_acc_vertical = bad_vert_accel; + } +} + +Likelihood Ekf::estimateInertialNavFallingLikelihood() const +{ + bool likelihood_high = false; + bool likelihood_medium = false; + + bool failed_min[4] = {false, false, false, false}; + bool failed_lim[4] = {false, false, false, false}; + + if (isVerticalPositionAidingActive()) { + if (_control_status.flags.baro_hgt) { + const float innov_ratio = _aid_src_baro_hgt.innovation / sqrtf(_aid_src_baro_hgt.innovation_variance); + failed_min[HeightSensorRef::BARO] = innov_ratio > _params.vert_innov_test_min; + failed_lim[HeightSensorRef::BARO] = innov_ratio > _params.vert_innov_test_lim; + } + + if (_control_status.flags.gps_hgt) { + const float innov_ratio = _aid_src_gnss_pos.innovation[2] / sqrtf(_aid_src_gnss_pos.innovation_variance[2]); + failed_min[HeightSensorRef::GPS] = innov_ratio > _params.vert_innov_test_min; + failed_lim[HeightSensorRef::GPS] = innov_ratio > _params.vert_innov_test_lim; + } + + if (_control_status.flags.rng_hgt) { + const float innov_ratio = _aid_src_rng_hgt.innovation / sqrtf(_aid_src_rng_hgt.innovation_variance); + failed_min[HeightSensorRef::RANGE] = innov_ratio > _params.vert_innov_test_min; + failed_lim[HeightSensorRef::RANGE] = innov_ratio > _params.vert_innov_test_lim; + } + + if (_control_status.flags.ev_hgt) { + const float innov_ratio = _ev_pos_innov(2) / sqrtf(_ev_pos_innov_var(2)); + failed_min[HeightSensorRef::EV] = innov_ratio > _params.vert_innov_test_min; + failed_lim[HeightSensorRef::EV] = innov_ratio > _params.vert_innov_test_lim; + } + + for (unsigned i = 0; i < 4; i++) { + + if (failed_lim[i]) { + // There is a chance that the interial nav is falling if one height source is failing the test + likelihood_medium = true; + } + + for (unsigned j = 0; j < 4; j++) { + + if ((i != j) && failed_lim[i] && failed_min[j]) { + // There is a high chance that the interial nav is falling if two height sources are failing the test + likelihood_high = true; + } + } + } + } + + if (isVerticalVelocityAidingActive()) { + bool gps_vel_failed_min = false; + bool gps_vel_failed_lim = false; + + if (_control_status.flags.gps) { + const float innov_ratio = _aid_src_gnss_vel.innovation[2] / sqrtf(_aid_src_gnss_vel.innovation_variance[2]); + gps_vel_failed_min = innov_ratio > _params.vert_innov_test_min; + gps_vel_failed_lim = innov_ratio > _params.vert_innov_test_lim; + } + + bool ev_vel_failed_min = false; + bool ev_vel_failed_lim = false; + + if (_control_status.flags.ev_vel) { + const float innov_ratio = _ev_vel_innov(2) / sqrtf(_ev_vel_innov_var(2)); + ev_vel_failed_min = innov_ratio > _params.vert_innov_test_min; + ev_vel_failed_lim = innov_ratio > _params.vert_innov_test_lim; + } + + // 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 + likelihood_high |= gps_vel_failed_lim && (failed_min[HeightSensorRef::BARO] + || failed_min[HeightSensorRef::RANGE] + || failed_min[HeightSensorRef::EV] + || ev_vel_failed_min); + likelihood_high |= gps_vel_failed_min && (failed_lim[HeightSensorRef::BARO] + || failed_lim[HeightSensorRef::RANGE] + || failed_lim[HeightSensorRef::EV] + || ev_vel_failed_lim); + + likelihood_high |= ev_vel_failed_lim && (failed_min[HeightSensorRef::BARO] + || failed_min[HeightSensorRef::RANGE] + || failed_min[HeightSensorRef::GPS] + || gps_vel_failed_min); + likelihood_high |= ev_vel_failed_min && (failed_lim[HeightSensorRef::BARO] + || failed_lim[HeightSensorRef::RANGE] + || failed_lim[HeightSensorRef::GPS] + || gps_vel_failed_lim); + + likelihood_medium |= gps_vel_failed_lim; + likelihood_medium |= ev_vel_failed_lim; + } + + if (likelihood_high) { + return Likelihood::HIGH; + + } else if (likelihood_medium) { + return Likelihood::MEDIUM; + } + + return Likelihood::LOW; +}