mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-16 06:13:34 +08:00
ekf2: refactor vertical acceleration check for multiple height sources
This commit is contained in:
bresch
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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();
|
||||
|
||||
@@ -53,6 +53,8 @@
|
||||
#include <uORB/topics/estimator_aid_source_2d.h>
|
||||
#include <uORB/topics/estimator_aid_source_3d.h>
|
||||
|
||||
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();
|
||||
|
||||
@@ -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) {
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
Reference in New Issue
Block a user