From f91279cb8a6fe718ab4bfd96132729780528c160 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 28 Apr 2021 12:40:28 +0200 Subject: [PATCH 1/2] First cut on Alpha-Beta-Gamma filter for gyro --- src/main/build/debug.h | 1 + src/main/common/filter.c | 66 +++++++++++++++++++++++++++++++++++++++ src/main/common/filter.h | 15 +++++++++ src/main/fc/settings.yaml | 24 +++++++++++++- src/main/sensors/gyro.c | 38 +++++++++++++++++++++- src/main/sensors/gyro.h | 5 +++ src/main/target/common.h | 1 + 7 files changed, 148 insertions(+), 2 deletions(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index e8f47543ebc..dfdb4c9533c 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -83,5 +83,6 @@ typedef enum { DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, + DEBUG_GYRO_ALPHA_BETA_GAMMA, DEBUG_COUNT } debugType_e; diff --git a/src/main/common/filter.c b/src/main/common/filter.c index 3a4c9797efa..0656a5dba78 100644 --- a/src/main/common/filter.c +++ b/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 \ No newline at end of file diff --git a/src/main/common/filter.h b/src/main/common/filter.h index 81c76cd0dc2..a6406ee91c9 100644 --- a/src/main/common/filter.h +++ b/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); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c40ca0d1cbc..34a9f9c9e30 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -90,7 +90,8 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "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"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", + "DEBUG_GYRO_ALPHA_BETA_GAMMA"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -292,6 +293,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 diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0d32b49bfd1..bfc05c2ba48 100644 --- a/src/main/sensors/gyro.c +++ b/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); diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b84477d6863..e2d224d4774 100644 --- a/src/main/sensors/gyro.h +++ b/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); diff --git a/src/main/target/common.h b/src/main/target/common.h index c0310e7da8c..7692dbfa15a 100755 --- a/src/main/target/common.h +++ b/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_EXTENDED_CMS_MENUS From 344de36d4e1f1e9397b26e96d68b827c030b2eb5 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 28 Apr 2021 12:41:25 +0200 Subject: [PATCH 2/2] Docs update --- docs/Settings.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index ec328afa938..0d8eeb7f7ef 100644 --- a/docs/Settings.md +++ b/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 |