Copter: filter ekf variances for failsafe and vibration checks

add a parameter to control EKF failsafe filtering

ArduCopter/Copter.cpp

@@ -830,7 +830,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)
 {
 }
 

ArduCopter/Copter.h

@@ -342,6 +342,14 @@ class Copter : public AP_Vehicle {
         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
 

ArduCopter/Parameters.cpp

@@ -1234,6 +1234,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

ArduCopter/Parameters.h

@@ -681,6 +681,9 @@ class ParametersG2 {
     AP_Int16 takeoff_rpm_max;
 #endif
 
+    // EKF variance filter cutoff
+    AP_Float fs_ekf_filt_hz;
+
 #if WEATHERVANE_ENABLED == ENABLED
     AC_WeatherVane weathervane;
 #endif

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

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) {

ArduCopter/system.cpp

@@ -200,6 +200,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;
 }