Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-28 19:32:36 +08:00
Code Issues Actions 28 Packages Projects Releases Wiki Activity

ekf2/commander: simplify navigation filter preflight checks

Browse Source 
- remove commander test ratio "tuning knobs" (COM_ARM_EKF_{HGT,POS,VEL,YAW})
   - these are effectively redundant with the actual tuning (noise & gate)
     in the estimator, plus most users have no idea why they'd be
     adjusting these other than to silence an annoying preflight complaint
 - remove ekf2 "PreFlightChecker" with hard coded innovation limits
 - ekf2 preflight innovation flags are now simply if any active source
   exceeds half the limit preflight
This commit is contained in:
Daniel Agar
2024-06-17 18:48:12 -04:00
committed by Mathieu Bresciani
parent a42dc2165c
commit eac14b7db2
15 changed files with 104 additions and 806 deletions
Download Patch File Download Diff File Expand all files Collapse all files
boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld
-6
View File
@@ -335,7 +335,6 @@
*(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv) *(.text._ZThn16_N7sensors22VehicleAngularVelocity3RunEv)
*(.text._ZN29MavlinkStreamObstacleDistance4sendEv) *(.text._ZN29MavlinkStreamObstacleDistance4sendEv)
*(.text._ZN24MavlinkStreamOrbitStatus4sendEv) *(.text._ZN24MavlinkStreamOrbitStatus4sendEv)
*(.text._ZN16PreFlightChecker26preFlightCheckHeightFailedERK23estimator_innovations_sf)
*(.text._ZN9Navigator3runEv) *(.text._ZN9Navigator3runEv)
*(.text._ZN24MavlinkParametersManager11send_paramsEv) *(.text._ZN24MavlinkParametersManager11send_paramsEv)
*(.text._ZN17MavlinkLogHandler4sendEv) *(.text._ZN17MavlinkLogHandler4sendEv)
@@ -374,7 +373,6 @@
*(.text.imxrt_ioctl) *(.text.imxrt_ioctl)
*(.text._ZN3Ekf25checkMagBiasObservabilityEv) *(.text._ZN3Ekf25checkMagBiasObservabilityEv)
*(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv) *(.text._ZN36MavlinkStreamGimbalDeviceSetAttitude4sendEv)
*(.text._ZN16PreFlightChecker6updateEfRK23estimator_innovations_s)
*(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0) *(.text._ZN4math13expo_deadzoneIfEEKT_RS2_S3_S3_.isra.0)
*(.text._ZN19StickAccelerationXYC1EP12ModuleParams) *(.text._ZN19StickAccelerationXYC1EP12ModuleParams)
*(.text.imxrt_epsubmit) *(.text.imxrt_epsubmit)
@@ -458,7 +456,6 @@
*(.text._ZN14FlightTaskAuto17_evaluateTripletsEv) *(.text._ZN14FlightTaskAuto17_evaluateTripletsEv)
*(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb) *(.text._ZN11calibration9Gyroscope23SensorCorrectionsUpdateEb)
*(.text._ZN25MavlinkStreamMagCalReport4sendEv) *(.text._ZN25MavlinkStreamMagCalReport4sendEv)
*(.text._ZN16PreFlightChecker27preFlightCheckHeadingFailedERK23estimator_innovations_sf)
*(.text.imxrt_config_gpio) *(.text.imxrt_config_gpio)
*(.text.nxsig_timeout) *(.text.nxsig_timeout)
*(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_) *(.text._ZN11RateControl19setSaturationStatusERKN6matrix7Vector3IbEES4_)
@@ -593,7 +590,6 @@
*(.text.uart_xmitchars_done) *(.text.uart_xmitchars_done)
*(.text._ZN4EKF225PublishYawEstimatorStatusERKy) *(.text._ZN4EKF225PublishYawEstimatorStatusERKy)
*(.text.sin) *(.text.sin)
*(.text._ZN16PreFlightChecker27preFlightCheckVertVelFailedERK23estimator_innovations_sf)
*(.text._ZN6Safety19safetyButtonHandlerEv) *(.text._ZN6Safety19safetyButtonHandlerEv)
*(.text._ZN3Ekf19controlAuxVelFusionEv) *(.text._ZN3Ekf19controlAuxVelFusionEv)
*(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_) *(.text._ZNK6matrix6MatrixIfLj2ELj1EEplERKS1_)
@@ -667,10 +663,8 @@
*(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv) *(.text._ZN26MavlinkStreamCameraTrigger8get_sizeEv)
*(.text.iob_navail) *(.text.iob_navail)
*(.text._ZN12FailsafeBase25removeNonActivatedActionsEv) *(.text._ZN12FailsafeBase25removeNonActivatedActionsEv)
*(.text._ZN16PreFlightChecker28preFlightCheckHorizVelFailedERK23estimator_innovations_sf)
*(.text._ZN15TakeoffHandling10updateRampEff) *(.text._ZN15TakeoffHandling10updateRampEff)
*(.text._Z7led_offi) *(.text._Z7led_offi)
*(.text._ZN16PreFlightChecker22selectHeadingTestLimitEv)
*(.text.led_off) *(.text.led_off)
*(.text.udp_wrbuffer_test) *(.text.udp_wrbuffer_test)
*(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s) *(.text._ZNK3Ekf34updateVerticalPositionAidSrcStatusERKyfffR24estimator_aid_source1d_s)
msg/EstimatorStatus.msg
+2 -1
View File
@@ -102,9 +102,10 @@ uint8 reset_count_quat # number of quaternion reset events (allow to wrap if c
float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time float32 time_slip # cumulative amount of time in seconds that the EKF inertial calculation has slipped relative to system time
bool pre_flt_fail_innov_heading bool pre_flt_fail_innov_heading
bool pre_flt_fail_innov_height
bool pre_flt_fail_innov_pos_horiz
bool pre_flt_fail_innov_vel_horiz bool pre_flt_fail_innov_vel_horiz
bool pre_flt_fail_innov_vel_vert bool pre_flt_fail_innov_vel_vert
bool pre_flt_fail_innov_height
bool pre_flt_fail_mag_field_disturbed bool pre_flt_fail_mag_field_disturbed
uint32 accel_device_id uint32 accel_device_id
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
+17 -79
View File
@@ -63,6 +63,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
bool pre_flt_fail_innov_heading = false; bool pre_flt_fail_innov_heading = false;
bool pre_flt_fail_innov_vel_horiz = false; bool pre_flt_fail_innov_vel_horiz = false;
bool pre_flt_fail_innov_pos_horiz = false;
bool missing_data = false; bool missing_data = false;
const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude; const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
@@ -90,6 +91,7 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
if (_estimator_status_sub.copy(&estimator_status)) { if (_estimator_status_sub.copy(&estimator_status)) {
pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading; pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz; pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
pre_flt_fail_innov_pos_horiz = estimator_status.pre_flt_fail_innov_pos_horiz;
checkEstimatorStatus(context, reporter, estimator_status, required_groups); checkEstimatorStatus(context, reporter, estimator_status, required_groups);
checkEstimatorStatusFlags(context, reporter, estimator_status, lpos); checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
@@ -123,7 +125,8 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
} }
// set mode requirements // set mode requirements
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position, setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, pre_flt_fail_innov_pos_horiz,
lpos, vehicle_gps_position,
reporter.failsafeFlags(), reporter); reporter.failsafeFlags(), reporter);
@@ -166,6 +169,17 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable"); mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: vertical velocity unstable");
} }
} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_pos_horiz) {
/* EVENT
*/
reporter.armingCheckFailure(required_groups, health_component_t::local_position_estimate,
events::ID("check_estimator_hor_pos_not_stable"),
events::Log::Error, "Horizontal position unstable");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: horizontal position unstable");
}
} else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) { } else if (!context.isArmed() && estimator_status.pre_flt_fail_innov_height) {
/* EVENT /* EVENT
*/ */
@@ -208,82 +222,6 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
} }
} }
// check vertical position innovation test ratio
if (!context.isArmed() && (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get())) {
/* EVENT
* @description
* <profile name="dev">
* Test ratio: {1:.3}, limit: {2:.3}.
*
* This check can be configured via <param>COM_ARM_EKF_HGT</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
events::ID("check_estimator_hgt_est_err"),
events::Log::Error, "Height estimate error", estimator_status.hgt_test_ratio, _param_com_arm_ekf_hgt.get());
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: height estimate error");
}
}
// check velocity innovation test ratio
if (!context.isArmed() && (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get())) {
/* EVENT
* @description
* <profile name="dev">
* Test ratio: {1:.3}, limit: {2:.3}.
*
* This check can be configured via <param>COM_ARM_EKF_VEL</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
events::ID("check_estimator_vel_est_err"),
events::Log::Error, "Velocity estimate error", estimator_status.vel_test_ratio, _param_com_arm_ekf_vel.get());
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: velocity estimate error");
}
}
// check horizontal position innovation test ratio
if (!context.isArmed() && (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get())) {
/* EVENT
* @description
* <profile name="dev">
* Test ratio: {1:.3}, limit: {2:.3}.
*
* This check can be configured via <param>COM_ARM_EKF_POS</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
events::ID("check_estimator_pos_est_err"),
events::Log::Error, "Position estimate error", estimator_status.pos_test_ratio, _param_com_arm_ekf_pos.get());
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: position estimate error");
}
}
// check magnetometer innovation test ratio
if (!context.isArmed() && (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get())) {
/* EVENT
* @description
* <profile name="dev">
* Test ratio: {1:.3}, limit: {2:.3}.
*
* This check can be configured via <param>COM_ARM_EKF_YAW</param> parameter.
* </profile>
*/
reporter.armingCheckFailure<float, float>(required_groups, health_component_t::local_position_estimate,
events::ID("check_estimator_yaw_est_err"),
events::Log::Error, "Yaw estimate error", estimator_status.mag_test_ratio, _param_com_arm_ekf_yaw.get());
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Yaw estimate error");
}
}
// If GPS aiding is required, declare fault condition if the required GPS quality checks are failing // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
if (_param_sys_has_gps.get()) { if (_param_sys_has_gps.get()) {
const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS); const bool ekf_gps_fusion = estimator_status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
@@ -767,7 +705,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
} }
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, failsafe_flags_s &failsafe_flags,
Report &reporter) Report &reporter)
{ {
@@ -797,7 +735,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed; bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
if (!context.isArmed()) { if (!context.isArmed()) {
if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) { if (pre_flt_fail_innov_heading || pre_flt_fail_innov_pos_horiz) {
xy_valid = false; xy_valid = false;
} }
src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
+3 -5
View File
@@ -66,7 +66,9 @@ private:
const vehicle_local_position_s &lpos); const vehicle_local_position_s &lpos);
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const; void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const; void lowPositionAccuracy(const Context &context, Report &reporter, const vehicle_local_position_s &lpos) const;
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
bool pre_flt_fail_innov_vel_horiz, bool pre_flt_fail_innov_pos_horiz,
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
failsafe_flags_s &failsafe_flags, Report &reporter); failsafe_flags_s &failsafe_flags, Report &reporter);
@@ -108,10 +110,6 @@ private:
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase, DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode, (ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str, (ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
(ParamFloat<px4::params::COM_ARM_EKF_VEL>) _param_com_arm_ekf_vel,
(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps, (ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps, (ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph, (ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
src/modules/commander/commander_params.c
-44
View File
@@ -358,50 +358,6 @@ PARAM_DEFINE_INT32(COM_OBL_RC_ACT, 0);
*/ */
PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f); PARAM_DEFINE_FLOAT(COM_OBC_LOSS_T, 5.0f);
/**
* Maximum EKF position innovation test ratio that will allow arming
*
* @group Commander
* @min 0.1
* @max 1.0
* @decimal 2
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_ARM_EKF_POS, 0.5f);
/**
* Maximum EKF velocity innovation test ratio that will allow arming
*
* @group Commander
* @min 0.1
* @max 1.0
* @decimal 2
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_ARM_EKF_VEL, 0.5f);
/**
* Maximum EKF height innovation test ratio that will allow arming
*
* @group Commander
* @min 0.1
* @max 1.0
* @decimal 2
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_ARM_EKF_HGT, 1.0f);
/**
* Maximum EKF yaw innovation test ratio that will allow arming
*
* @group Commander
* @min 0.1
* @max 1.0
* @decimal 2
* @increment 0.05
*/
PARAM_DEFINE_FLOAT(COM_ARM_EKF_YAW, 0.5f);
/** /**
* Maximum accelerometer inconsistency between IMU units that will allow arming * Maximum accelerometer inconsistency between IMU units that will allow arming
* *
src/modules/ekf2/CMakeLists.txt
-3
View File
@@ -31,8 +31,6 @@
# #
############################################################################# #############################################################################
add_subdirectory(Utility)
option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF) option(EKF2_SYMFORCE_GEN "ekf2 generate symforce output" OFF)
# Symforce code generation TODO:fixme # Symforce code generation TODO:fixme
@@ -254,7 +252,6 @@ px4_add_module(
geo geo
hysteresis hysteresis
perf perf
EKF2Utility
px4_work_queue px4_work_queue
world_magnetic_model world_magnetic_model
src/modules/ekf2/EKF/ekf.h
+2 -1
View File
@@ -396,7 +396,8 @@ public:
float getHeadingInnovationTestRatio() const; float getHeadingInnovationTestRatio() const;
float getVelocityInnovationTestRatio() const; float getHorizontalVelocityInnovationTestRatio() const;
float getVerticalVelocityInnovationTestRatio() const;
float getHorizontalPositionInnovationTestRatio() const; float getHorizontalPositionInnovationTestRatio() const;
float getVerticalPositionInnovationTestRatio() const; float getVerticalPositionInnovationTestRatio() const;
src/modules/ekf2/EKF/ekf_helper.cpp
+56 -10
View File
@@ -319,7 +319,7 @@ void Ekf::resetAccelBias()
float Ekf::getHeadingInnovationTestRatio() const float Ekf::getHeadingInnovationTestRatio() const
{ {
// return the largest heading innovation test ratio // return the largest heading innovation test ratio
float test_ratio = 0.f; float test_ratio = -1.f;
#if defined(CONFIG_EKF2_MAGNETOMETER) #if defined(CONFIG_EKF2_MAGNETOMETER)
@@ -347,10 +347,14 @@ float Ekf::getHeadingInnovationTestRatio() const
#endif // CONFIG_EKF2_EXTERNAL_VISION #endif // CONFIG_EKF2_EXTERNAL_VISION
return sqrtf(test_ratio); if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
return sqrtf(test_ratio);
}
return NAN;
} }
float Ekf::getVelocityInnovationTestRatio() const float Ekf::getHorizontalVelocityInnovationTestRatio() const
{ {
// return the largest velocity innovation test ratio // return the largest velocity innovation test ratio
float test_ratio = -1.f; float test_ratio = -1.f;
@@ -358,7 +362,7 @@ float Ekf::getVelocityInnovationTestRatio() const
#if defined(CONFIG_EKF2_GNSS) #if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) { if (_control_status.flags.gps) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 2; i++) { // only xy
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i])); test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[i]));
} }
} }
@@ -368,7 +372,7 @@ float Ekf::getVelocityInnovationTestRatio() const
#if defined(CONFIG_EKF2_EXTERNAL_VISION) #if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_vel) { if (_control_status.flags.ev_vel) {
for (int i = 0; i < 3; i++) { for (int i = 0; i < 2; i++) { // only xy
test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i])); test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[i]));
} }
} }
@@ -392,6 +396,34 @@ float Ekf::getVelocityInnovationTestRatio() const
return NAN; return NAN;
} }
float Ekf::getVerticalVelocityInnovationTestRatio() const
{
// return the largest velocity innovation test ratio
float test_ratio = -1.f;
#if defined(CONFIG_EKF2_GNSS)
if (_control_status.flags.gps) {
test_ratio = math::max(test_ratio, fabsf(_aid_src_gnss_vel.test_ratio_filtered[2]));
}
#endif // CONFIG_EKF2_GNSS
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
if (_control_status.flags.ev_vel) {
test_ratio = math::max(test_ratio, fabsf(_aid_src_ev_vel.test_ratio_filtered[2]));
}
#endif // CONFIG_EKF2_EXTERNAL_VISION
if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) {
return sqrtf(test_ratio);
}
return NAN;
}
float Ekf::getHorizontalPositionInnovationTestRatio() const float Ekf::getHorizontalPositionInnovationTestRatio() const
{ {
// return the largest position innovation test ratio // return the largest position innovation test ratio
@@ -511,21 +543,35 @@ float Ekf::getSyntheticSideslipInnovationTestRatio() const
float Ekf::getHeightAboveGroundInnovationTestRatio() const float Ekf::getHeightAboveGroundInnovationTestRatio() const
{ {
float test_ratio = -1.f; // return the combined HAGL innovation test ratio
float hagl_sum = 0.f;
int n_hagl_sources = 0;
#if defined(CONFIG_EKF2_TERRAIN) #if defined(CONFIG_EKF2_TERRAIN)
# if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_control_status.flags.opt_flow_terrain) {
hagl_sum += sqrtf(math::max(fabsf(_aid_src_optical_flow.test_ratio_filtered[0]),
_aid_src_optical_flow.test_ratio_filtered[1]));
n_hagl_sources++;
}
# endif // CONFIG_EKF2_OPTICAL_FLOW
# if defined(CONFIG_EKF2_RANGE_FINDER) # if defined(CONFIG_EKF2_RANGE_FINDER)
if (_control_status.flags.rng_terrain) { if (_control_status.flags.rng_terrain) {
// return the terrain height innovation test ratio hagl_sum += sqrtf(fabsf(_aid_src_rng_hgt.test_ratio_filtered));
test_ratio = math::max(test_ratio, fabsf(_aid_src_rng_hgt.test_ratio_filtered)); n_hagl_sources++;
} }
# endif // CONFIG_EKF2_RANGE_FINDER # endif // CONFIG_EKF2_RANGE_FINDER
#endif // CONFIG_EKF2_TERRAIN #endif // CONFIG_EKF2_TERRAIN
if (PX4_ISFINITE(test_ratio) && (test_ratio >= 0.f)) { if (n_hagl_sources > 0) {
return sqrtf(test_ratio); return math::max(hagl_sum / static_cast<float>(n_hagl_sources), FLT_MIN);
} }
return NAN; return NAN;
src/modules/ekf2/EKF2.cpp
+24 -42
View File
@@ -1333,43 +1333,6 @@ void EKF2::PublishInnovations(const hrt_abstime &timestamp)
innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time(); innovations.timestamp = _replay_mode ? timestamp : hrt_absolute_time();
_estimator_innovations_pub.publish(innovations); _estimator_innovations_pub.publish(innovations);
// calculate noise filtered velocity innovations which are used for pre-flight checking
if (_ekf.control_status_prev_flags().in_air != _ekf.control_status_flags().in_air) {
// fully reset on takeoff or landing
_preflt_checker.reset();
}
if (!_ekf.control_status_flags().in_air) {
// TODO: move to run before publications
_preflt_checker.setUsingGpsAiding(_ekf.control_status_flags().gps);
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_preflt_checker.setUsingFlowAiding(_ekf.control_status_flags().opt_flow);
#endif // CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_TERRAIN) && defined(CONFIG_EKF2_OPTICAL_FLOW)
// set dist bottom to scale flow innovation
const float dist_bottom = _ekf.getHagl();
_preflt_checker.setDistBottom(dist_bottom);
#endif // CONFIG_EKF2_TERRAIN && CONFIG_EKF2_OPTICAL_FLOW
#if defined(CONFIG_EKF2_EXTERNAL_VISION)
_preflt_checker.setUsingEvPosAiding(_ekf.control_status_flags().ev_pos);
_preflt_checker.setUsingEvVelAiding(_ekf.control_status_flags().ev_vel);
_preflt_checker.setUsingEvHgtAiding(_ekf.control_status_flags().ev_hgt);
#endif // CONFIG_EKF2_EXTERNAL_VISION
#if defined(CONFIG_EKF2_BAROMETER)
_preflt_checker.setUsingBaroHgtAiding(_ekf.control_status_flags().baro_hgt);
#endif // CONFIG_EKF2_BAROMETER
_preflt_checker.setUsingGpsHgtAiding(_ekf.control_status_flags().gps_hgt);
#if defined(CONFIG_EKF2_RANGE_FINDER)
_preflt_checker.setUsingRngHgtAiding(_ekf.control_status_flags().rng_hgt);
#endif // CONFIG_EKF2_RANGE_FINDER
_preflt_checker.setVehicleCanObserveHeadingInFlight(_ekf.control_status_flags().fixed_wing);
_preflt_checker.update(_ekf.get_dt_ekf_avg(), innovations);
}
} }
void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp) void EKF2::PublishInnovationTestRatios(const hrt_abstime &timestamp)
@@ -1816,8 +1779,24 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.control_mode_flags = _ekf.control_status().value; status.control_mode_flags = _ekf.control_status().value;
status.filter_fault_flags = _ekf.fault_status().value; status.filter_fault_flags = _ekf.fault_status().value;
// vel_test_ratio
float vel_xy_test_ratio = _ekf.getHorizontalVelocityInnovationTestRatio();
float vel_z_test_ratio = _ekf.getVerticalVelocityInnovationTestRatio();
if (PX4_ISFINITE(vel_xy_test_ratio) && PX4_ISFINITE(vel_z_test_ratio)) {
status.vel_test_ratio = math::max(vel_xy_test_ratio, vel_z_test_ratio);
} else if (PX4_ISFINITE(vel_xy_test_ratio)) {
status.vel_test_ratio = vel_xy_test_ratio;
} else if (PX4_ISFINITE(vel_z_test_ratio)) {
status.vel_test_ratio = vel_z_test_ratio;
} else {
status.vel_test_ratio = NAN;
}
status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio(); status.mag_test_ratio = _ekf.getHeadingInnovationTestRatio();
status.vel_test_ratio = _ekf.getVelocityInnovationTestRatio();
status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio(); status.pos_test_ratio = _ekf.getHorizontalPositionInnovationTestRatio();
status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio(); status.hgt_test_ratio = _ekf.getVerticalPositionInnovationTestRatio();
status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio(); status.tas_test_ratio = _ekf.getAirspeedInnovationTestRatio();
@@ -1836,10 +1815,13 @@ void EKF2::PublishStatus(const hrt_abstime &timestamp)
status.time_slip = _last_time_slip_us * 1e-6f; status.time_slip = _last_time_slip_us * 1e-6f;
status.pre_flt_fail_innov_heading = _preflt_checker.hasHeadingFailed(); static constexpr float kMinTestRatioPreflight = 0.5f;
status.pre_flt_fail_innov_vel_horiz = _preflt_checker.hasHorizVelFailed(); status.pre_flt_fail_innov_heading = (kMinTestRatioPreflight < status.mag_test_ratio);
status.pre_flt_fail_innov_vel_vert = _preflt_checker.hasVertVelFailed(); status.pre_flt_fail_innov_height = (kMinTestRatioPreflight < status.hgt_test_ratio);
status.pre_flt_fail_innov_height = _preflt_checker.hasHeightFailed(); status.pre_flt_fail_innov_pos_horiz = (kMinTestRatioPreflight < status.pos_test_ratio);
status.pre_flt_fail_innov_vel_horiz = (kMinTestRatioPreflight < vel_xy_test_ratio);
status.pre_flt_fail_innov_vel_vert = (kMinTestRatioPreflight < vel_z_test_ratio);
status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed; status.pre_flt_fail_mag_field_disturbed = _ekf.control_status_flags().mag_field_disturbed;
status.accel_device_id = _device_id_accel; status.accel_device_id = _device_id_accel;
src/modules/ekf2/EKF2.hpp
-3
View File
@@ -42,7 +42,6 @@
#define EKF2_HPP #define EKF2_HPP
#include "EKF/ekf.h" #include "EKF/ekf.h"
#include "Utility/PreFlightChecker.hpp"
#include "EKF2Selector.hpp" #include "EKF2Selector.hpp"
@@ -478,8 +477,6 @@ private:
uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)}; uORB::PublicationMulti<estimator_aid_source3d_s> _estimator_aid_src_gravity_pub{ORB_ID(estimator_aid_src_gravity)};
#endif // CONFIG_EKF2_GRAVITY_FUSION #endif // CONFIG_EKF2_GRAVITY_FUSION
PreFlightChecker _preflt_checker;
Ekf _ekf; Ekf _ekf;
parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance) parameters *_params; ///< pointer to ekf parameter struct (located in _ekf class instance)
src/modules/ekf2/Utility/CMakeLists.txt
-45
View File
@@ -1,45 +0,0 @@
############################################################################
#
# Copyright (c) 2019 PX4 Development Team. 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.
#
#############################################################################
px4_add_library(EKF2Utility
PreFlightChecker.cpp
)
target_include_directories(EKF2Utility
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
)
target_link_libraries(EKF2Utility PRIVATE mathlib)
px4_add_unit_gtest(SRC PreFlightCheckerTest.cpp LINKLIBS EKF2Utility)
src/modules/ekf2/Utility/InnovationLpf.hpp
-79
View File
@@ -1,79 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. 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.
*
****************************************************************************/
/**
* First order "alpha" IIR digital filter with input saturation
*/
#include <mathlib/mathlib.h>
class InnovationLpf final
{
public:
InnovationLpf() = default;
~InnovationLpf() = default;
void reset(float val = 0.f) { _x = val; }
/**
* Update the filter with a new value and returns the filtered state
* The new value is constained by the limit set in setSpikeLimit
* @param val new input
* @param alpha normalized weight of the new input
* @param spike_limit the amplitude of the saturation at the input of the filter
* @return filtered output
*/
float update(float val, float alpha, float spike_limit)
{
float val_constrained = math::constrain(val, -spike_limit, spike_limit);
float beta = 1.f - alpha;
_x = beta * _x + alpha * val_constrained;
return _x;
}
/**
* Helper function to compute alpha from dt and the inverse of tau
* @param dt sampling time in seconds
* @param tau_inv inverse of the time constant of the filter
* @return alpha, the normalized weight of a new measurement
*/
static float computeAlphaFromDtAndTauInv(float dt, float tau_inv)
{
return math::constrain(dt * tau_inv, 0.f, 1.f);
}
private:
float _x{}; ///< current state of the filter
};
src/modules/ekf2/Utility/PreFlightChecker.cpp
-180
View File
@@ -1,180 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. 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 PreFlightCheckHelper.cpp
* Class handling the EKF2 innovation pre flight checks
*/
#include "PreFlightChecker.hpp"
void PreFlightChecker::update(const float dt, const estimator_innovations_s &innov)
{
const float alpha = InnovationLpf::computeAlphaFromDtAndTauInv(dt, _innov_lpf_tau_inv);
_has_heading_failed = preFlightCheckHeadingFailed(innov, alpha);
_has_horiz_vel_failed = preFlightCheckHorizVelFailed(innov, alpha);
_has_vert_vel_failed = preFlightCheckVertVelFailed(innov, alpha);
_has_height_failed = preFlightCheckHeightFailed(innov, alpha);
}
bool PreFlightChecker::preFlightCheckHeadingFailed(const estimator_innovations_s &innov, const float alpha)
{
const float heading_test_limit = selectHeadingTestLimit();
const float heading_innov_spike_lim = 2.0f * heading_test_limit;
const float heading_innov_lpf = _filter_heading_innov.update(innov.heading, alpha, heading_innov_spike_lim);
return checkInnovFailed(heading_innov_lpf, innov.heading, heading_test_limit, heading_innov_spike_lim);
}
float PreFlightChecker::selectHeadingTestLimit()
{
// Select the max allowed heading innovaton depending on whether we are not aiding navigation using
// observations in the NE reference frame and if the vehicle can use GPS course to realign in flight (fixedwing sideslip fusion).
const bool is_ne_aiding = _is_using_gps_aiding || _is_using_ev_pos_aiding;
return (is_ne_aiding && !_can_observe_heading_in_flight)
? _nav_heading_innov_test_lim // more restrictive test limit
: _heading_innov_test_lim; // less restrictive test limit
}
bool PreFlightChecker::preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, const float alpha)
{
bool has_failed = false;
if (_is_using_gps_aiding || _is_using_ev_vel_aiding) {
const Vector2f vel_ne_innov = Vector2f(fmaxf(fabsf(innov.gps_hvel[0]), fabsf(innov.ev_hvel[0])),
fmaxf(fabsf(innov.gps_hvel[1]), fabsf(innov.ev_hvel[1])));
Vector2f vel_ne_innov_lpf;
vel_ne_innov_lpf(0) = _filter_vel_n_innov.update(vel_ne_innov(0), alpha, _vel_innov_spike_lim);
vel_ne_innov_lpf(1) = _filter_vel_e_innov.update(vel_ne_innov(1), alpha, _vel_innov_spike_lim);
has_failed |= checkInnov2DFailed(vel_ne_innov_lpf, vel_ne_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
}
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
if (_is_using_flow_aiding) {
const Vector2f flow_innov = Vector2f(innov.flow) * _flow_dist_bottom;
Vector2f flow_innov_lpf;
flow_innov_lpf(0) = _filter_flow_x_innov.update(flow_innov(0), alpha, _flow_innov_spike_lim);
flow_innov_lpf(1) = _filter_flow_y_innov.update(flow_innov(1), alpha, _flow_innov_spike_lim);
has_failed |= checkInnov2DFailed(flow_innov_lpf, flow_innov, _flow_innov_test_lim, 5.f * _flow_innov_spike_lim);
}
#endif // CONFIG_EKF2_OPTICAL_FLOW
return has_failed;
}
bool PreFlightChecker::preFlightCheckVertVelFailed(const estimator_innovations_s &innov, const float alpha)
{
const float vel_d_innov = fmaxf(fabsf(innov.gps_vvel), fabs(innov.ev_vvel)); // only temporary solution
const float vel_d_innov_lpf = _filter_vel_d_innov.update(vel_d_innov, alpha, _vel_innov_spike_lim);
return checkInnovFailed(vel_d_innov_lpf, vel_d_innov, _vel_innov_test_lim, _vel_innov_spike_lim);
}
bool PreFlightChecker::preFlightCheckHeightFailed(const estimator_innovations_s &innov, const float alpha)
{
bool has_failed = false;
if (_is_using_baro_hgt_aiding) {
const float baro_hgt_innov_lpf = _filter_baro_hgt_innov.update(innov.baro_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(baro_hgt_innov_lpf, innov.baro_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
if (_is_using_gps_hgt_aiding) {
const float gps_hgt_innov_lpf = _filter_gps_hgt_innov.update(innov.gps_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(gps_hgt_innov_lpf, innov.gps_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
#if defined(CONFIG_EKF2_RANGE_FINDER)
if (_is_using_rng_hgt_aiding) {
const float rng_hgt_innov_lpf = _filter_rng_hgt_innov.update(innov.rng_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(rng_hgt_innov_lpf, innov.rng_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
#endif // CONFIG_EKF2_RANGE_FINDER
if (_is_using_ev_hgt_aiding) {
const float ev_hgt_innov_lpf = _filter_ev_hgt_innov.update(innov.ev_vpos, alpha, _hgt_innov_spike_lim);
has_failed |= checkInnovFailed(ev_hgt_innov_lpf, innov.ev_vpos, _hgt_innov_test_lim, _hgt_innov_spike_lim);
}
return has_failed;
}
bool PreFlightChecker::checkInnovFailed(const float innov_lpf, const float innov, const float test_limit,
const float spike_limit)
{
return fabsf(innov_lpf) > test_limit || fabsf(innov) > spike_limit;
}
bool PreFlightChecker::checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, const float test_limit,
const float spike_limit)
{
return innov_lpf.norm_squared() > sq(test_limit)
|| innov.norm_squared() > sq(spike_limit);
}
void PreFlightChecker::reset()
{
_is_using_gps_aiding = false;
_is_using_ev_pos_aiding = false;
_is_using_ev_vel_aiding = false;
_is_using_baro_hgt_aiding = false;
_is_using_gps_hgt_aiding = false;
_is_using_ev_hgt_aiding = false;
_has_heading_failed = false;
_has_horiz_vel_failed = false;
_has_vert_vel_failed = false;
_has_height_failed = false;
_filter_vel_n_innov.reset();
_filter_vel_e_innov.reset();
_filter_vel_d_innov.reset();
_filter_baro_hgt_innov.reset();
_filter_gps_hgt_innov.reset();
_filter_ev_hgt_innov.reset();
_filter_heading_innov.reset();
#if defined(CONFIG_EKF2_RANGE_FINDER)
_is_using_rng_hgt_aiding = false;
_filter_rng_hgt_innov.reset();
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
_is_using_flow_aiding = false;
_filter_flow_x_innov.reset();
_filter_flow_y_innov.reset();
#endif // CONFIG_EKF2_OPTICAL_FLOW
}
src/modules/ekf2/Utility/PreFlightChecker.hpp
-211
View File
@@ -1,211 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2019 PX4 Development Team. 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 PreFlightChecker.hpp
* Class handling the EKF2 innovation pre flight checks
*
* First call the update(...) function and then get the results
* using the hasXxxFailed() getters
*/
#ifndef EKF2_PREFLIGHTCHECKER_HPP
#define EKF2_PREFLIGHTCHECKER_HPP
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/estimator_innovations.h>
#include <matrix/matrix/math.hpp>
#include "InnovationLpf.hpp"
using matrix::Vector2f;
class PreFlightChecker
{
public:
PreFlightChecker() = default;
~PreFlightChecker() = default;
/*
* Reset all the internal states of the class to their default value
*/
void reset();
/*
* Update the internal states
* @param dt the sampling time
* @param innov the ekf2_innovation_s struct containing the current innovations
*/
void update(float dt, const estimator_innovations_s &innov);
/*
* If set to true, the checker will use a less conservative heading innovation check
*/
void setVehicleCanObserveHeadingInFlight(bool val) { _can_observe_heading_in_flight = val; }
void setUsingGpsAiding(bool val) { _is_using_gps_aiding = val; }
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
void setUsingFlowAiding(bool val) { _is_using_flow_aiding = val; }
void setDistBottom(float dist_bottom) { _flow_dist_bottom = dist_bottom; }
#endif // CONFIG_EKF2_OPTICAL_FLOW
void setUsingEvPosAiding(bool val) { _is_using_ev_pos_aiding = val; }
void setUsingEvVelAiding(bool val) { _is_using_ev_vel_aiding = val; }
void setUsingBaroHgtAiding(bool val) { _is_using_baro_hgt_aiding = val; }
void setUsingGpsHgtAiding(bool val) { _is_using_gps_hgt_aiding = val; }
#if defined(CONFIG_EKF2_RANGE_FINDER)
void setUsingRngHgtAiding(bool val) { _is_using_rng_hgt_aiding = val; }
#endif // CONFIG_EKF2_RANGE_FINDER
void setUsingEvHgtAiding(bool val) { _is_using_ev_hgt_aiding = val; }
bool hasHeadingFailed() const { return _has_heading_failed; }
bool hasHorizVelFailed() const { return _has_horiz_vel_failed; }
bool hasVertVelFailed() const { return _has_vert_vel_failed; }
bool hasHeightFailed() const { return _has_height_failed; }
/*
* Overall state of the pre fligh checks
* @return true if any of the check failed
*/
bool hasFailed() const { return hasHorizFailed() || hasVertFailed(); }
/*
* Horizontal checks overall result
* @return true if one of the horizontal checks failed
*/
bool hasHorizFailed() const { return _has_heading_failed || _has_horiz_vel_failed; }
/*
* Vertical checks overall result
* @return true if one of the vertical checks failed
*/
bool hasVertFailed() const { return _has_vert_vel_failed || _has_height_failed; }
/*
* Check if the innovation fails the test
* To pass the test, the following conditions should be true:
* innov_lpf <= test_limit
* innov <= spike_limit
* @param innov_lpf the low-pass filtered innovation
* @param innov the current unfiltered innovation
* @param test_limit the magnitude test limit for innov_lpf
* @param spike_limit the magnitude test limit for innov
* @return true if the check failed the test, false otherwise
*/
static bool checkInnovFailed(float innov_lpf, float innov, float test_limit, float spike_limit);
/*
* Check if the a innovation of a 2D vector fails the test
* To pass the test, the following conditions should be true:
* innov_lpf <= test_limit
* innov <= spike_limit
* @param innov_lpf the low-pass filtered innovation
* @param innov the current unfiltered innovation
* @param test_limit the magnitude test limit for innov_lpf
* @param spike_limit the magnitude test limit for innov
* @return true if the check failed the test, false otherwise
*/
static bool checkInnov2DFailed(const Vector2f &innov_lpf, const Vector2f &innov, float test_limit, float spike_limit);
static constexpr float sq(float var) { return var * var; }
private:
bool preFlightCheckHeadingFailed(const estimator_innovations_s &innov, float alpha);
float selectHeadingTestLimit();
bool preFlightCheckHorizVelFailed(const estimator_innovations_s &innov, float alpha);
bool preFlightCheckVertVelFailed(const estimator_innovations_s &innov, float alpha);
bool preFlightCheckHeightFailed(const estimator_innovations_s &innov, float alpha);
bool _has_heading_failed{};
bool _has_horiz_vel_failed{};
bool _has_vert_vel_failed{};
bool _has_height_failed{};
bool _can_observe_heading_in_flight{};
bool _is_using_gps_aiding{};
bool _is_using_ev_pos_aiding{};
bool _is_using_ev_vel_aiding{};
bool _is_using_baro_hgt_aiding{};
bool _is_using_gps_hgt_aiding{};
bool _is_using_ev_hgt_aiding{};
// Low-pass filters for innovation pre-flight checks
InnovationLpf _filter_vel_n_innov; ///< Preflight low pass filter N axis velocity innovations (m/sec)
InnovationLpf _filter_vel_e_innov; ///< Preflight low pass filter E axis velocity innovations (m/sec)
InnovationLpf _filter_vel_d_innov; ///< Preflight low pass filter D axis velocity innovations (m/sec)
InnovationLpf _filter_heading_innov; ///< Preflight low pass filter heading innovation magntitude (rad)
// Preflight low pass filter height innovation (m)
InnovationLpf _filter_baro_hgt_innov;
InnovationLpf _filter_gps_hgt_innov;
InnovationLpf _filter_ev_hgt_innov;
#if defined(CONFIG_EKF2_RANGE_FINDER)
bool _is_using_rng_hgt_aiding {};
InnovationLpf _filter_rng_hgt_innov;
#endif // CONFIG_EKF2_RANGE_FINDER
#if defined(CONFIG_EKF2_OPTICAL_FLOW)
bool _is_using_flow_aiding {};
float _flow_dist_bottom {};
InnovationLpf _filter_flow_x_innov; ///< Preflight low pass filter optical flow innovation (rad)
InnovationLpf _filter_flow_y_innov; ///< Preflight low pass filter optical flow innovation (rad)
// Maximum permissible flow innovation to pass pre-flight checks
static constexpr float _flow_innov_test_lim = 0.5f;
// Preflight flow innovation spike limit (rad)
static constexpr float _flow_innov_spike_lim = 2.0f * _flow_innov_test_lim;
#endif // CONFIG_EKF2_OPTICAL_FLOW
// Preflight low pass filter time constant inverse (1/sec)
static constexpr float _innov_lpf_tau_inv = 0.2f;
// Maximum permissible velocity innovation to pass pre-flight checks (m/sec)
static constexpr float _vel_innov_test_lim = 0.5f;
// Maximum permissible height innovation to pass pre-flight checks (m)
static constexpr float _hgt_innov_test_lim = 1.5f;
// Maximum permissible yaw innovation to pass pre-flight checks when aiding inertial nav using NE frame observations (rad)
static constexpr float _nav_heading_innov_test_lim = 0.25f;
// Maximum permissible yaw innovation to pass pre-flight checks when not aiding inertial nav using NE frame observations (rad)
static constexpr float _heading_innov_test_lim = 0.52f;
// Preflight velocity innovation spike limit (m/sec)
static constexpr float _vel_innov_spike_lim = 2.0f * _vel_innov_test_lim;
// Preflight position innovation spike limit (m)
static constexpr float _hgt_innov_spike_lim = 2.0f * _hgt_innov_test_lim;
};
#endif // !EKF2_PREFLIGHTCHECKER_HPP
src/modules/ekf2/Utility/PreFlightCheckerTest.cpp
-97
View File
@@ -1,97 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2019 PX4 Development Team. 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.
*
****************************************************************************/
/**
* Test code for PreFlightChecker class
* Run this test only using make tests TESTFILTER=PreFlightChecker
*/
#include <gtest/gtest.h>
#include "PreFlightChecker.hpp"
class PreFlightCheckerTest : public ::testing::Test
{
};
TEST_F(PreFlightCheckerTest, testInnovFailed)
{
const float test_limit = 1.0; ///< is the limit for innovation_lpf
const float spike_limit = 2.f * test_limit; ///< is the limit for innovation_lpf
const float innovations[9] = {0.0, 1.5, 2.5, -1.5, -2.5, 1.5, -1.5, -2.5, -2.5};
const float innovations_lpf[9] = {0.0, 0.9, 0.9, -0.9, -0.9, 1.1, -1.1, -1.1, 1.1};
const bool expected_result[9] = {false, false, true, false, true, true, true, true, true};
for (int i = 0; i < 9; i++) {
EXPECT_EQ(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
expected_result[i]);
}
// Smaller test limit, all the checks should fail except the first
EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[0], innovations[0], 0.0, 0.0));
for (int i = 1; i < 9; i++) {
EXPECT_TRUE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
}
// Larger test limit, none of the checks should fail
for (int i = 0; i < 9; i++) {
EXPECT_FALSE(PreFlightChecker::checkInnovFailed(innovations_lpf[i], innovations[i], 2.0, 4.0));
}
}
TEST_F(PreFlightCheckerTest, testInnov2dFailed)
{
const float test_limit = 1.0;
const float spike_limit = 2.0;
Vector2f innovations[4] = {{0.0, 0.0}, {0.0, 0.0}, {0.0, -2.5}, {1.5, -1.5}};
Vector2f innovations_lpf[4] = {{0.0, 0.0}, {1.1, 0.0}, {0.5, 0.5}, {1.0, -1.0}};
const bool expected_result[4] = {false, true, true, true};
for (int i = 0; i < 4; i++) {
EXPECT_EQ(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], test_limit, spike_limit),
expected_result[i]);
}
// Smaller test limit, all the checks should fail except the first
EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations[0], innovations_lpf[0], 0.0, 0.0));
for (int i = 1; i < 4; i++) {
EXPECT_TRUE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 0.0, 0.0));
}
// Larger test limit, none of the checks should fail
for (int i = 0; i < 4; i++) {
EXPECT_FALSE(PreFlightChecker::checkInnov2DFailed(innovations_lpf[i], innovations[i], 1.42, 2.84));
}
}