update the kalman filter (#541)

* update the kalman filter * Update gyro_filter_impl.h * Update kalman.c Co-authored-by: nerdCopter <[email protected]>
emuflight · Jun 21, 2021 · 07af11e · 07af11e
1 parent e0bd49c
commit 07af11e
Show file tree

Hide file tree

Showing 9 changed files with 47 additions and 56 deletions.
diff --git 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
@@ -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
@@ -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
@@ -25,28 +25,30 @@
 
 #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
     float k;     //kalman gain
     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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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);