From 1621b1c7d21ed94dec1a42dd4f1fbde36fb2b425 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 20 Jan 2022 19:36:18 -0700
Subject: [PATCH 01/10] Add the QuickFlash kalman filter for nice levels of
 smooth gyros :)

---
 make/source.mk                      |   1 +
 src/main/cli/settings.c             |   3 +
 src/main/cms/cms_menu_imu.c         |   8 ++
 src/main/common/kalman.c            | 109 ++++++++++++++++++++++++++++
 src/main/common/kalman.h            |  53 ++++++++++++++
 src/main/sensors/gyro.c             |   2 +
 src/main/sensors/gyro.h             |   5 ++
 src/main/sensors/gyro_filter_impl.c |   2 +
 8 files changed, 183 insertions(+)
 create mode 100644 src/main/common/kalman.c
 create mode 100644 src/main/common/kalman.h

diff --git a/make/source.mk b/make/source.mk
index 3bed09d3e9..a7cad60ea6 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -229,6 +229,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             common/maths.c \
             common/sdft.c \
             common/typeconversion.c \
+            common/kalman.c \
             drivers/accgyro/accgyro_mpu.c \
             drivers/accgyro/accgyro_mpu3050.c \
             drivers/accgyro/accgyro_spi_bmi160.c \
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 819b5ae15c..0565abec07 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -679,6 +679,9 @@ const clivalue_t valueTable[] = {
     { "gyro_calib_duration",        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 50,  3000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroCalibrationDuration) },
     { "gyro_calib_noise_limit",     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
     { "gyro_offset_yaw",            VAR_INT16  | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
+    { "imuf_q",                     VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_q) },
+    { "imuf_w",                     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 65   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) },
+
 #ifdef USE_GYRO_OVERFLOW_CHECK
     { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
 #endif
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index edb0be75b6..f4c9688b55 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -776,6 +776,8 @@ static uint16_t dtermLpfDynMin;
 static uint16_t dtermLpfDynMax;
 static uint8_t dtermLpfDynExpo;
 #endif
+static uint16_t gyroConfig_imuf_q;
+static uint16_t gyroConfig_imuf_w;
 
 static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
 {
@@ -796,6 +798,8 @@ static const void *cmsx_menuDynFilt_onEnter(displayPort_t *pDisp)
     dtermLpfDynMax  = pidProfile->dterm_lpf1_dyn_max_hz;
     dtermLpfDynExpo = pidProfile->dterm_lpf1_dyn_expo;
 #endif
+    gyroConfig_imuf_q = gyroConfig()->imuf_q;
+    gyroConfig_imuf_w = gyroConfig()->imuf_w;
 
     return NULL;
 }
@@ -820,6 +824,8 @@ static const void *cmsx_menuDynFilt_onExit(displayPort_t *pDisp, const OSD_Entry
     pidProfile->dterm_lpf1_dyn_max_hz         = dtermLpfDynMax;
     pidProfile->dterm_lpf1_dyn_expo           = dtermLpfDynExpo;
 #endif
+    gyroConfigMutable()->imuf_q = gyroConfig_imuf_q;
+    gyroConfigMutable()->imuf_w = gyroConfig_imuf_w;
 
     return NULL;
 }
@@ -843,6 +849,8 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
     { "DTERM DLPF MAX",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 } },
     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 #endif
+    { "IMUF W",          OME_UINT8, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w,         0,   64,   1 } },
+    { "IMUF Q",          OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_q,       100, 16000, 100 } },
 
     { "BACK", OME_Back, NULL, NULL },
     { NULL, OME_END, NULL, NULL}
diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c
new file mode 100644
index 0000000000..67af78471e
--- /dev/null
+++ b/src/main/common/kalman.c
@@ -0,0 +1,109 @@
+/*
+ * This file is part of Cleanflight and Betaflight and EmuFlight.
+ *
+ * Cleanflight and Betaflight and EmuFlight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight and EmuFlight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <string.h>
+#include "arm_math.h"
+
+#include "kalman.h"
+#include "fc/rc.h"
+#include "build/debug.h"
+#include "sensors/gyro.h"
+
+static void init_kalman(kalman_t *filter, float q)
+{
+    memset(filter, 0, sizeof(kalman_t));
+    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->w = gyroConfig()->imuf_w;
+    filter->inverseN = 1.0f/(float)(filter->w);
+
+    pt1FilterInit(&filter->kFilter, pt1FilterGain(50, gyro.sampleLooptime * 1e-6f));
+}
+
+
+void kalman_init(void)
+{
+    init_kalman(&gyro.kalmanFilterStateRate[X],  gyroConfig()->imuf_q);
+    init_kalman(&gyro.kalmanFilterStateRate[Y],  gyroConfig()->imuf_q);
+    init_kalman(&gyro.kalmanFilterStateRate[Z],  gyroConfig()->imuf_q);
+}
+
+void update_kalman_covariance(kalman_t *kalmanState, float rate) {
+  if (gyroConfig()->imuf_w >= 3) {
+    kalmanState->axisWindow[kalmanState->windex] = rate;
+    kalmanState->axisSumMean += kalmanState->axisWindow[kalmanState->windex];
+
+    float varianceElement = kalmanState->axisWindow[kalmanState->windex] - kalmanState->axisMean;
+    varianceElement = varianceElement * varianceElement;
+    kalmanState->axisSumVar += varianceElement;
+    kalmanState->varianceWindow[kalmanState->windex] = varianceElement;
+    kalmanState->windex++;
+
+    if (kalmanState->windex > kalmanState->w) {
+        kalmanState->windex = 0;
+    }
+
+    kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
+    kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];
+
+    //New mean
+    kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
+    kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;
+
+    float squirt;
+    arm_sqrt_f32(kalmanState->axisVar, &squirt);
+    kalmanState->r = squirt * VARIANCE_SCALE;
+  }
+}
+
+FAST_CODE float kalman_process(kalman_t* kalmanState, float input)
+{
+  //project the state ahead using acceleration
+  kalmanState->x += (kalmanState->x - kalmanState->lastX) * kalmanState->k;
+
+  kalmanState->lastX = kalmanState->x;
+
+  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->x += kalmanState->k * (input - kalmanState->x);
+  kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
+
+  return kalmanState->x;
+}
+
+FAST_CODE float kalman_update(float input, int axis)
+{
+ if (gyroConfig()->imuf_w >= 3) {
+    input = kalman_process(&gyro.kalmanFilterStateRate[axis], input);
+ }
+    return input;
+}
\ No newline at end of file
diff --git a/src/main/common/kalman.h b/src/main/common/kalman.h
new file mode 100644
index 0000000000..3c908d7282
--- /dev/null
+++ b/src/main/common/kalman.h
@@ -0,0 +1,53 @@
+/*
+ * This file is part of Cleanflight and Betaflight and EmuFlight.
+ *
+ * Cleanflight and Betaflight and EmuFlight are free software. You can redistribute
+ * this software and/or modify this software under the terms of the
+ * GNU General Public License as published by the Free Software
+ * Foundation, either version 3 of the License, or (at your option)
+ * any later version.
+ *
+ * Cleanflight and Betaflight and EmuFlight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "filter.h"
+
+#define MAX_KALMAN_WINDOW_SIZE 65
+
+#define VARIANCE_SCALE 1.0f
+
+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 axisVar;
+    uint16_t windex;
+    float axisWindow[MAX_KALMAN_WINDOW_SIZE];
+    float varianceWindow[MAX_KALMAN_WINDOW_SIZE];
+    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);
+void update_kalman_covariance(kalman_t *kalmanState, float rate);
\ No newline at end of file
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index abccba6d9b..c4c0c8254c 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -133,6 +133,8 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyro_lpf1_dyn_expo = 5;
     gyroConfig->simplified_gyro_filter = true;
     gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
+    gyroConfig->imuf_w = 32;
+    gyroConfig->imuf_q = 10000;
 }
 
 FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 5ff3eb8889..86a7507493 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -24,6 +24,7 @@
 #include "common/filter.h"
 #include "common/time.h"
 #include "common/utils.h"
+#include "common/kalman.h"
 
 #include "drivers/accgyro/accgyro.h"
 #include "drivers/bus.h"
@@ -110,6 +111,8 @@ typedef struct gyro_s {
     filterApplyFnPtr notchFilter2ApplyFn;
     biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
 
+    kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
+
     uint16_t accSampleRateHz;
     uint8_t gyroToUse;
     uint8_t gyroDebugMode;
@@ -196,6 +199,8 @@ typedef struct gyroConfig_s {
     uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
     uint8_t simplified_gyro_filter;
     uint8_t simplified_gyro_filter_multiplier;
+    uint8_t imuf_w;
+    uint16_t imuf_q;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);
diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 58d7006ba2..0e91771c0a 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -49,6 +49,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
         GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
 
+        gyroADCf = kalman_update(gyroADCf, axis);
 #ifdef USE_RPM_FILTER
         gyroADCf = rpmFilterGyro(axis, gyroADCf);
 #endif
@@ -81,6 +82,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
             }
         }
 #endif
+        update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
 
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));

From 86e829d100a3ad36b9193458f4d47846b90f52ad Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Fri, 21 Jan 2022 08:55:08 -0700
Subject: [PATCH 02/10] add kalman init

---
 src/main/sensors/gyro_init.c | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c
index 0d434a9a89..3699f1011c 100644
--- a/src/main/sensors/gyro_init.c
+++ b/src/main/sensors/gyro_init.c
@@ -266,6 +266,7 @@ void gyroInitFilters(void)
 #ifdef USE_DYN_NOTCH_FILTER
     dynNotchInit(dynNotchConfig(), gyro.targetLooptime);
 #endif
+    kalman_init();
 }
 
 #if defined(USE_GYRO_SLEW_LIMITER)

From 44fec1fb431c8aee83ef34059bd8667cef9062e2 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Mon, 28 Feb 2022 09:51:42 -0700
Subject: [PATCH 03/10] debugging

add proper blackbox debugging
---
 src/main/build/debug.c              | 3 ++-
 src/main/build/debug.h              | 1 +
 src/main/sensors/gyro_filter_impl.c | 9 ++++++++-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/main/build/debug.c b/src/main/build/debug.c
index d3ae5fb3f4..f69c3966b1 100644
--- a/src/main/build/debug.c
+++ b/src/main/build/debug.c
@@ -99,5 +99,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "TIMING_ACCURACY",
     "RX_EXPRESSLRS_SPI",
     "RX_EXPRESSLRS_PHASELOCK",
-    "RX_STATE_TIME"
+    "RX_STATE_TIME",
+    "KALMAN",
 };
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index 8494550257..583f7cd5eb 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -98,6 +98,7 @@ typedef enum {
     DEBUG_RX_EXPRESSLRS_SPI,
     DEBUG_RX_EXPRESSLRS_PHASELOCK,
     DEBUG_RX_STATE_TIME,
+    DEBUG_KALMAN,
     DEBUG_COUNT
 } debugType_e;
 
diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 0e91771c0a..5e4c3ac568 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -46,10 +46,11 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
             gyro.sampleSum[axis] = 0;
         }
 
+        gyroADCf = kalman_update(gyroADCf, axis);
+
         // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
         GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
 
-        gyroADCf = kalman_update(gyroADCf, axis);
 #ifdef USE_RPM_FILTER
         gyroADCf = rpmFilterGyro(axis, gyroADCf);
 #endif
@@ -82,7 +83,13 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
             }
         }
 #endif
+        if (axis != FD_YAW) {
+            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis, lrintf(gyroADCf));
+        }
         update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
+        if (axis != FD_YAW) {
+            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis+2, lrintf(gyroADCf));
+        }
 
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));

From 3e0dcbb2048986d94607dea4fbea53ad31024563 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Mon, 28 Feb 2022 10:13:46 -0700
Subject: [PATCH 04/10] fix max imuf w

---
 src/main/cli/settings.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 0565abec07..20296d6721 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -680,7 +680,7 @@ const clivalue_t valueTable[] = {
     { "gyro_calib_noise_limit",     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
     { "gyro_offset_yaw",            VAR_INT16  | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
     { "imuf_q",                     VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_q) },
-    { "imuf_w",                     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 65   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) },
+    { "imuf_w",                     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 64   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) },
 
 #ifdef USE_GYRO_OVERFLOW_CHECK
     { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },

From 2a3263a6167c814caa38734a0700499e40668ccc Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Mon, 28 Feb 2022 18:37:45 -0700
Subject: [PATCH 05/10] Update kalman.c

---
 src/main/common/kalman.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c
index 67af78471e..397e6c99ea 100644
--- a/src/main/common/kalman.c
+++ b/src/main/common/kalman.c
@@ -22,7 +22,6 @@
 #include "arm_math.h"
 
 #include "kalman.h"
-#include "fc/rc.h"
 #include "build/debug.h"
 #include "sensors/gyro.h"
 
@@ -106,4 +105,4 @@ FAST_CODE float kalman_update(float input, int axis)
     input = kalman_process(&gyro.kalmanFilterStateRate[axis], input);
  }
     return input;
-}
\ No newline at end of file
+}

From 7e536bf7bbc17e19a7a6df55bd8d70fd866221ec Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Mon, 28 Feb 2022 18:38:10 -0700
Subject: [PATCH 06/10] Update kalman.h

---
 src/main/common/kalman.h | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/common/kalman.h b/src/main/common/kalman.h
index 3c908d7282..2db7cd1386 100644
--- a/src/main/common/kalman.h
+++ b/src/main/common/kalman.h
@@ -36,18 +36,18 @@ typedef struct kalman_s
     float lastX; //previous state
     float e;
     float axisVar;
-    uint16_t windex;
+    uint8_t windex;
     float axisWindow[MAX_KALMAN_WINDOW_SIZE];
     float varianceWindow[MAX_KALMAN_WINDOW_SIZE];
     float axisSumMean;
     float axisMean;
     float axisSumVar;
     float inverseN;
-    uint16_t w;
+    uint8_t w;
 
     pt1Filter_t kFilter;
 } kalman_t;
 
 extern void kalman_init(void);
 extern float kalman_update(float input, int axis);
-void update_kalman_covariance(kalman_t *kalmanState, float rate);
\ No newline at end of file
+void update_kalman_covariance(kalman_t *kalmanState, float rate);

From e284cc21dd917e1d9c75099f223c2d556d97b1ee Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 3 Mar 2022 17:39:49 -0700
Subject: [PATCH 07/10] fix kalman debugging

---
 src/main/sensors/gyro_filter_impl.c | 13 +++++++------
 src/main/sensors/gyro_init.c        |  1 +
 src/main/target/YUPIF4/target.h     |  1 -
 3 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 5e4c3ac568..93ab368e99 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -46,7 +46,13 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
             gyro.sampleSum[axis] = 0;
         }
 
+        if (axis != FD_YAW) {
+            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis, lrintf(gyroADCf));
+        }
         gyroADCf = kalman_update(gyroADCf, axis);
+        if (axis != FD_YAW) {
+            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis+2, lrintf(gyroADCf));
+        }
 
         // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis
         GYRO_FILTER_AXIS_DEBUG_SET(axis, DEBUG_GYRO_SAMPLE, 1, lrintf(gyroADCf));
@@ -83,13 +89,8 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
             }
         }
 #endif
-        if (axis != FD_YAW) {
-            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis, lrintf(gyroADCf));
-        }
+
         update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
-        if (axis != FD_YAW) {
-            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis+2, lrintf(gyroADCf));
-        }
 
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));
diff --git a/src/main/sensors/gyro_init.c b/src/main/sensors/gyro_init.c
index 3699f1011c..017a1a64f8 100644
--- a/src/main/sensors/gyro_init.c
+++ b/src/main/sensors/gyro_init.c
@@ -581,6 +581,7 @@ bool gyroInit(void)
     case DEBUG_GYRO_FILTERED:
     case DEBUG_DYN_LPF:
     case DEBUG_GYRO_SAMPLE:
+    case DEBUG_KALMAN:
         gyro.gyroDebugMode = debugMode;
         break;
     case DEBUG_DUAL_GYRO_DIFF:
diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h
index 97f3b5908f..62ed74dbd1 100644
--- a/src/main/target/YUPIF4/target.h
+++ b/src/main/target/YUPIF4/target.h
@@ -123,7 +123,6 @@
 #define SERIALRX_PROVIDER       SERIALRX_SBUS
 #define SERIALRX_UART           SERIAL_PORT_USART6
 #define DEFAULT_FEATURES        (FEATURE_OSD)
-#define USE_GYRO_FAST_KALMAN
 
 // Target IO and timers
 #define TARGET_IO_PORTA         0xffff

From 4b6ecb8d84947b40f3d1170e4e28b447693cbcb9 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Fri, 4 Mar 2022 19:39:37 -0700
Subject: [PATCH 08/10] separate noise from movement

requires testing, but looks good :)
---
 src/main/cli/settings.c             |  2 +-
 src/main/cms/cms_menu_imu.c         |  3 +-
 src/main/common/kalman.c            | 54 +++++++++++++++++++++++++++--
 src/main/common/kalman.h            |  9 +++--
 src/main/sensors/gyro_filter_impl.c |  8 ++++-
 5 files changed, 67 insertions(+), 9 deletions(-)

diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 20296d6721..e538ff9bb5 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -680,7 +680,7 @@ const clivalue_t valueTable[] = {
     { "gyro_calib_noise_limit",     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0,  200 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyroMovementCalibrationThreshold) },
     { "gyro_offset_yaw",            VAR_INT16  | MASTER_VALUE, .config.minmax = { -1000, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_offset_yaw) },
     { "imuf_q",                     VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1000, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_q) },
-    { "imuf_w",                     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, 64   }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) },
+    { "imuf_w",                     VAR_UINT8  | MASTER_VALUE, .config.minmaxUnsigned = { 0, MAX_KALMAN_WINDOW_SIZE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, imuf_w) },
 
 #ifdef USE_GYRO_OVERFLOW_CHECK
     { "gyro_overflow_detect",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO_OVERFLOW_CHECK }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, checkOverflow) },
diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c
index f4c9688b55..bb0df59baa 100644
--- a/src/main/cms/cms_menu_imu.c
+++ b/src/main/cms/cms_menu_imu.c
@@ -38,6 +38,7 @@
 #include "cms/cms_menu_imu.h"
 
 #include "common/utils.h"
+#include "common/kalman.h"
 
 #include "config/feature.h"
 #include "config/simplified_tuning.h"
@@ -849,7 +850,7 @@ static const OSD_Entry cmsx_menuDynFiltEntries[] =
     { "DTERM DLPF MAX",  OME_UINT16 | SLIDER_DTERM, NULL, &(OSD_UINT16_t) { &dtermLpfDynMax, 0, 1000, 1 } },
     { "DTERM DLPF EXPO", OME_UINT8, NULL, &(OSD_UINT8_t) { &dtermLpfDynExpo,  0, 10, 1 } },
 #endif
-    { "IMUF W",          OME_UINT8, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w,         0,   64,   1 } },
+    { "IMUF W",          OME_UINT8, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w,         0,   MAX_KALMAN_WINDOW_SIZE, 1 } },
     { "IMUF Q",          OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_q,       100, 16000, 100 } },
 
     { "BACK", OME_Back, NULL, NULL },
diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c
index 397e6c99ea..c7186b07fc 100644
--- a/src/main/common/kalman.c
+++ b/src/main/common/kalman.c
@@ -33,7 +33,15 @@ static void init_kalman(kalman_t *filter, float q)
     filter->p = 30.0f;                  //seeding P at 30.0f
     filter->e = 1.0f;
     filter->w = gyroConfig()->imuf_w;
-    filter->inverseN = 1.0f/(float)(filter->w);
+    filter->inverseN = 1.0f / filter->w;
+
+    for (int i = 0; i < filter->w; i++) {
+      float currentX = 1 * (i + 1.0);
+      filter->sumX += currentX;
+      filter->sumXSquared += currentX * currentX;
+    }
+    filter->sumXSquared = filter->sumXSquared * filter->w;
+    filter->squaredSumX = filter->sumX * filter->sumX;
 
     pt1FilterInit(&filter->kFilter, pt1FilterGain(50, gyro.sampleLooptime * 1e-6f));
 }
@@ -46,6 +54,7 @@ void kalman_init(void)
     init_kalman(&gyro.kalmanFilterStateRate[Z],  gyroConfig()->imuf_q);
 }
 
+// may not need to be updated at pidloop speed, updating this at lower rates might be viable and desireable
 void update_kalman_covariance(kalman_t *kalmanState, float rate) {
   if (gyroConfig()->imuf_w >= 3) {
     kalmanState->axisWindow[kalmanState->windex] = rate;
@@ -61,6 +70,39 @@ void update_kalman_covariance(kalman_t *kalmanState, float rate) {
         kalmanState->windex = 0;
     }
 
+    // least squares regression line
+    // look at a recursive formula
+    float sumXY = 0.0f;
+    float sumY = 0.0f;
+    int tempPointer = kalmanState->windex;
+    for (int i = 0; i < kalmanState->w; i++) {
+        float currentX = 1 * (i + 1.0);
+        sumXY += currentX * kalmanState->axisWindow[tempPointer];
+        sumY += kalmanState->axisWindow[tempPointer];
+        tempPointer++;
+        if (tempPointer > kalmanState->w) {
+            tempPointer = 0;
+        }
+    }
+
+    tempPointer = kalmanState->windex;
+    float m = (kalmanState->w * sumXY - kalmanState->sumX * sumY) / (kalmanState->sumXSquared - kalmanState->squaredSumX);
+    float b = (sumY - m * kalmanState->sumX) * kalmanState->inverseN;
+
+    // calculate variance against the curve
+    float variance = 0.0f;
+    for (int i = 0; i < kalmanState->w; i++) {
+        float currentX = 1 * (i + 1.0);
+        float pointOnLine = m * currentX + b;
+        float error = kalmanState->axisWindow[tempPointer] - pointOnLine;
+        variance += error * error;
+
+        tempPointer++;
+        if (tempPointer > kalmanState->w) {
+            tempPointer = 0;
+        }
+    }
+
     kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
     kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];
 
@@ -68,8 +110,14 @@ void update_kalman_covariance(kalman_t *kalmanState, float rate) {
     kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
     kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;
 
+    // compare the variance with the least squares variance
+    // this tells us how much of our variance is movement, vs noise
+    // the more noise we have the less we should boost Q
+    // our prediction is that the more filtering we have the less
+    // we can trust our prediction
+    float varianceDifference = MAX(kalmanState->axisVar - variance, 0.0);
     float squirt;
-    arm_sqrt_f32(kalmanState->axisVar, &squirt);
+    arm_sqrt_f32(varianceDifference, &squirt);
     kalmanState->r = squirt * VARIANCE_SCALE;
   }
 }
@@ -81,7 +129,7 @@ FAST_CODE float kalman_process(kalman_t* kalmanState, float input)
 
   kalmanState->lastX = kalmanState->x;
 
-  float e = constrainf(kalmanState->r / 45.0f + 0.005f, 0.005f, 0.9f);
+  float e = constrainf(kalmanState->r / 45.0f + 0.005f, 0.005f, 0.95f);
   //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;
diff --git a/src/main/common/kalman.h b/src/main/common/kalman.h
index 2db7cd1386..aeac5472c2 100644
--- a/src/main/common/kalman.h
+++ b/src/main/common/kalman.h
@@ -22,7 +22,7 @@
 
 #include "filter.h"
 
-#define MAX_KALMAN_WINDOW_SIZE 65
+#define MAX_KALMAN_WINDOW_SIZE 64
 
 #define VARIANCE_SCALE 1.0f
 
@@ -37,13 +37,16 @@ typedef struct kalman_s
     float e;
     float axisVar;
     uint8_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;
     uint8_t w;
+    float sumX;
+    float sumXSquared;
+    float squaredSumX;
 
     pt1Filter_t kFilter;
 } kalman_t;
diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 93ab368e99..abe414d947 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -90,7 +90,13 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         }
 #endif
 
-        update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
+        // downsample updating the kalman covariance to be a third of pidloop rate
+        static int covarianceDownSampler = 1;
+        if (covarianceDownSampler == 3) {
+            update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
+            covarianceDownSampler = 0;
+        }
+            covarianceDownSampler += 1;
 
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));

From 1bf967510a49db605c93db3b19b7062658a187c1 Mon Sep 17 00:00:00 2001
From: Kevin Plaizier <46289813+Quick-Flash@users.noreply.github.com>
Date: Thu, 17 Mar 2022 16:28:59 -0600
Subject: [PATCH 09/10] revert bad kalman update

---
 src/main/common/kalman.c            | 50 ++---------------------------
 src/main/common/kalman.h            |  3 --
 src/main/sensors/gyro_filter_impl.c |  9 ++----
 3 files changed, 4 insertions(+), 58 deletions(-)

diff --git a/src/main/common/kalman.c b/src/main/common/kalman.c
index c7186b07fc..c22f59f9c1 100644
--- a/src/main/common/kalman.c
+++ b/src/main/common/kalman.c
@@ -35,14 +35,6 @@ static void init_kalman(kalman_t *filter, float q)
     filter->w = gyroConfig()->imuf_w;
     filter->inverseN = 1.0f / filter->w;
 
-    for (int i = 0; i < filter->w; i++) {
-      float currentX = 1 * (i + 1.0);
-      filter->sumX += currentX;
-      filter->sumXSquared += currentX * currentX;
-    }
-    filter->sumXSquared = filter->sumXSquared * filter->w;
-    filter->squaredSumX = filter->sumX * filter->sumX;
-
     pt1FilterInit(&filter->kFilter, pt1FilterGain(50, gyro.sampleLooptime * 1e-6f));
 }
 
@@ -70,39 +62,6 @@ void update_kalman_covariance(kalman_t *kalmanState, float rate) {
         kalmanState->windex = 0;
     }
 
-    // least squares regression line
-    // look at a recursive formula
-    float sumXY = 0.0f;
-    float sumY = 0.0f;
-    int tempPointer = kalmanState->windex;
-    for (int i = 0; i < kalmanState->w; i++) {
-        float currentX = 1 * (i + 1.0);
-        sumXY += currentX * kalmanState->axisWindow[tempPointer];
-        sumY += kalmanState->axisWindow[tempPointer];
-        tempPointer++;
-        if (tempPointer > kalmanState->w) {
-            tempPointer = 0;
-        }
-    }
-
-    tempPointer = kalmanState->windex;
-    float m = (kalmanState->w * sumXY - kalmanState->sumX * sumY) / (kalmanState->sumXSquared - kalmanState->squaredSumX);
-    float b = (sumY - m * kalmanState->sumX) * kalmanState->inverseN;
-
-    // calculate variance against the curve
-    float variance = 0.0f;
-    for (int i = 0; i < kalmanState->w; i++) {
-        float currentX = 1 * (i + 1.0);
-        float pointOnLine = m * currentX + b;
-        float error = kalmanState->axisWindow[tempPointer] - pointOnLine;
-        variance += error * error;
-
-        tempPointer++;
-        if (tempPointer > kalmanState->w) {
-            tempPointer = 0;
-        }
-    }
-
     kalmanState->axisSumMean -= kalmanState->axisWindow[kalmanState->windex];
     kalmanState->axisSumVar -= kalmanState->varianceWindow[kalmanState->windex];
 
@@ -110,14 +69,8 @@ void update_kalman_covariance(kalman_t *kalmanState, float rate) {
     kalmanState->axisMean = kalmanState->axisSumMean * kalmanState->inverseN;
     kalmanState->axisVar = kalmanState->axisSumVar * kalmanState->inverseN;
 
-    // compare the variance with the least squares variance
-    // this tells us how much of our variance is movement, vs noise
-    // the more noise we have the less we should boost Q
-    // our prediction is that the more filtering we have the less
-    // we can trust our prediction
-    float varianceDifference = MAX(kalmanState->axisVar - variance, 0.0);
     float squirt;
-    arm_sqrt_f32(varianceDifference, &squirt);
+    arm_sqrt_f32(kalmanState->axisVar, &squirt);
     kalmanState->r = squirt * VARIANCE_SCALE;
   }
 }
@@ -129,6 +82,7 @@ FAST_CODE float kalman_process(kalman_t* kalmanState, float input)
 
   kalmanState->lastX = kalmanState->x;
 
+  // look at changing this range
   float e = constrainf(kalmanState->r / 45.0f + 0.005f, 0.005f, 0.95f);
   //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;
diff --git a/src/main/common/kalman.h b/src/main/common/kalman.h
index aeac5472c2..ae866da0e8 100644
--- a/src/main/common/kalman.h
+++ b/src/main/common/kalman.h
@@ -44,9 +44,6 @@ typedef struct kalman_s
     float axisSumVar;
     float inverseN;
     uint8_t w;
-    float sumX;
-    float sumXSquared;
-    float squaredSumX;
 
     pt1Filter_t kFilter;
 } kalman_t;
diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index abe414d947..9124cc1679 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -90,13 +90,8 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         }
 #endif
 
-        // downsample updating the kalman covariance to be a third of pidloop rate
-        static int covarianceDownSampler = 1;
-        if (covarianceDownSampler == 3) {
-            update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
-            covarianceDownSampler = 0;
-        }
-            covarianceDownSampler += 1;
+        update_kalman_covariance(&gyro.kalmanFilterStateRate[axis], gyroADCf);
+
 
         // DEBUG_GYRO_FILTERED records the scaled, filtered, after all software filtering has been applied.
         GYRO_FILTER_DEBUG_SET(DEBUG_GYRO_FILTERED, axis, lrintf(gyroADCf));

From 1683b887d5e6e3629f2736a5235a8e358366edf6 Mon Sep 17 00:00:00 2001
From: nerdCopter <56646290+nerdCopter@users.noreply.github.com>
Date: Wed, 25 May 2022 15:25:21 -0500
Subject: [PATCH 10/10] kalman - fix 'error: void value not ignored as it ought
 to be'

---
 src/main/sensors/gyro_filter_impl.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/main/sensors/gyro_filter_impl.c b/src/main/sensors/gyro_filter_impl.c
index 9124cc1679..85b39430f3 100644
--- a/src/main/sensors/gyro_filter_impl.c
+++ b/src/main/sensors/gyro_filter_impl.c
@@ -51,7 +51,7 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(void)
         }
         gyroADCf = kalman_update(gyroADCf, axis);
         if (axis != FD_YAW) {
-            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, axis+2, lrintf(gyroADCf));
+            GYRO_FILTER_DEBUG_SET(DEBUG_KALMAN, (axis+2), lrintf(gyroADCf));
         }
 
         // DEBUG_GYRO_SAMPLE(1) Record the post-downsample value for the selected debug axis