From 2c63a9497132c26116c88b290341af26277e831e Mon Sep 17 00:00:00 2001 From: Quick-Flash <46289813+Quick-Flash@users.noreply.github.com> Date: Tue, 20 Oct 2020 23:41:05 -0600 Subject: [PATCH] Non dynamic biquad/refactored sharpness (#10) * new sharpness new sharpness nondynamic biquad * update the sharpness code make it compile again. * Update version.h * version as imuf_250 Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com> --- src/board_comm/board_comm.c | 2 +- src/filter/filter.c | 28 ------------------- src/filter/kalman.c | 56 ++++++++++++++++++++++--------------- src/filter/kalman.h | 3 +- src/version.h | 2 +- 5 files changed, 38 insertions(+), 53 deletions(-) diff --git a/src/board_comm/board_comm.c b/src/board_comm/board_comm.c index 50ad96ba..47356cb6 100644 --- a/src/board_comm/board_comm.c +++ b/src/board_comm/board_comm.c @@ -157,7 +157,7 @@ static void run_command(volatile imufCommand_t* command, volatile imufCommand_t* filterConfig.sharpness = (int16_t) ((int16_t)(command->param10 >> 16)); if (!filterConfig.sharpness) { - filterConfig.sharpness = 35; + filterConfig.sharpness = 2500; } filterConfig.acc_lpf_hz = (int16_t)(command->param10 & 0xFFFF); if (!filterConfig.acc_lpf_hz) diff --git a/src/filter/filter.c b/src/filter/filter.c index 578a18d7..ce62747f 100644 --- a/src/filter/filter.c +++ b/src/filter/filter.c @@ -106,37 +106,9 @@ void filter_data(volatile axisData_t *gyroRateData, volatile axisData_t *gyroAcc filteredData->rateData.y = biquad_update(filteredData->rateData.y, &(lpfFilterStateRate.y)); filteredData->rateData.z = biquad_update(filteredData->rateData.z, &(lpfFilterStateRate.z)); -// calculate the error - float errorMultiplierX = ABS(setPoint.x - filteredData->rateData.x) * sharpness; - float errorMultiplierY = ABS(setPoint.y - filteredData->rateData.y) * sharpness; - float errorMultiplierZ = ABS(setPoint.z - filteredData->rateData.z) * sharpness; - -// give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata - - errorMultiplierX = CONSTRAIN(errorMultiplierX * ABS(1.0f - (setPoint.x / filteredData->rateData.x)) + 1.0f, 1.0f, 10.0f); - errorMultiplierY = CONSTRAIN(errorMultiplierY * ABS(1.0f - (setPoint.y / filteredData->rateData.y)) + 1.0f, 1.0f, 10.0f); - errorMultiplierZ = CONSTRAIN(errorMultiplierZ * ABS(1.0f - (setPoint.z / filteredData->rateData.z)) + 1.0f, 1.0f, 10.0f); - - if (setPointNew) { setPointNew = 0; - if (setPoint.x != 0.0f && oldSetPoint.x != setPoint.x) - { - filterConfig.roll_lpf_hz = CONSTRAIN((float)filterConfig.i_roll_lpf_hz * ABS(1.0f - ((setPoint.x * errorMultiplierX) / filteredData->rateData.x)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.roll_lpf_hz, &(lpfFilterStateRate.x)); - } - if (setPoint.y != 0.0f && oldSetPoint.y != setPoint.y) - { - filterConfig.pitch_lpf_hz = CONSTRAIN((float)filterConfig.i_pitch_lpf_hz * ABS(1.0f - ((setPoint.y * errorMultiplierY) / filteredData->rateData.y)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.pitch_lpf_hz, &(lpfFilterStateRate.y)); - } - if (setPoint.z != 0.0f && oldSetPoint.z != setPoint.z) - { - filterConfig.yaw_lpf_hz = CONSTRAIN((float)filterConfig.i_yaw_lpf_hz * ABS(1.0f - ((setPoint.z * errorMultiplierZ) / filteredData->rateData.z)), 10.0f, 500.0f); - filter_biquad_init(filterConfig.yaw_lpf_hz, &(lpfFilterStateRate.z)); - } - memcpy((uint32_t *)&oldSetPoint, (uint32_t *)&setPoint, sizeof(axisData_t)); } //no need to filter ACC is used in quaternions filteredData->accData.x = gyroAccData->x; diff --git a/src/filter/kalman.c b/src/filter/kalman.c index 7389637c..0624f84b 100644 --- a/src/filter/kalman.c +++ b/src/filter/kalman.c @@ -6,22 +6,23 @@ variance_t varStruct; kalman_t kalmanFilterStateRate[3]; -void init_kalman(kalman_t *filter, float q) +void init_kalman(kalman_t *filter, float q, float sharpness) { memset(filter, 0, sizeof(kalman_t)); filter->q = q * 0.001f; //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 = sharpness * 0.01f; } void kalman_init(void) { setPointNew = 0; memset(&varStruct, 0, sizeof(varStruct)); - init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q); - init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q); - init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q); + init_kalman(&kalmanFilterStateRate[ROLL], filterConfig.roll_q, filterConfig.sharpness); + init_kalman(&kalmanFilterStateRate[PITCH], filterConfig.pitch_q, filterConfig.sharpness); + init_kalman(&kalmanFilterStateRate[YAW], filterConfig.yaw_q, filterConfig.sharpness); varStruct.inverseN = 1.0f/filterConfig.w; } @@ -85,28 +86,39 @@ void update_kalman_covariance(volatile axisData_t *gyroRateData) } inline float kalman_process(kalman_t* kalmanState, volatile float input, volatile float target) { - //project the state ahead using acceleration - kalmanState->x += (kalmanState->x - kalmanState->lastX); - - //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 (target != 0.0f) { - kalmanState->e = ABS(1.0f - (target / kalmanState->lastX)); - } else { - kalmanState->e = 1.0f; + //project the state ahead using acceleration + kalmanState->x += (kalmanState->x - kalmanState->lastX); + + //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); - - //measurement update - kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); - kalmanState->x += kalmanState->k * (input - kalmanState->x); - kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; - return kalmanState->x; + } + //measurement update + kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r); + kalmanState->x += kalmanState->k * (input - kalmanState->x); + kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p; + return kalmanState->x; } void kalman_update(volatile axisData_t* input, filteredData_t* output) diff --git a/src/filter/kalman.h b/src/filter/kalman.h index 19e2b8f9..11f8569e 100644 --- a/src/filter/kalman.h +++ b/src/filter/kalman.h @@ -18,7 +18,8 @@ typedef struct kalman float k; //kalman gain float x; //state float lastX; //previous state - float e; + float e; //multiplier or adder to q + float s; //changes how dynamic e is } kalman_t; typedef struct variance diff --git a/src/version.h b/src/version.h index a27cb4bd..ba08b6da 100644 --- a/src/version.h +++ b/src/version.h @@ -3,4 +3,4 @@ #define HARDWARE_VERSION 101 #define BOOTLOADER_VERSION 101 -#define FIRMWARE_VERSION 231 +#define FIRMWARE_VERSION 250