From a88626aa44530f7b11c9273e7d1374dd68261e9c Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 23 Nov 2023 18:03:38 +0000 Subject: [PATCH] Copter: filter ekf variances for failsafe and vibration checks add a parameter to control EKF failsafe filtering --- ArduCopter/Copter.cpp | 5 ++++- ArduCopter/Copter.h | 8 ++++++++ ArduCopter/Parameters.cpp | 7 +++++++ ArduCopter/Parameters.h | 3 +++ ArduCopter/config.h | 4 ++++ ArduCopter/ekf_check.cpp | 38 ++++++++++++++++++++++++-------------- ArduCopter/system.cpp | 4 ++++ 7 files changed, 54 insertions(+), 15 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 740bb95e5f..eb01308409 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -833,7 +833,10 @@ Copter::Copter(void) rc_throttle_control_in_filter(1.0f), inertial_nav(ahrs), param_loader(var_info), - flightmode(&mode_stabilize) + flightmode(&mode_stabilize), + pos_variance_filt(FS_EKF_FILT_DEFAULT), + vel_variance_filt(FS_EKF_FILT_DEFAULT), + hgt_variance_filt(FS_EKF_FILT_DEFAULT) { } diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 8685f4cb8f..9c0d1acf33 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -343,6 +343,14 @@ private: uint32_t clear_ms; // system time high vibrations stopped } vibration_check; + // EKF variances are unfiltered and are designed to recover very quickly when possible + // thus failsafes should be triggered on filtered values in order to avoid transient errors + LowPassFilterFloat pos_variance_filt; + LowPassFilterFloat vel_variance_filt; + LowPassFilterFloat hgt_variance_filt; + bool variances_valid; + uint32_t last_ekf_check_us; + // takeoff check uint32_t takeoff_check_warning_ms; // system time user was last warned of takeoff check failure diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index d1de6fe45d..7cccd21c51 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1242,6 +1242,13 @@ const AP_Param::GroupInfo ParametersG2::var_info2[] = { AP_GROUPINFO("TKOFF_RPM_MAX", 7, ParametersG2, takeoff_rpm_max, 0), #endif + // @Param: FS_EKF_FILT + // @DisplayName: EKF Failsafe filter cutoff + // @Description: EKF Failsafe filter cutoff frequency. EKF variances are filtered using this value to avoid spurious failsafes from transient high variances. A higher value means the failsafe is more likely to trigger. + // @Range: 0 10 + // @User: Advanced + AP_GROUPINFO("FS_EKF_FILT", 8, ParametersG2, fs_ekf_filt_hz, FS_EKF_FILT_DEFAULT), + // ID 62 is reserved for the AP_SUBGROUPEXTENSION AP_GROUPEND diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 3d8b62b96d..017eb05437 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -690,6 +690,9 @@ public: AP_Int16 takeoff_rpm_max; #endif + // EKF variance filter cutoff + AP_Float fs_ekf_filt_hz; + #if WEATHERVANE_ENABLED == ENABLED AC_WeatherVane weathervane; #endif diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 97793741c9..a7fcaccadf 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -134,6 +134,10 @@ # define EKF_ORIGIN_MAX_ALT_KM 50 // EKF origin and home must be within 50km vertically #endif +#ifndef FS_EKF_FILT_DEFAULT +# define FS_EKF_FILT_DEFAULT 5.0f // frequency cutoff of EKF variance filters +#endif + ////////////////////////////////////////////////////////////////////////////// // Auto Tuning #ifndef AUTOTUNE_ENABLED diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 87cbd20f6e..47d9121be9 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -113,15 +113,27 @@ void Copter::ekf_check() // ekf_over_threshold - returns true if the ekf's variance are over the tolerance bool Copter::ekf_over_threshold() { - // return false immediately if disabled - if (g.fs_ekf_thresh <= 0.0f) { + // use EKF to get variance + float position_var, vel_var, height_var, tas_variance; + Vector3f mag_variance; + variances_valid = ahrs.get_variances(vel_var, position_var, height_var, mag_variance, tas_variance); + + if (!variances_valid) { return false; } - // use EKF to get variance - float position_variance, vel_variance, height_variance, tas_variance; - Vector3f mag_variance; - if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) { + uint32_t now_us = AP_HAL::micros(); + float dt = (now_us - last_ekf_check_us) / 1000000.0f; + + // always update filtered values as this serves the vibration check as well + position_var = pos_variance_filt.apply(position_var, dt); + vel_var = vel_variance_filt.apply(vel_var, dt); + height_var = hgt_variance_filt.apply(height_var, dt); + + last_ekf_check_us = now_us; + + // return false if disabled + if (g.fs_ekf_thresh <= 0.0f) { return false; } @@ -137,13 +149,13 @@ bool Copter::ekf_over_threshold() #if AP_OPTICALFLOW_ENABLED optflow_healthy = optflow.healthy(); #endif - if (!optflow_healthy && (vel_variance >= (2.0f * g.fs_ekf_thresh))) { + if (!optflow_healthy && (vel_var >= (2.0f * g.fs_ekf_thresh))) { over_thresh_count += 2; - } else if (vel_variance >= g.fs_ekf_thresh) { + } else if (vel_var >= g.fs_ekf_thresh) { over_thresh_count++; } - if ((position_variance >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { + if ((position_var >= g.fs_ekf_thresh && over_thresh_count >= 1) || over_thresh_count >= 2) { return true; } @@ -264,14 +276,12 @@ void Copter::check_vibration() const bool innov_velD_posD_positive = is_positive(vel_innovation.z) && is_positive(pos_innovation.z); // check if vertical velocity variance is at least 1 (NK4.SV >= 1.0) - float position_variance, vel_variance, height_variance, tas_variance; - Vector3f mag_variance; - if (!ahrs.get_variances(vel_variance, position_variance, height_variance, mag_variance, tas_variance)) { + // filtered variances are updated in ekf_check() which runs at the same rate (10Hz) as this check + if (!variances_valid) { innovation_checks_valid = false; } - const bool is_vibration_affected = ahrs.is_vibration_affected(); - const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance > 1.0f)) || is_vibration_affected; + const bool bad_vibe_detected = (innovation_checks_valid && innov_velD_posD_positive && (vel_variance_filt.get() > 1.0f)) || is_vibration_affected; const bool do_bad_vibe_actions = (g2.fs_vibe_enabled == 1) && bad_vibe_detected && motors->armed() && !flightmode->has_manual_throttle(); if (!vibration_check.high_vibes) { diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 83c4cbf5b8..efa9334b2a 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -215,6 +215,10 @@ void Copter::init_ardupilot() set_mode(Mode::Number::STABILIZE, ModeReason::UNAVAILABLE); } + pos_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + vel_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + hgt_variance_filt.set_cutoff_frequency(g2.fs_ekf_filt_hz); + // flag that initialisation has completed ap.initialised = true; }