From dd8fef1e20994ff9a8f05e36f70c540f2513f9c2 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 20 Mar 2020 15:18:17 +0100
Subject: [PATCH 01/11] Direct copy from EmuFlight, compiles, does not wrk

---
 make/source.mk            |   1 +
 src/main/build/debug.h    |   1 +
 src/main/fc/settings.yaml |   3 +-
 src/main/flight/kalman.c  | 165 ++++++++++++++++++++++++++++++++++++++
 src/main/flight/kalman.h  |  76 ++++++++++++++++++
 src/main/target/common.h  |   1 +
 6 files changed, 246 insertions(+), 1 deletion(-)
 create mode 100755 src/main/flight/kalman.c
 create mode 100755 src/main/flight/kalman.h

diff --git a/make/source.mk b/make/source.mk
index 50b10f84193..85912779dcf 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -99,6 +99,7 @@ COMMON_SRC = \
             flight/gyroanalyse.c \
             flight/rpm_filter.c \
             flight/dynamic_gyro_notch.c \
+            flight/kalman.c \
             io/beeper.c \
             io/esc_serialshot.c \
             io/frsky_osd.c \
diff --git a/src/main/build/debug.h b/src/main/build/debug.h
index a300b70ade6..c76fa7fc5f2 100644
--- a/src/main/build/debug.h
+++ b/src/main/build/debug.h
@@ -70,5 +70,6 @@ typedef enum {
     DEBUG_NAV_YAW,
     DEBUG_DYNAMIC_FILTER,
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
+    DEBUG_KALMAN,
     DEBUG_COUNT
 } debugType_e;
diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 464bf02e0b1..5db22bd9d4f 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -81,7 +81,8 @@ tables:
     values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
       "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
       "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
-      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY"]
+      "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", 
+      "KALMAN"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
new file mode 100755
index 00000000000..f60a2ff987e
--- /dev/null
+++ b/src/main/flight/kalman.c
@@ -0,0 +1,165 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight 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 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 "platform.h"
+#ifdef USE_GYRO_KALMAN
+
+FILE_COMPILE_FOR_SPEED
+
+#include <string.h>
+#include "arm_math.h"
+
+#include "kalman.h"
+#include "build/debug.h"
+
+kalman_t    kalmanFilterStateRate[XYZ_AXIS_COUNT];
+variance_t  varStruct;
+float       setPoint[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->r = 88.0f;                  //seeding R at 88.0f
+    filter->p = 30.0f;                  //seeding P at 30.0f
+    filter->e = 1.0f;
+    //FIXME hardcoded config
+    filter->s = 2500 / 250.0f;     //adding the new sharpness :) time to overfilter :O
+}
+
+
+void kalman_init(void)
+{
+    // isSetpointNew = 0;
+
+    memset(&varStruct, 0, sizeof(varStruct));
+    //FIXME values hardcoded for now
+    init_kalman(&kalmanFilterStateRate[X],  3000);
+    init_kalman(&kalmanFilterStateRate[Y],  3000);
+    init_kalman(&kalmanFilterStateRate[Z],  3000);
+
+    //FIXME values hardcoded for now
+    varStruct.w = 32;
+    varStruct.inverseN = 1.0f/(float)(varStruct.w);
+}
+
+void update_kalman_covariance(float *gyroRateData)
+{
+     varStruct.xWindow[ varStruct.windex] = gyroRateData[X];
+     varStruct.yWindow[ varStruct.windex] = gyroRateData[Y];
+     varStruct.zWindow[ varStruct.windex] = gyroRateData[Z];
+
+     varStruct.xSumMean +=  varStruct.xWindow[ varStruct.windex];
+     varStruct.ySumMean +=  varStruct.yWindow[ varStruct.windex];
+     varStruct.zSumMean +=  varStruct.zWindow[ varStruct.windex];
+     varStruct.xSumVar =  varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.xWindow[ varStruct.windex]);
+     varStruct.ySumVar =  varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
+     varStruct.zSumVar =  varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+     varStruct.xySumCoVar =  varStruct.xySumCoVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
+     varStruct.xzSumCoVar =  varStruct.xzSumCoVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+     varStruct.yzSumCoVar =  varStruct.yzSumCoVar + ( varStruct.yWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+     varStruct.windex++;
+    if ( varStruct.windex >= varStruct.w)
+    {
+         varStruct.windex = 0;
+    }
+     varStruct.xSumMean -=  varStruct.xWindow[ varStruct.windex];
+     varStruct.ySumMean -=  varStruct.yWindow[ varStruct.windex];
+     varStruct.zSumMean -=  varStruct.zWindow[ varStruct.windex];
+     varStruct.xSumVar =  varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.xWindow[ varStruct.windex]);
+     varStruct.ySumVar =  varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
+     varStruct.zSumVar =  varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+     varStruct.xySumCoVar =  varStruct.xySumCoVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
+     varStruct.xzSumCoVar =  varStruct.xzSumCoVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+     varStruct.yzSumCoVar =  varStruct.yzSumCoVar - ( varStruct.yWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
+
+     varStruct.xMean =  varStruct.xSumMean *  varStruct.inverseN;
+     varStruct.yMean =  varStruct.ySumMean *  varStruct.inverseN;
+     varStruct.zMean =  varStruct.zSumMean *  varStruct.inverseN;
+
+     varStruct.xVar =  fabsf(varStruct.xSumVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.xMean));
+     varStruct.yVar =  fabsf(varStruct.ySumVar *  varStruct.inverseN - ( varStruct.yMean *  varStruct.yMean));
+     varStruct.zVar =  fabsf(varStruct.zSumVar *  varStruct.inverseN - ( varStruct.zMean *  varStruct.zMean));
+     varStruct.xyCoVar =  fabsf(varStruct.xySumCoVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.yMean));
+     varStruct.xzCoVar =  fabsf(varStruct.xzSumCoVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.zMean));
+     varStruct.yzCoVar =  fabsf(varStruct.yzSumCoVar *  varStruct.inverseN - ( varStruct.yMean *  varStruct.zMean));
+
+    float squirt;
+    arm_sqrt_f32(varStruct.xVar +  varStruct.xyCoVar +  varStruct.xzCoVar, &squirt);
+    kalmanFilterStateRate[X].r = squirt * VARIANCE_SCALE;
+
+    arm_sqrt_f32(varStruct.yVar +  varStruct.xyCoVar +  varStruct.yzCoVar, &squirt);
+    kalmanFilterStateRate[Y].r = squirt * VARIANCE_SCALE;
+
+    arm_sqrt_f32(varStruct.zVar +  varStruct.yzCoVar +  varStruct.xzCoVar, &squirt);
+    kalmanFilterStateRate[Z].r = squirt * VARIANCE_SCALE;
+}
+
+FAST_CODE float kalman_process(kalman_t* kalmanState, float input, 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;
+
+  // calculate the error
+  	float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
+
+  // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
+
+  	errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
+
+  if (target != 0.0f) {
+      kalmanState->e = fabsf(1.0f - ((target * errorMultiplier) / kalmanState->lastX));
+  } else {
+      kalmanState->e = 1.0f;
+  }
+
+  //kalmanState->e = ABS((target - input) * 3) + ABS(input/4);
+
+  //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;
+}
+
+
+void FAST_CODE kalman_update(float* input, float* output)
+{
+    update_kalman_covariance(input);
+    output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X] );
+    output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y] );
+    output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z] );
+
+    DEBUG_SET(DEBUG_KALMAN, 1, input[X]);                               //Gyro input
+
+    int16_t Kgain = (kalmanFilterStateRate[X].k * 1000.0f);
+    DEBUG_SET(DEBUG_KALMAN, 2, Kgain);                                  //Kalman gain
+    DEBUG_SET(DEBUG_KALMAN, 3, output[X]);                              //Kalman output
+}
+
+#endif
\ No newline at end of file
diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h
new file mode 100755
index 00000000000..d914d91fbfb
--- /dev/null
+++ b/src/main/flight/kalman.h
@@ -0,0 +1,76 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight 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 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 "sensors/gyro.h"
+#include "common/filter.h"
+
+#define MAX_KALMAN_WINDOW_SIZE 512
+
+#define VARIANCE_SCALE 0.67f
+
+typedef struct variance
+{
+    float xVar;
+    float yVar;
+    float zVar;
+    float xyCoVar;
+    float xzCoVar;
+    float yzCoVar;
+
+    uint32_t windex;
+    float xWindow[MAX_KALMAN_WINDOW_SIZE];
+    float yWindow[MAX_KALMAN_WINDOW_SIZE];
+    float zWindow[MAX_KALMAN_WINDOW_SIZE];
+
+    float xSumMean;
+    float ySumMean;
+    float zSumMean;
+
+    float xMean;
+    float yMean;
+    float zMean;
+
+    float xSumVar;
+    float ySumVar;
+    float zSumVar;
+    float xySumCoVar;
+    float xzSumCoVar;
+    float yzSumCoVar;
+
+    float inverseN;
+    uint16_t w;
+} variance_t;
+
+typedef struct kalman
+{
+    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;
+} kalman_t;
+
+extern void kalman_init(void);
+extern void kalman_update(float* input, float* output);
diff --git a/src/main/target/common.h b/src/main/target/common.h
index 5c8803245f3..a747fc99e2f 100755
--- a/src/main/target/common.h
+++ b/src/main/target/common.h
@@ -68,6 +68,7 @@
 
 #if (FLASH_SIZE > 256)
 #define USE_DYNAMIC_FILTERS
+#define USE_GYRO_KALMAN
 #define USE_EXTENDED_CMS_MENUS
 #define USE_UAV_INTERCONNECT
 #define USE_RX_UIB

From 929249ca840f4adb1f085150cf57376d574d85ad Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 21 Mar 2020 10:12:52 +0100
Subject: [PATCH 02/11] Fix Kalman computation

---
 src/main/flight/kalman.c | 185 ++++++++++++++++++++-------------------
 src/main/flight/kalman.h |   5 +-
 src/main/flight/pid.c    |   4 +
 src/main/sensors/gyro.c  |  19 ++++
 4 files changed, 120 insertions(+), 93 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index f60a2ff987e..756816f26da 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -28,138 +28,141 @@ FILE_COMPILE_FOR_SPEED
 #include "kalman.h"
 #include "build/debug.h"
 
-kalman_t    kalmanFilterStateRate[XYZ_AXIS_COUNT];
-variance_t  varStruct;
-float       setPoint[XYZ_AXIS_COUNT];
+kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
+variance_t varStruct;
+float setPoint[XYZ_AXIS_COUNT];
 
-
-void init_kalman(kalman_t *filter, float q)
+static void gyroKalmanInitAxis(kalman_t *filter, float q)
 {
     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->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;
     //FIXME hardcoded config
-    filter->s = 2500 / 250.0f;     //adding the new sharpness :) time to overfilter :O
+    filter->s = 2500 / 250.0f; //adding the new sharpness :) time to overfilter :O
 }
 
-
-void kalman_init(void)
+void gyroKalmanSetSetpoint(uint8_t axis, float rate)
 {
-    // isSetpointNew = 0;
+    setPoint[axis] = rate;
+}
 
+void gyroKalmanInitialize(void)
+{
     memset(&varStruct, 0, sizeof(varStruct));
     //FIXME values hardcoded for now
-    init_kalman(&kalmanFilterStateRate[X],  3000);
-    init_kalman(&kalmanFilterStateRate[Y],  3000);
-    init_kalman(&kalmanFilterStateRate[Z],  3000);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X], 3000);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], 3000);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], 3000);
 
     //FIXME values hardcoded for now
     varStruct.w = 32;
-    varStruct.inverseN = 1.0f/(float)(varStruct.w);
+    varStruct.inverseN = 1.0f / (float)(varStruct.w);
 }
 
-void update_kalman_covariance(float *gyroRateData)
+static void update_kalman_covariance(float *gyroRateData)
 {
-     varStruct.xWindow[ varStruct.windex] = gyroRateData[X];
-     varStruct.yWindow[ varStruct.windex] = gyroRateData[Y];
-     varStruct.zWindow[ varStruct.windex] = gyroRateData[Z];
-
-     varStruct.xSumMean +=  varStruct.xWindow[ varStruct.windex];
-     varStruct.ySumMean +=  varStruct.yWindow[ varStruct.windex];
-     varStruct.zSumMean +=  varStruct.zWindow[ varStruct.windex];
-     varStruct.xSumVar =  varStruct.xSumVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.xWindow[ varStruct.windex]);
-     varStruct.ySumVar =  varStruct.ySumVar + ( varStruct.yWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
-     varStruct.zSumVar =  varStruct.zSumVar + ( varStruct.zWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-     varStruct.xySumCoVar =  varStruct.xySumCoVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
-     varStruct.xzSumCoVar =  varStruct.xzSumCoVar + ( varStruct.xWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-     varStruct.yzSumCoVar =  varStruct.yzSumCoVar + ( varStruct.yWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-     varStruct.windex++;
-    if ( varStruct.windex >= varStruct.w)
+    varStruct.xWindow[varStruct.windex] = gyroRateData[X];
+    varStruct.yWindow[varStruct.windex] = gyroRateData[Y];
+    varStruct.zWindow[varStruct.windex] = gyroRateData[Z];
+
+    varStruct.xSumMean += varStruct.xWindow[varStruct.windex];
+    varStruct.ySumMean += varStruct.yWindow[varStruct.windex];
+    varStruct.zSumMean += varStruct.zWindow[varStruct.windex];
+    varStruct.xSumVar = varStruct.xSumVar + (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
+    varStruct.ySumVar = varStruct.ySumVar + (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
+    varStruct.zSumVar = varStruct.zSumVar + (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+    varStruct.xySumCoVar = varStruct.xySumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
+    varStruct.xzSumCoVar = varStruct.xzSumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+    varStruct.yzSumCoVar = varStruct.yzSumCoVar + (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+    varStruct.windex++;
+    if (varStruct.windex >= varStruct.w)
     {
-         varStruct.windex = 0;
+        varStruct.windex = 0;
     }
-     varStruct.xSumMean -=  varStruct.xWindow[ varStruct.windex];
-     varStruct.ySumMean -=  varStruct.yWindow[ varStruct.windex];
-     varStruct.zSumMean -=  varStruct.zWindow[ varStruct.windex];
-     varStruct.xSumVar =  varStruct.xSumVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.xWindow[ varStruct.windex]);
-     varStruct.ySumVar =  varStruct.ySumVar - ( varStruct.yWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
-     varStruct.zSumVar =  varStruct.zSumVar - ( varStruct.zWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-     varStruct.xySumCoVar =  varStruct.xySumCoVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.yWindow[ varStruct.windex]);
-     varStruct.xzSumCoVar =  varStruct.xzSumCoVar - ( varStruct.xWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-     varStruct.yzSumCoVar =  varStruct.yzSumCoVar - ( varStruct.yWindow[ varStruct.windex] *  varStruct.zWindow[ varStruct.windex]);
-
-     varStruct.xMean =  varStruct.xSumMean *  varStruct.inverseN;
-     varStruct.yMean =  varStruct.ySumMean *  varStruct.inverseN;
-     varStruct.zMean =  varStruct.zSumMean *  varStruct.inverseN;
-
-     varStruct.xVar =  fabsf(varStruct.xSumVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.xMean));
-     varStruct.yVar =  fabsf(varStruct.ySumVar *  varStruct.inverseN - ( varStruct.yMean *  varStruct.yMean));
-     varStruct.zVar =  fabsf(varStruct.zSumVar *  varStruct.inverseN - ( varStruct.zMean *  varStruct.zMean));
-     varStruct.xyCoVar =  fabsf(varStruct.xySumCoVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.yMean));
-     varStruct.xzCoVar =  fabsf(varStruct.xzSumCoVar *  varStruct.inverseN - ( varStruct.xMean *  varStruct.zMean));
-     varStruct.yzCoVar =  fabsf(varStruct.yzSumCoVar *  varStruct.inverseN - ( varStruct.yMean *  varStruct.zMean));
+    varStruct.xSumMean -= varStruct.xWindow[varStruct.windex];
+    varStruct.ySumMean -= varStruct.yWindow[varStruct.windex];
+    varStruct.zSumMean -= varStruct.zWindow[varStruct.windex];
+    varStruct.xSumVar = varStruct.xSumVar - (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
+    varStruct.ySumVar = varStruct.ySumVar - (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
+    varStruct.zSumVar = varStruct.zSumVar - (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+    varStruct.xySumCoVar = varStruct.xySumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
+    varStruct.xzSumCoVar = varStruct.xzSumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+    varStruct.yzSumCoVar = varStruct.yzSumCoVar - (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
+
+    varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
+    varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
+    varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;
+
+    varStruct.xVar = fabsf(varStruct.xSumVar * varStruct.inverseN - (varStruct.xMean * varStruct.xMean));
+    varStruct.yVar = fabsf(varStruct.ySumVar * varStruct.inverseN - (varStruct.yMean * varStruct.yMean));
+    varStruct.zVar = fabsf(varStruct.zSumVar * varStruct.inverseN - (varStruct.zMean * varStruct.zMean));
+    varStruct.xyCoVar = fabsf(varStruct.xySumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.yMean));
+    varStruct.xzCoVar = fabsf(varStruct.xzSumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.zMean));
+    varStruct.yzCoVar = fabsf(varStruct.yzSumCoVar * varStruct.inverseN - (varStruct.yMean * varStruct.zMean));
 
     float squirt;
-    arm_sqrt_f32(varStruct.xVar +  varStruct.xyCoVar +  varStruct.xzCoVar, &squirt);
+    arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
     kalmanFilterStateRate[X].r = squirt * VARIANCE_SCALE;
 
-    arm_sqrt_f32(varStruct.yVar +  varStruct.xyCoVar +  varStruct.yzCoVar, &squirt);
+    arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
     kalmanFilterStateRate[Y].r = squirt * VARIANCE_SCALE;
 
-    arm_sqrt_f32(varStruct.zVar +  varStruct.yzCoVar +  varStruct.xzCoVar, &squirt);
+    arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
     kalmanFilterStateRate[Z].r = squirt * VARIANCE_SCALE;
 }
 
-FAST_CODE float kalman_process(kalman_t* kalmanState, float input, float target)
+float kalman_process(kalman_t *kalmanState, float input, float target)
 {
-  //project the state ahead using acceleration
-  kalmanState->x += (kalmanState->x - kalmanState->lastX);
+    //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;
+    //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;
 
-  // calculate the error
-  	float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
+    // calculate the error
+    // float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
 
-  // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
+    // // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
 
-  	errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
+    // errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
 
-  if (target != 0.0f) {
-      kalmanState->e = fabsf(1.0f - ((target * errorMultiplier) / kalmanState->lastX));
-  } else {
-      kalmanState->e = 1.0f;
-  }
+    // if (target != 0.0f)
+    // {
+    //     kalmanState->e = fabsf(1.0f - ((target * errorMultiplier) / kalmanState->lastX));
+    // }
+    // else
+    // {
+    //     kalmanState->e = 1.0f;
+    // }
 
-  //kalmanState->e = ABS((target - input) * 3) + ABS(input/4);
+    kalmanState->e = ABS((target - input) * 3) + ABS(input/4);
 
-  //prediction update
-  kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
+    //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 FAST_CODE kalman_update(float* input, float* output)
+void gyroKalmanUpdate(float *input, float *output)
 {
     update_kalman_covariance(input);
-    output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X] );
-    output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y] );
-    output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z] );
-
-    DEBUG_SET(DEBUG_KALMAN, 1, input[X]);                               //Gyro input
-
-    int16_t Kgain = (kalmanFilterStateRate[X].k * 1000.0f);
-    DEBUG_SET(DEBUG_KALMAN, 2, Kgain);                                  //Kalman gain
-    DEBUG_SET(DEBUG_KALMAN, 3, output[X]);                              //Kalman output
+    output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X]);
+    output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y]);
+    output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z]);
+
+    DEBUG_SET(DEBUG_KALMAN, 0, input[X]); //Gyro input
+    DEBUG_SET(DEBUG_KALMAN, 1, setPoint[X]);
+    DEBUG_SET(DEBUG_KALMAN, 2, kalmanFilterStateRate[X].k * 1000.0f);     //Kalman gain
+    DEBUG_SET(DEBUG_KALMAN, 3, output[X]); //Kalman output
+    DEBUG_SET(DEBUG_KALMAN, 4, input[X] - output[X]);
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h
index d914d91fbfb..ab2872bbf71 100755
--- a/src/main/flight/kalman.h
+++ b/src/main/flight/kalman.h
@@ -72,5 +72,6 @@ typedef struct kalman
     float s;
 } kalman_t;
 
-extern void kalman_init(void);
-extern void kalman_update(float* input, float* output);
+void gyroKalmanInitialize(void);
+void gyroKalmanUpdate(float* input, float* output);
+void gyroKalmanSetSetpoint(uint8_t axis, float rate);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 740fd28a778..7ed13f1b13b 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -44,6 +44,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/rpm_filter.h"
+#include "flight/kalman.h"
 
 #include "io/gps.h"
 
@@ -987,6 +988,9 @@ void FAST_CODE pidController(float dT)
 
         // Limit desired rate to something gyro can measure reliably
         pidState[axis].rateTarget = constrainf(rateTarget, -GYRO_SATURATION_LIMIT, +GYRO_SATURATION_LIMIT);
+#ifdef USE_GYRO_KALMAN
+        gyroKalmanSetSetpoint(axis, pidState[axis].rateTarget);
+#endif
     }
 
     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index c51466322ee..78900b40583 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -72,6 +72,7 @@ FILE_COMPILE_FOR_SPEED
 #include "flight/gyroanalyse.h"
 #include "flight/rpm_filter.h"
 #include "flight/dynamic_gyro_notch.h"
+#include "flight/kalman.h"
 
 #ifdef USE_HARDWARE_REVISION_DETECTION
 #include "hardware_revision.h"
@@ -292,6 +293,9 @@ bool gyroInit(void)
     }
 
     gyroInitFilters();
+#ifdef USE_GYRO_KALMAN
+    gyroKalmanInitialize();
+#endif
 #ifdef USE_DYNAMIC_FILTERS
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
     gyroDataAnalyseStateInit(
@@ -454,6 +458,21 @@ void gyroUpdate()
         gyro.gyroADCf[axis] = gyroADCf;
     }
 
+#ifdef USE_GYRO_KALMAN
+    float input[XYZ_AXIS_COUNT];
+    float output[XYZ_AXIS_COUNT];
+
+    input[X] = gyro.gyroADCf[X];
+    input[Y] = gyro.gyroADCf[Y];
+    input[Z] = gyro.gyroADCf[Z];
+
+    gyroKalmanUpdate(input, output);
+
+    gyro.gyroADCf[X] = output[X];
+    gyro.gyroADCf[Y] = output[Y];
+    gyro.gyroADCf[Z] = output[Z];
+#endif
+
 #ifdef USE_DYNAMIC_FILTERS
     if (dynamicGyroNotchState.enabled) {
         gyroDataAnalyse(&gyroAnalyseState);

From 6399928fff0b95fc235509b6a2bca6da031a9a19 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 24 Apr 2020 14:37:49 +0200
Subject: [PATCH 03/11] Catchup on Emu implementation

---
 src/main/flight/kalman.c | 28 +++++++++++-----------------
 1 file changed, 11 insertions(+), 17 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index 756816f26da..93b6223cf05 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -115,6 +115,7 @@ static void update_kalman_covariance(float *gyroRateData)
 
 float kalman_process(kalman_t *kalmanState, float input, float target)
 {
+    float targetAbs = fabsf(target);
     //project the state ahead using acceleration
     kalmanState->x += (kalmanState->x - kalmanState->lastX);
 
@@ -123,23 +124,16 @@ float kalman_process(kalman_t *kalmanState, float input, float target)
     //update last state
     kalmanState->lastX = kalmanState->x;
 
-    // calculate the error
-    // float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
-
-    // // give a boost to the setpoint, used to caluclate the filter cutoff, based on the error and setpoint/gyrodata
-
-    // errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
+    if (kalmanState->lastX != 0.0f)
+    {
+        // calculate the error and add multiply sharpness boost
+        float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
 
-    // if (target != 0.0f)
-    // {
-    //     kalmanState->e = fabsf(1.0f - ((target * errorMultiplier) / kalmanState->lastX));
-    // }
-    // else
-    // {
-    //     kalmanState->e = 1.0f;
-    // }
+        // give a boost to the setpoint, used to caluclate the kalman q, based on the error and setpoint/gyrodata
+        errorMultiplier = constrainf(errorMultiplier * fabsf(1.0f - (target / kalmanState->lastX)) + 1.0f, 1.0f, 50.0f);
 
-    kalmanState->e = ABS((target - input) * 3) + ABS(input/4);
+        kalmanState->e = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX)));
+    }
 
     //prediction update
     kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
@@ -160,8 +154,8 @@ void gyroKalmanUpdate(float *input, float *output)
 
     DEBUG_SET(DEBUG_KALMAN, 0, input[X]); //Gyro input
     DEBUG_SET(DEBUG_KALMAN, 1, setPoint[X]);
-    DEBUG_SET(DEBUG_KALMAN, 2, kalmanFilterStateRate[X].k * 1000.0f);     //Kalman gain
-    DEBUG_SET(DEBUG_KALMAN, 3, output[X]); //Kalman output
+    DEBUG_SET(DEBUG_KALMAN, 2, kalmanFilterStateRate[X].k * 1000.0f); //Kalman gain
+    DEBUG_SET(DEBUG_KALMAN, 3, output[X]);                            //Kalman output
     DEBUG_SET(DEBUG_KALMAN, 4, input[X] - output[X]);
 }
 

From 2dd26513bffca573c4b72b14c9e49ae22cc2cbee Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 6 May 2020 14:19:15 +0200
Subject: [PATCH 04/11] Make Q, W and Sharpness configurable

---
 src/main/flight/kalman.c | 10 +++++-----
 src/main/sensors/gyro.c  |  7 +++++--
 src/main/sensors/gyro.h  |  3 +++
 3 files changed, 13 insertions(+), 7 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index 93b6223cf05..b7cc196c658 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -40,7 +40,7 @@ static void gyroKalmanInitAxis(kalman_t *filter, float q)
     filter->p = 30.0f;      //seeding P at 30.0f
     filter->e = 1.0f;
     //FIXME hardcoded config
-    filter->s = 2500 / 250.0f; //adding the new sharpness :) time to overfilter :O
+    filter->s = gyroConfig()->kalman_sharpness / 250.0f; //adding the new sharpness :) time to overfilter :O
 }
 
 void gyroKalmanSetSetpoint(uint8_t axis, float rate)
@@ -52,12 +52,12 @@ void gyroKalmanInitialize(void)
 {
     memset(&varStruct, 0, sizeof(varStruct));
     //FIXME values hardcoded for now
-    gyroKalmanInitAxis(&kalmanFilterStateRate[X], 3000);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], 3000);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], 3000);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X], gyroConfig()->kalman_q);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], gyroConfig()->kalman_q);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], gyroConfig()->kalman_q);
 
     //FIXME values hardcoded for now
-    varStruct.w = 32;
+    varStruct.w = gyroConfig()->kalman_w;
     varStruct.inverseN = 1.0f / (float)(varStruct.w);
 }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 574ab36fae0..212f6ac15f5 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 8);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
@@ -124,7 +124,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
-    .dynamicGyroNotchEnabled = 0
+    .dynamicGyroNotchEnabled = 0,
+    .kalman_q = 3000,
+    .kalman_w = 32,
+    .kalman_sharpness = 2500,
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 414d9042cc2..5ef24305a16 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -76,6 +76,9 @@ typedef struct gyroConfig_s {
     uint16_t dynamicGyroNotchQ;
     uint16_t dynamicGyroNotchMinHz;
     uint8_t dynamicGyroNotchEnabled;
+    uint16_t kalman_q;
+    uint16_t kalman_w;
+    uint16_t kalman_sharpness;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

From 54d151916d33462588a7db881d80779846b2b9aa Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Wed, 6 May 2020 14:27:17 +0200
Subject: [PATCH 05/11] Settings for Kalman Q, W and Sharpness

---
 src/main/fc/settings.yaml | 15 +++++++++++++++
 1 file changed, 15 insertions(+)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 016f74df66d..52fb429b3c6 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -208,6 +208,21 @@ groups:
         condition: USE_DYNAMIC_FILTERS
         min: 30
         max: 1000
+      - name: gyro_kalman_q
+        field: kalman_q
+        condition: USE_GYRO_KALMAN
+        min: 1000
+        max: 16000
+      - name: gyro_kalman_w
+        field: kalman_w
+        condition: USE_GYRO_KALMAN
+        min: 4
+        max: 512
+      - name: gyro_kalman_sharpness
+        field: kalman_sharpness
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 16000
       - name: gyro_to_use
         condition: USE_DUAL_GYRO
         min: 0

From 825f36e00729df6943c45bd3624bb06b36e60ddc Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 8 May 2020 14:52:30 +0200
Subject: [PATCH 06/11] Make it possible to enable/disable Kalman filter

---
 src/main/fc/settings.yaml |  4 ++++
 src/main/sensors/gyro.c   | 25 +++++++++++++++----------
 src/main/sensors/gyro.h   |  1 +
 3 files changed, 20 insertions(+), 10 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index 4e1faff490e..bdaae8114b9 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -210,6 +210,10 @@ groups:
         condition: USE_DYNAMIC_FILTERS
         min: 30
         max: 1000
+      - name: gyro_kalman_enabled
+        condition: USE_GYRO_KALMAN
+        field: kalmanEnabled
+        type: bool
       - name: gyro_kalman_q
         field: kalman_q
         condition: USE_GYRO_KALMAN
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 212f6ac15f5..86f70b55a33 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -128,6 +128,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .kalman_q = 3000,
     .kalman_w = 32,
     .kalman_sharpness = 2500,
+    .kalmanEnabled = 0,
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -327,7 +328,9 @@ bool gyroInit(void)
 
     gyroInitFilters();
 #ifdef USE_GYRO_KALMAN
-    gyroKalmanInitialize();
+    if (gyroConfig()->kalmanEnabled) {
+        gyroKalmanInitialize();
+    }
 #endif
 #ifdef USE_DYNAMIC_FILTERS
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
@@ -474,18 +477,20 @@ void FAST_CODE NOINLINE gyroUpdate()
     }
 
 #ifdef USE_GYRO_KALMAN
-    float input[XYZ_AXIS_COUNT];
-    float output[XYZ_AXIS_COUNT];
+    if (gyroConfig()->kalmanEnabled) {
+        float input[XYZ_AXIS_COUNT];
+        float output[XYZ_AXIS_COUNT];
 
-    input[X] = gyro.gyroADCf[X];
-    input[Y] = gyro.gyroADCf[Y];
-    input[Z] = gyro.gyroADCf[Z];
+        input[X] = gyro.gyroADCf[X];
+        input[Y] = gyro.gyroADCf[Y];
+        input[Z] = gyro.gyroADCf[Z];
 
-    gyroKalmanUpdate(input, output);
+        gyroKalmanUpdate(input, output);
 
-    gyro.gyroADCf[X] = output[X];
-    gyro.gyroADCf[Y] = output[Y];
-    gyro.gyroADCf[Z] = output[Z];
+        gyro.gyroADCf[X] = output[X];
+        gyro.gyroADCf[Y] = output[Y];
+        gyro.gyroADCf[Z] = output[Z];
+    }
 #endif
 
 #ifdef USE_DYNAMIC_FILTERS
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index 5ef24305a16..f03d418cd0d 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -79,6 +79,7 @@ typedef struct gyroConfig_s {
     uint16_t kalman_q;
     uint16_t kalman_w;
     uint16_t kalman_sharpness;
+    uint8_t kalmanEnabled;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

From 982adc31cffb412345ebd33edb5f0bf3fd9fdd66 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Fri, 29 May 2020 16:45:44 +0200
Subject: [PATCH 07/11] Change scaling to make initial values simpler

---
 src/main/flight/kalman.c | 9 +++------
 src/main/sensors/gyro.c  | 6 +++---
 2 files changed, 6 insertions(+), 9 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index b7cc196c658..aafbcf8f089 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -35,12 +35,11 @@ float setPoint[XYZ_AXIS_COUNT];
 static void gyroKalmanInitAxis(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.03f; //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;
-    //FIXME hardcoded config
-    filter->s = gyroConfig()->kalman_sharpness / 250.0f; //adding the new sharpness :) time to overfilter :O
+    filter->s = gyroConfig()->kalman_sharpness / 10.0f; 
 }
 
 void gyroKalmanSetSetpoint(uint8_t axis, float rate)
@@ -51,13 +50,11 @@ void gyroKalmanSetSetpoint(uint8_t axis, float rate)
 void gyroKalmanInitialize(void)
 {
     memset(&varStruct, 0, sizeof(varStruct));
-    //FIXME values hardcoded for now
     gyroKalmanInitAxis(&kalmanFilterStateRate[X], gyroConfig()->kalman_q);
     gyroKalmanInitAxis(&kalmanFilterStateRate[Y], gyroConfig()->kalman_q);
     gyroKalmanInitAxis(&kalmanFilterStateRate[Z], gyroConfig()->kalman_q);
 
-    //FIXME values hardcoded for now
-    varStruct.w = gyroConfig()->kalman_w;
+    varStruct.w = gyroConfig()->kalman_w * 8;
     varStruct.inverseN = 1.0f / (float)(varStruct.w);
 }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 86f70b55a33..d37ce69eeb0 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -125,9 +125,9 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
     .dynamicGyroNotchEnabled = 0,
-    .kalman_q = 3000,
-    .kalman_w = 32,
-    .kalman_sharpness = 2500,
+    .kalman_q = 100,
+    .kalman_w = 4,
+    .kalman_sharpness = 100,
     .kalmanEnabled = 0,
 );
 

From 7ea19d9ab6ec2ded66b6d650e526ed73448d3250 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sat, 30 May 2020 17:52:02 +0200
Subject: [PATCH 08/11] Change Kalman constrains

---
 src/main/fc/settings.yaml | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml
index bdaae8114b9..b09e9ab363b 100644
--- a/src/main/fc/settings.yaml
+++ b/src/main/fc/settings.yaml
@@ -217,13 +217,13 @@ groups:
       - name: gyro_kalman_q
         field: kalman_q
         condition: USE_GYRO_KALMAN
-        min: 1000
+        min: 1
         max: 16000
       - name: gyro_kalman_w
         field: kalman_w
         condition: USE_GYRO_KALMAN
-        min: 4
-        max: 512
+        min: 1
+        max: 40
       - name: gyro_kalman_sharpness
         field: kalman_sharpness
         condition: USE_GYRO_KALMAN

From 76cb705391e65812af88255d92083057c5a0b661 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 31 May 2020 16:14:23 +0200
Subject: [PATCH 09/11] Compute real variance

---
 src/main/flight/kalman.c | 103 +++++++++++++++------------------------
 src/main/flight/kalman.h |  43 ++++------------
 2 files changed, 49 insertions(+), 97 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index aafbcf8f089..9df7789145e 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -29,17 +29,18 @@ FILE_COMPILE_FOR_SPEED
 #include "build/debug.h"
 
 kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
-variance_t varStruct;
 float setPoint[XYZ_AXIS_COUNT];
 
-static void gyroKalmanInitAxis(kalman_t *filter, float q)
+static void gyroKalmanInitAxis(kalman_t *filter)
 {
     memset(filter, 0, sizeof(kalman_t));
-    filter->q = q * 0.03f; //add multiplier to make tuning easier
+    filter->q = gyroConfig()->kalman_q * 0.03f; //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()->kalman_sharpness / 10.0f; 
+    filter->s = gyroConfig()->kalman_sharpness / 10.0f;
+    filter->w = gyroConfig()->kalman_w * 8;
+    filter->inverseN = 1.0f / (float)(filter->w);
 }
 
 void gyroKalmanSetSetpoint(uint8_t axis, float rate)
@@ -49,65 +50,9 @@ void gyroKalmanSetSetpoint(uint8_t axis, float rate)
 
 void gyroKalmanInitialize(void)
 {
-    memset(&varStruct, 0, sizeof(varStruct));
-    gyroKalmanInitAxis(&kalmanFilterStateRate[X], gyroConfig()->kalman_q);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], gyroConfig()->kalman_q);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], gyroConfig()->kalman_q);
-
-    varStruct.w = gyroConfig()->kalman_w * 8;
-    varStruct.inverseN = 1.0f / (float)(varStruct.w);
-}
-
-static void update_kalman_covariance(float *gyroRateData)
-{
-    varStruct.xWindow[varStruct.windex] = gyroRateData[X];
-    varStruct.yWindow[varStruct.windex] = gyroRateData[Y];
-    varStruct.zWindow[varStruct.windex] = gyroRateData[Z];
-
-    varStruct.xSumMean += varStruct.xWindow[varStruct.windex];
-    varStruct.ySumMean += varStruct.yWindow[varStruct.windex];
-    varStruct.zSumMean += varStruct.zWindow[varStruct.windex];
-    varStruct.xSumVar = varStruct.xSumVar + (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
-    varStruct.ySumVar = varStruct.ySumVar + (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
-    varStruct.zSumVar = varStruct.zSumVar + (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-    varStruct.xySumCoVar = varStruct.xySumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
-    varStruct.xzSumCoVar = varStruct.xzSumCoVar + (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-    varStruct.yzSumCoVar = varStruct.yzSumCoVar + (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-    varStruct.windex++;
-    if (varStruct.windex >= varStruct.w)
-    {
-        varStruct.windex = 0;
-    }
-    varStruct.xSumMean -= varStruct.xWindow[varStruct.windex];
-    varStruct.ySumMean -= varStruct.yWindow[varStruct.windex];
-    varStruct.zSumMean -= varStruct.zWindow[varStruct.windex];
-    varStruct.xSumVar = varStruct.xSumVar - (varStruct.xWindow[varStruct.windex] * varStruct.xWindow[varStruct.windex]);
-    varStruct.ySumVar = varStruct.ySumVar - (varStruct.yWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
-    varStruct.zSumVar = varStruct.zSumVar - (varStruct.zWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-    varStruct.xySumCoVar = varStruct.xySumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.yWindow[varStruct.windex]);
-    varStruct.xzSumCoVar = varStruct.xzSumCoVar - (varStruct.xWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-    varStruct.yzSumCoVar = varStruct.yzSumCoVar - (varStruct.yWindow[varStruct.windex] * varStruct.zWindow[varStruct.windex]);
-
-    varStruct.xMean = varStruct.xSumMean * varStruct.inverseN;
-    varStruct.yMean = varStruct.ySumMean * varStruct.inverseN;
-    varStruct.zMean = varStruct.zSumMean * varStruct.inverseN;
-
-    varStruct.xVar = fabsf(varStruct.xSumVar * varStruct.inverseN - (varStruct.xMean * varStruct.xMean));
-    varStruct.yVar = fabsf(varStruct.ySumVar * varStruct.inverseN - (varStruct.yMean * varStruct.yMean));
-    varStruct.zVar = fabsf(varStruct.zSumVar * varStruct.inverseN - (varStruct.zMean * varStruct.zMean));
-    varStruct.xyCoVar = fabsf(varStruct.xySumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.yMean));
-    varStruct.xzCoVar = fabsf(varStruct.xzSumCoVar * varStruct.inverseN - (varStruct.xMean * varStruct.zMean));
-    varStruct.yzCoVar = fabsf(varStruct.yzSumCoVar * varStruct.inverseN - (varStruct.yMean * varStruct.zMean));
-
-    float squirt;
-    arm_sqrt_f32(varStruct.xVar + varStruct.xyCoVar + varStruct.xzCoVar, &squirt);
-    kalmanFilterStateRate[X].r = squirt * VARIANCE_SCALE;
-
-    arm_sqrt_f32(varStruct.yVar + varStruct.xyCoVar + varStruct.yzCoVar, &squirt);
-    kalmanFilterStateRate[Y].r = squirt * VARIANCE_SCALE;
-
-    arm_sqrt_f32(varStruct.zVar + varStruct.yzCoVar + varStruct.xzCoVar, &squirt);
-    kalmanFilterStateRate[Z].r = squirt * VARIANCE_SCALE;
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X]);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y]);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z]);
 }
 
 float kalman_process(kalman_t *kalmanState, float input, float target)
@@ -142,9 +87,39 @@ float kalman_process(kalman_t *kalmanState, float input, float target)
     return kalmanState->x;
 }
 
+static void updateAxisVariance(uint8_t axis, kalman_t *kalmanState, float rate)
+{
+    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;
+}
+
 void gyroKalmanUpdate(float *input, float *output)
 {
-    update_kalman_covariance(input);
+    updateAxisVariance(X, &kalmanFilterStateRate[X], input[X]);
+    updateAxisVariance(Y, &kalmanFilterStateRate[Y], input[Y]);
+    updateAxisVariance(Z, &kalmanFilterStateRate[Z], input[Z]);
+
     output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X]);
     output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y]);
     output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z]);
diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h
index ab2872bbf71..0bb7b0e0c2e 100755
--- a/src/main/flight/kalman.h
+++ b/src/main/flight/kalman.h
@@ -27,39 +27,6 @@
 
 #define VARIANCE_SCALE 0.67f
 
-typedef struct variance
-{
-    float xVar;
-    float yVar;
-    float zVar;
-    float xyCoVar;
-    float xzCoVar;
-    float yzCoVar;
-
-    uint32_t windex;
-    float xWindow[MAX_KALMAN_WINDOW_SIZE];
-    float yWindow[MAX_KALMAN_WINDOW_SIZE];
-    float zWindow[MAX_KALMAN_WINDOW_SIZE];
-
-    float xSumMean;
-    float ySumMean;
-    float zSumMean;
-
-    float xMean;
-    float yMean;
-    float zMean;
-
-    float xSumVar;
-    float ySumVar;
-    float zSumVar;
-    float xySumCoVar;
-    float xzSumCoVar;
-    float yzSumCoVar;
-
-    float inverseN;
-    uint16_t w;
-} variance_t;
-
 typedef struct kalman
 {
     float q;     //process noise covariance
@@ -70,6 +37,16 @@ typedef struct kalman
     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 axisSumMean;
+    float axisMean;
+    float axisSumVar;
+    float inverseN;
+    uint16_t w;
 } kalman_t;
 
 void gyroKalmanInitialize(void);

From 8110138ac2060282eef77d507fd14ba46cb62819 Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Sun, 31 May 2020 20:51:07 +0200
Subject: [PATCH 10/11] Drop unused function parameter

---
 src/main/flight/kalman.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index 9df7789145e..e719cc4751f 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -87,7 +87,7 @@ float kalman_process(kalman_t *kalmanState, float input, float target)
     return kalmanState->x;
 }
 
-static void updateAxisVariance(uint8_t axis, kalman_t *kalmanState, float rate)
+static void updateAxisVariance(kalman_t *kalmanState, float rate)
 {
     kalmanState->axisWindow[kalmanState->windex] = rate;
 
@@ -116,9 +116,9 @@ static void updateAxisVariance(uint8_t axis, kalman_t *kalmanState, float rate)
 
 void gyroKalmanUpdate(float *input, float *output)
 {
-    updateAxisVariance(X, &kalmanFilterStateRate[X], input[X]);
-    updateAxisVariance(Y, &kalmanFilterStateRate[Y], input[Y]);
-    updateAxisVariance(Z, &kalmanFilterStateRate[Z], input[Z]);
+    updateAxisVariance(&kalmanFilterStateRate[X], input[X]);
+    updateAxisVariance(&kalmanFilterStateRate[Y], input[Y]);
+    updateAxisVariance(&kalmanFilterStateRate[Z], input[Z]);
 
     output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X]);
     output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y]);

From efe7d583833d676da1cad58897a9ae2239171d6d Mon Sep 17 00:00:00 2001
From: "Pawel Spychalski (DzikuVx)" <pspychalski@gmail.com>
Date: Thu, 11 Jun 2020 12:45:00 +0200
Subject: [PATCH 11/11] Improve EKF processing for gyro

---
 src/main/flight/kalman.c | 20 ++++++--------------
 src/main/flight/kalman.h |  2 +-
 src/main/sensors/gyro.c  | 15 +++------------
 3 files changed, 10 insertions(+), 27 deletions(-)

diff --git a/src/main/flight/kalman.c b/src/main/flight/kalman.c
index e719cc4751f..9e672a4a6ac 100755
--- a/src/main/flight/kalman.c
+++ b/src/main/flight/kalman.c
@@ -114,21 +114,13 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
     kalmanState->r = squirt * VARIANCE_SCALE;
 }
 
-void gyroKalmanUpdate(float *input, float *output)
+float gyroKalmanUpdate(uint8_t axis, float input)
 {
-    updateAxisVariance(&kalmanFilterStateRate[X], input[X]);
-    updateAxisVariance(&kalmanFilterStateRate[Y], input[Y]);
-    updateAxisVariance(&kalmanFilterStateRate[Z], input[Z]);
-
-    output[X] = kalman_process(&kalmanFilterStateRate[X], input[X], setPoint[X]);
-    output[Y] = kalman_process(&kalmanFilterStateRate[Y], input[Y], setPoint[Y]);
-    output[Z] = kalman_process(&kalmanFilterStateRate[Z], input[Z], setPoint[Z]);
-
-    DEBUG_SET(DEBUG_KALMAN, 0, input[X]); //Gyro input
-    DEBUG_SET(DEBUG_KALMAN, 1, setPoint[X]);
-    DEBUG_SET(DEBUG_KALMAN, 2, kalmanFilterStateRate[X].k * 1000.0f); //Kalman gain
-    DEBUG_SET(DEBUG_KALMAN, 3, output[X]);                            //Kalman output
-    DEBUG_SET(DEBUG_KALMAN, 4, input[X] - output[X]);
+    updateAxisVariance(&kalmanFilterStateRate[axis], input);
+
+    DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
+
+    return kalman_process(&kalmanFilterStateRate[axis], input, setPoint[axis]);
 }
 
 #endif
\ No newline at end of file
diff --git a/src/main/flight/kalman.h b/src/main/flight/kalman.h
index 0bb7b0e0c2e..f493c1f628d 100755
--- a/src/main/flight/kalman.h
+++ b/src/main/flight/kalman.h
@@ -50,5 +50,5 @@ typedef struct kalman
 } kalman_t;
 
 void gyroKalmanInitialize(void);
-void gyroKalmanUpdate(float* input, float* output);
+float gyroKalmanUpdate(uint8_t axis, float input);
 void gyroKalmanSetSetpoint(uint8_t axis, float rate);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index d37ce69eeb0..afe18ba0b72 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -478,18 +478,9 @@ void FAST_CODE NOINLINE gyroUpdate()
 
 #ifdef USE_GYRO_KALMAN
     if (gyroConfig()->kalmanEnabled) {
-        float input[XYZ_AXIS_COUNT];
-        float output[XYZ_AXIS_COUNT];
-
-        input[X] = gyro.gyroADCf[X];
-        input[Y] = gyro.gyroADCf[Y];
-        input[Z] = gyro.gyroADCf[Z];
-
-        gyroKalmanUpdate(input, output);
-
-        gyro.gyroADCf[X] = output[X];
-        gyro.gyroADCf[Y] = output[Y];
-        gyro.gyroADCf[Z] = output[Z];
+        gyro.gyroADCf[X] = gyroKalmanUpdate(X, gyro.gyroADCf[X]);
+        gyro.gyroADCf[Y] = gyroKalmanUpdate(Y, gyro.gyroADCf[Y]);
+        gyro.gyroADCf[Z] = gyroKalmanUpdate(Z, gyro.gyroADCf[Z]);
     }
 #endif