Copter: add a parameter to control EKF failsafe filtering

ArduCopter/Copter.cpp

@@ -823,9 +823,9 @@ Copter::Copter(void)
     inertial_nav(ahrs),
     param_loader(var_info),
     flightmode(&mode_stabilize),
-    position_variance(EKF_LPF_CUTOFF),
-    vel_variance(EKF_LPF_CUTOFF),
-    height_variance(EKF_LPF_CUTOFF)
+    pos_variance_filt(EKF_LPF_CUTOFF),
+    vel_variance_filt(EKF_LPF_CUTOFF),
+    hgt_variance_filt(EKF_LPF_CUTOFF)
 {
 }
 

ArduCopter/Copter.h

@@ -349,9 +349,9 @@ class Copter : public AP_Vehicle {
 
     // 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 position_variance;
-    LowPassFilterFloat vel_variance;
-    LowPassFilterFloat height_variance;
+    LowPassFilterFloat pos_variance_filt;
+    LowPassFilterFloat vel_variance_filt;
+    LowPassFilterFloat hgt_variance_filt;
     bool variances_valid;
     uint32_t last_ekf_check_us;
 

ArduCopter/Parameters.cpp

@@ -1229,6 +1229,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, EKF_LPF_CUTOFF),
+
     // ID 62 is reserved for the AP_SUBGROUPEXTENSION
 
     AP_GROUPEND

ArduCopter/Parameters.h

@@ -686,6 +686,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/ekf_check.cpp

@@ -113,11 +113,6 @@ 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) {
-        return false;
-    }
-
     // use EKF to get variance
     float position_var, vel_var, height_var, tas_variance;
     Vector3f mag_variance;
@@ -130,12 +125,18 @@ bool Copter::ekf_over_threshold()
     uint32_t now_us = AP_HAL::micros();
     float dt = (now_us - last_ekf_check_us) / 1000000.0f;
 
-    position_var = position_variance.apply(position_var, dt);
-    vel_var = vel_variance.apply(vel_var, dt);
-    height_var = height_variance.apply(height_var, dt);
+    // 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;
+    }
+
     const float mag_max = fmaxf(fmaxf(mag_variance.x,mag_variance.y),mag_variance.z);
 
     // return true if two of compass, velocity and position variances are over the threshold OR velocity variance is twice the threshold
@@ -275,11 +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)
+    // 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.get() > 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

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