Merge pull request #6902 from iNavFlight/dzikuvx-gyro-alpha-beta-gamma

Alpha-Beta-Gamma filter for gyro

docs/Settings.md

@@ -148,6 +148,9 @@
 | gps_provider | UBLOX |  |  | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). |
 | gps_sbas_mode | NONE |  |  | Which SBAS mode to be used |
 | gps_ublox_use_galileo | OFF |  |  | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. |
+| gyro_abg_alpha | 0 | 0 | 1 | Alpha factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_boost | 0.35 | 0 | 2 | Boost factor for Gyro Alpha-Beta-Gamma filter |
+| gyro_abg_half_life | 0.5 | 0 | 10 | Sample half-life for Gyro Alpha-Beta-Gamma filter |
 | gyro_dyn_lpf_curve_expo | 5 | 1 | 10 | Expo value for the throttle-to-frequency mapping for Dynamic LPF |
 | gyro_dyn_lpf_max_hz | 500 | 40 | 1000 | Maximum frequency of the gyro Dynamic LPF |
 | gyro_dyn_lpf_min_hz | 200 | 40 | 400 | Minimum frequency of the gyro Dynamic LPF |

src/main/build/debug.h

@@ -83,6 +83,7 @@ typedef enum {
     DEBUG_FW_D,
     DEBUG_IMU2,
     DEBUG_ALTITUDE,
-    DEBUG_SMITH_COMPENSATOR,
+    DEBUG_GYRO_ALPHA_BETA_GAMMA,
+    DEBUG_SMITH_PREDICTOR,
     DEBUG_COUNT
 } debugType_e;

src/main/common/filter.c

@@ -250,3 +250,69 @@ FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint
     filter->y1 = y1;
     filter->y2 = y2;
 }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT) {
+    // beta, gamma, and eta gains all derived from
+    // http://yadda.icm.edu.pl/yadda/element/bwmeta1.element.baztech-922ff6cb-e991-417f-93f0-77448f1ef4ec/c/A_Study_Jeong_1_2017.pdf
+
+    const float xi = powf(-alpha + 1.0f, 0.25); // fourth rool of -a + 1
+    filter->xk = 0.0f;
+    filter->vk = 0.0f;
+    filter->ak = 0.0f;
+    filter->jk = 0.0f;
+    filter->a = alpha;
+    filter->b = (1.0f / 6.0f) * powf(1.0f - xi, 2) * (11.0f + 14.0f * xi + 11 * xi * xi);
+    filter->g = 2 * powf(1.0f - xi, 3) * (1 + xi);
+    filter->e = (1.0f / 6.0f) * powf(1 - xi, 4);
+	filter->dT = dT;
+	filter->dT2 = dT * dT;
+    filter->dT3 = dT * dT * dT;
+    pt1FilterInit(&filter->boostFilter, 100, dT);
+
+    const float boost = boostGain * 100;
+
+    filter->boost = (boost * boost / 10000) * 0.003;
+    filter->halfLife = halfLife != 0 ? powf(0.5f, dT / halfLife): 1.0f;
+}
+
+FAST_CODE float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input) {
+    //xk - current system state (ie: position)
+	//vk - derivative of system state (ie: velocity)
+    //ak - derivative of system velociy (ie: acceleration)
+    //jk - derivative of system acceleration (ie: jerk)
+    float rk;   // residual error
+
+    // give the filter limited history
+    filter->xk *= filter->halfLife;
+    filter->vk *= filter->halfLife;
+    filter->ak *= filter->halfLife;
+    filter->jk *= filter->halfLife;
+
+    // update our (estimated) state 'x' from the system (ie pos = pos + vel (last).dT)
+    filter->xk += filter->dT * filter->vk + (1.0f / 2.0f) * filter->dT2 * filter->ak + (1.0f / 6.0f) * filter->dT3 * filter->jk;
+
+    // update (estimated) velocity (also estimated dterm from measurement)
+    filter->vk += filter->dT * filter->ak + 0.5f * filter->dT2 * filter->jk;
+    filter->ak += filter->dT * filter->jk;
+
+    // what is our residual error (measured - estimated)
+    rk = input - filter->xk;
+
+    // artificially boost the error to increase the response of the filter
+    rk += pt1FilterApply(&filter->boostFilter, fabsf(rk) * rk * filter->boost);
+    if ((fabsf(rk * filter->a) > fabsf(input - filter->xk))) {
+        rk = (input - filter->xk) / filter->a;
+    }
+    filter->rk = rk; // for logging
+
+    // update our estimates given the residual error.
+    filter->xk += filter->a * rk;
+    filter->vk += filter->b / filter->dT * rk;
+    filter->ak += filter->g / (2.0f * filter->dT2) * rk;
+    filter->jk += filter->e / (6.0f * filter->dT3) * rk;
+
+	return filter->xk;
+}
+
+#endif

src/main/common/filter.h

@@ -56,6 +56,18 @@ typedef struct firFilter_s {
     uint8_t coeffsLength;
 } firFilter_t;
 
+typedef struct alphaBetaGammaFilter_s {
+    float a, b, g, e;
+    float ak; // derivative of system velociy (ie: acceleration)
+    float vk; // derivative of system state (ie: velocity)
+    float xk; // current system state (ie: position)
+    float jk; // derivative of system acceleration (ie: jerk)
+    float rk; // residual error
+    float dT, dT2, dT3;
+    float halfLife, boost;
+    pt1Filter_t boostFilter;
+} alphaBetaGammaFilter_t;
+
 typedef float (*filterApplyFnPtr)(void *filter, float input);
 typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
 
@@ -86,3 +98,6 @@ float biquadFilterReset(biquadFilter_t *filter, float value);
 float biquadFilterApplyDF1(biquadFilter_t *filter, float input);
 float filterGetNotchQ(float centerFrequencyHz, float cutoffFrequencyHz);
 void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType);
+
+void alphaBetaGammaFilterInit(alphaBetaGammaFilter_t *filter, float alpha, float boostGain, float halfLife, float dT);
+float alphaBetaGammaFilterApply(alphaBetaGammaFilter_t *filter, float input);

src/main/fc/settings.yaml

@@ -94,7 +94,7 @@ tables:
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC",
       "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY",
       "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE",
-      "SMITH_COMPENSATOR"]
+      "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -296,6 +296,27 @@ groups:
         min: 0
         max: 1
         default_value: 0
+      - name: gyro_abg_alpha
+        description: "Alpha factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0
+        field: alphaBetaGammaAlpha
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 1
+      - name: gyro_abg_boost
+        description: "Boost factor for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.35
+        field: alphaBetaGammaBoost
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 2
+      - name: gyro_abg_half_life
+        description: "Sample half-life for Gyro Alpha-Beta-Gamma filter"
+        default_value: 0.5
+        field: alphaBetaGammaHalfLife
+        condition: USE_ALPHA_BETA_GAMMA_FILTER
+        min: 0
+        max: 10
 
   - name: PG_ADC_CHANNEL_CONFIG
     type: adcChannelConfig_t

src/main/flight/pid.c

@@ -1087,9 +1087,9 @@ void FAST_CODE pidController(float dT)
 #endif
 
 #ifdef USE_SMITH_PREDICTOR
-        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis, pidState[axis].gyroRate);
         pidState[axis].gyroRate = applySmithPredictor(axis, &pidState[axis].smithPredictor, pidState[axis].gyroRate);
-        DEBUG_SET(DEBUG_SMITH_COMPENSATOR, axis + 3, pidState[axis].gyroRate);
+        DEBUG_SET(DEBUG_SMITH_PREDICTOR, axis + 3, pidState[axis].gyroRate);
 #endif
 
         DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);

src/main/sensors/gyro.c

@@ -104,7 +104,14 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12);
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+
+STATIC_FASTRAM filterApplyFnPtr abgFilterApplyFn;
+STATIC_FASTRAM alphaBetaGammaFilter_t abgFilter[XYZ_AXIS_COUNT];
+
+#endif
+
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 13);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT,
@@ -130,6 +137,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT,
     .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT,
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    .alphaBetaGammaAlpha = SETTING_GYRO_ABG_ALPHA_DEFAULT,
+    .alphaBetaGammaBoost = SETTING_GYRO_ABG_BOOST_DEFAULT,
+    .alphaBetaGammaHalfLife = SETTING_GYRO_ABG_HALF_LIFE_DEFAULT,
+#endif
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -284,6 +296,24 @@ static void gyroInitFilters(void)
             biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff);
         }
     }
+
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+
+    abgFilterApplyFn = (filterApplyFnPtr)nullFilterApply;
+
+    if (gyroConfig()->alphaBetaGammaAlpha > 0) {
+        abgFilterApplyFn = (filterApplyFnPtr)alphaBetaGammaFilterApply;
+        for (int axis = 0; axis < 3; axis++) {
+            alphaBetaGammaFilterInit(
+                &abgFilter[axis], 
+                gyroConfig()->alphaBetaGammaAlpha, 
+                gyroConfig()->alphaBetaGammaBoost, 
+                gyroConfig()->alphaBetaGammaHalfLife, 
+                getLooptime() * 1e-6f
+            );
+        }
+    } 
+#endif
 }
 
 bool gyroInit(void)
@@ -458,6 +488,12 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf);
         gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
 
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis, gyroADCf);
+        gyroADCf = abgFilterApplyFn(&abgFilter[axis], gyroADCf);
+        DEBUG_SET(DEBUG_GYRO_ALPHA_BETA_GAMMA, axis + 3, gyroADCf);
+#endif
+
 #ifdef USE_DYNAMIC_FILTERS
         if (dynamicGyroNotchState.enabled) {
             gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);

src/main/sensors/gyro.h

@@ -82,6 +82,11 @@ typedef struct gyroConfig_s {
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;
 #endif
+#ifdef USE_ALPHA_BETA_GAMMA_FILTER
+    float alphaBetaGammaAlpha;
+    float alphaBetaGammaBoost;
+    float alphaBetaGammaHalfLife;
+#endif
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/target/common.h

@@ -84,6 +84,7 @@
 #define USE_PITOT_ADC
 #define USE_PITOT_VIRTUAL
 
+#define USE_ALPHA_BETA_GAMMA_FILTER
 #define USE_DYNAMIC_FILTERS
 #define USE_GYRO_KALMAN
 #define USE_SMITH_PREDICTOR