diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index df6ad0ec9d..c96af46de4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1274,12 +1274,12 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("IMUF_lowpass_pitch", " %d", gyroConfig()->imuf_pitch_lpf_cutoff_hz); BLACKBOX_PRINT_HEADER_LINE("IMUF_lowpass_yaw", " %d", gyroConfig()->imuf_yaw_lpf_cutoff_hz); BLACKBOX_PRINT_HEADER_LINE("IMUF_acc_lpf_cutoff", " %d", gyroConfig()->imuf_acc_lpf_cutoff_hz); + BLACKBOX_PRINT_HEADER_LINE("IMUF_sharpness", " %d", gyroConfig()->imuf_sharpness); #endif BLACKBOX_PRINT_HEADER_LINE("IMUF_roll_q", " %d", gyroConfig()->imuf_roll_q); BLACKBOX_PRINT_HEADER_LINE("IMUF_pitch_q", " %d", gyroConfig()->imuf_pitch_q); BLACKBOX_PRINT_HEADER_LINE("IMUF_yaw_q", " %d", gyroConfig()->imuf_yaw_q); BLACKBOX_PRINT_HEADER_LINE("IMUF_w", " %d", gyroConfig()->imuf_w); - BLACKBOX_PRINT_HEADER_LINE("IMUF_sharpness", " %d", gyroConfig()->imuf_sharpness); BLACKBOX_PRINT_HEADER_LINE("Actual Version Number", "%s %s (%s) %s", FC_FIRMWARE_NAME, FC_VERSION_STRING, shortGitRevision, targetName); default: return true; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index dc99c7a13b..ca2202d90f 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -505,7 +505,6 @@ static uint16_t gyroConfig_imuf_roll_q; static uint16_t gyroConfig_imuf_pitch_q; static uint16_t gyroConfig_imuf_yaw_q; static uint16_t gyroConfig_imuf_w; -static uint16_t gyroConfig_imuf_sharpness; #endif #ifdef USE_SMITH_PREDICTOR static uint8_t smithPredictor_enabled; @@ -538,7 +537,6 @@ static long cmsx_menuGyro_onEnter(void) { gyroConfig_imuf_pitch_q = gyroConfig()->imuf_pitch_q; gyroConfig_imuf_yaw_q = gyroConfig()->imuf_yaw_q; gyroConfig_imuf_w = gyroConfig()->imuf_w; - gyroConfig_imuf_sharpness = gyroConfig()->imuf_sharpness; #endif #ifdef USE_SMITH_PREDICTOR smithPredictor_enabled = gyroConfig()->smithPredictorEnabled; @@ -573,7 +571,6 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { gyroConfigMutable()->imuf_pitch_q = gyroConfig_imuf_pitch_q; gyroConfigMutable()->imuf_yaw_q = gyroConfig_imuf_yaw_q; gyroConfigMutable()->imuf_w = gyroConfig_imuf_w; - gyroConfigMutable()->imuf_sharpness = gyroConfig_imuf_sharpness; #endif #ifdef USE_SMITH_PREDICTOR gyroConfigMutable()->smithPredictorEnabled = smithPredictor_enabled; @@ -604,7 +601,6 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 100, 16000, 100 }, 0 }, { "PITCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_pitch_q, 100, 16000, 100 }, 0 }, { "YAW Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_yaw_q, 100, 16000, 100 }, 0 }, - { "IMUF SHARPNESS", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_sharpness, 0, 16000, 5 }, 0 }, #endif { "GYRO NF1", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_1, 0, 500, 1 }, 0 }, diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c index d9c9d12620..fd030d1475 100644 --- a/src/main/common/kalman.c +++ b/src/main/common/kalman.c @@ -29,13 +29,14 @@ kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT]; void init_kalman(kalman_t *filter, float q) { memset(filter, 0, sizeof(kalman_t)); - filter->q = q * 0.001f; //add multiplier to make tuning easier + filter->q = q * 0.0001f; //add multiplier to make tuning easier filter->r = 88.0f; //seeding R at 88.0f filter->p = 30.0f; //seeding P at 30.0f filter->e = 1.0f; - filter->s = gyroConfig()->imuf_sharpness / 250.0f; //adding the new sharpness :) time to overfilter :O filter->w = gyroConfig()->imuf_w; filter->inverseN = 1.0f / (float)(filter->w); + + pt1FilterInit(&filter->kFilter, pt1FilterGain(50, gyro.targetLooptime * 1e-6f)); } @@ -46,51 +47,46 @@ void kalman_init(void) { init_kalman(&kalmanFilterStateRate[Z], gyroConfig()->imuf_yaw_q); } -void update_kalman_covariance(kalman_t *kalmanState, float rate) { - kalmanState->axisWindow[kalmanState->windex] = rate; - kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex]; - float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean; +void update_kalman_covariance(float rate, int axis) { + kalmanFilterStateRate[axis].axisWindow[kalmanFilterStateRate[axis].windex] = rate; + kalmanFilterStateRate[axis].axisSumMean += kalmanFilterStateRate[axis].axisWindow[kalmanFilterStateRate[axis].windex]; + float varianceElement = kalmanFilterStateRate[axis].axisWindow[kalmanFilterStateRate[axis].windex] - kalmanFilterStateRate[axis].axisMean; varianceElement = varianceElement * varianceElement; - kalmanState->axisSumVar += varianceElement; - kalmanState->varianceWindow[kalmanState->windex] = varianceElement; - kalmanState->windex++; - if (kalmanState->windex >= kalmanState->w) { - kalmanState->windex = 0; + kalmanFilterStateRate[axis].axisSumVar += varianceElement; + kalmanFilterStateRate[axis].varianceWindow[kalmanFilterStateRate[axis].windex] = varianceElement; + kalmanFilterStateRate[axis].windex++; + if (kalmanFilterStateRate[axis].windex > kalmanFilterStateRate[axis].w) { + kalmanFilterStateRate[axis].windex = 0; } - kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex]; - kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex]; + kalmanFilterStateRate[axis].axisSumMean -= kalmanFilterStateRate[axis].axisWindow[kalmanFilterStateRate[axis].windex]; + kalmanFilterStateRate[axis].axisSumVar -= kalmanFilterStateRate[axis].varianceWindow[kalmanFilterStateRate[axis].windex]; //New mean - kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN; - kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN; + kalmanFilterStateRate[axis].axisMean = kalmanFilterStateRate[axis].axisSumMean * kalmanFilterStateRate[axis].inverseN; + kalmanFilterStateRate[axis].axisVar = kalmanFilterStateRate[axis].axisSumVar * kalmanFilterStateRate[axis].inverseN; float squirt; - arm_sqrt_f32(kalmanState->axisVar, &squirt); - kalmanState->r = squirt * VARIANCE_SCALE; + arm_sqrt_f32(kalmanFilterStateRate[axis].axisVar, &squirt); + kalmanFilterStateRate[axis].r = squirt * VARIANCE_SCALE; } FAST_CODE float kalman_process(kalman_t* kalmanState, float input, float target) { + UNUSED(target); //project the state ahead using acceleration - kalmanState->x += (kalmanState->x - kalmanState->lastX); + kalmanState->x += (kalmanState->x - kalmanState->lastX) * kalmanState->k; //figure out how much to boost or reduce our error in the estimate based on setpoint target. //this should be close to 0 as we approach the sepoint and really high the futher away we are from the setpoint. //update last state kalmanState->lastX = kalmanState->x; - if (kalmanState->s != 0.0f) { - float average = fabsf(target + kalmanState->lastX) * 0.5f; - if (average > 10.0f) { - float error = fabsf(target - kalmanState->lastX); - float ratio = error / average; - kalmanState->e = kalmanState->s * powf(ratio, 3.0f); //"ratio" power 3 and multiply by a gain - } - //prediction update - kalmanState->p = kalmanState->p + (kalmanState->q + kalmanState->e); - } else { - if (kalmanState->lastX != 0.0f) { - kalmanState->e = fabsf(1.0f - (target / kalmanState->lastX)); - } - //prediction update - kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e); - } + + float e = constrainf(kalmanState->r / 45.0f + 0.005f, 0.005f, 0.9f); + //make the 1 a configurable value for testing purposes + e = -powf(e - 1.0f, 2) * 0.7f + (e - 1.0f) * (1.0f - 0.7f) + 1.0f; + kalmanState->e = e; + + //prediction update + kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e); //measurement update + kalmanState->k = kalmanState->p / (kalmanState->p + 10.0f); + kalmanState->k = pt1FilterApply(&kalmanState->kFilter, kalmanState->k); kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); kalmanState->x += kalmanState->k * (input - kalmanState->x); kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; @@ -99,7 +95,6 @@ FAST_CODE float kalman_process(kalman_t* kalmanState, float input, float target) FAST_CODE float kalman_update(float input, int axis) { if (gyroConfig()->imuf_w >= 3) { - update_kalman_covariance(&kalmanFilterStateRate[axis], input); input = kalman_process(&kalmanFilterStateRate[axis], input, getSetpointRate(axis) ); } int16_t Kgain = (kalmanFilterStateRate[axis].k * 1000.0f); diff --git a/src/main/common/kalman.h b/src/main/common/kalman.h index 2b4a6394e6..b43c3d0133 100755 --- a/src/main/common/kalman.h +++ b/src/main/common/kalman.h @@ -25,10 +25,10 @@ #define MAX_KALMAN_WINDOW_SIZE 512 -#define VARIANCE_SCALE 0.67f +#define VARIANCE_SCALE 1.0f -typedef struct kalman { +typedef struct kalman_s { float q; //process noise covariance float r; //measurement noise covariance float p; //estimation error covariance matrix @@ -36,17 +36,19 @@ typedef struct kalman { float x; //state float lastX; //previous state float e; - float s; float axisVar; uint16_t windex; - float axisWindow[MAX_KALMAN_WINDOW_SIZE]; - float varianceWindow[MAX_KALMAN_WINDOW_SIZE]; + float axisWindow[MAX_KALMAN_WINDOW_SIZE + 1]; + float varianceWindow[MAX_KALMAN_WINDOW_SIZE + 1]; float axisSumMean; float axisMean; float axisSumVar; float inverseN; uint16_t w; + + pt1Filter_t kFilter; } kalman_t; extern void kalman_init(void); extern float kalman_update(float input, int axis); +extern void update_kalman_covariance(float rate, int axis); diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 477f97de0e..e350bbd22d 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1209,7 +1209,7 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU16(dst, gyroConfig()->imuf_pitch_q); sbufWriteU16(dst, gyroConfig()->imuf_yaw_q); sbufWriteU16(dst, gyroConfig()->imuf_w); - sbufWriteU16(dst, gyroConfig()->imuf_sharpness); + sbufWriteU16(dst, 0); // imuf_sharpness #ifdef USE_GYRO_IMUF9001 sbufWriteU16(dst, gyroConfig()->imuf_roll_lpf_cutoff_hz); sbufWriteU16(dst, gyroConfig()->imuf_pitch_lpf_cutoff_hz); @@ -1792,7 +1792,7 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { gyroConfigMutable()->imuf_pitch_q = sbufReadU16(src); gyroConfigMutable()->imuf_yaw_q = sbufReadU16(src); gyroConfigMutable()->imuf_w = sbufReadU16(src); - gyroConfigMutable()->imuf_sharpness = sbufReadU16(src); + sbufReadU16(src); // imuf_sharpness #ifdef USE_GYRO_IMUF9001 gyroConfigMutable()->imuf_roll_lpf_cutoff_hz = sbufReadU16(src); gyroConfigMutable()->imuf_pitch_lpf_cutoff_hz = sbufReadU16(src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 79ab8441e9..fb80b1273d 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -520,7 +520,6 @@ const clivalue_t valueTable[] = { { "imuf_pitch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_pitch_q) }, { "imuf_yaw_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_yaw_q) }, { "imuf_w", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 512 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) }, - { "imuf_sharpness", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_sharpness) }, #endif #ifdef USE_GYRO_OVERFLOW_CHECK diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 9d7110c820..a98f7ec5bc 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -300,7 +300,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .imuf_pitch_q = 3000, .imuf_yaw_q = 3000, .imuf_w = 32, - .imuf_sharpness = 2500, .gyro_offset_yaw = 0, .yaw_spin_recovery = true, .yaw_spin_threshold = 1950, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 25d76f739b..0d737badde 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -143,12 +143,12 @@ typedef struct gyroConfig_s { uint16_t imuf_roll_lpf_cutoff_hz; uint16_t imuf_yaw_lpf_cutoff_hz; uint16_t imuf_acc_lpf_cutoff_hz; + uint16_t imuf_sharpness; #endif uint16_t imuf_pitch_q; uint16_t imuf_roll_q; uint16_t imuf_yaw_q; uint16_t imuf_w; - uint16_t imuf_sharpness; uint8_t smithPredictorEnabled; uint8_t smithPredictorStrength; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index ec9829ff2f..c0291092d4 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -30,6 +30,11 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) { #endif // DEBUG_GYRO_SCALED records the unfiltered, scaled gyro output GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_SCALED, axis, lrintf(gyroADCf)); + +#ifndef USE_GYRO_IMUF9001 + gyroADCf = kalman_update(gyroADCf, axis); +#endif + // apply static notch filters and software lowpass filters gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); @@ -54,12 +59,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) { } } #endif - - gyroADCf = gyroSensor->gyroABGFilterApplyFn((filter_t *)&gyroSensor->gyroABGFilter[axis], gyroADCf); - -#ifndef USE_GYRO_IMUF9001 - gyroADCf = kalman_update(gyroADCf, axis); -#endif + update_kalman_covariance(gyroADCf, axis); applySmithPredictor(gyroSensor->smithPredictor, gyroADCf);