Gyro Kalman filter from EmuFlight (#5519)

* Direct copy from EmuFlight, compiles, does not wrk

* Fix Kalman computation

* Catchup on Emu implementation

* Make Q, W and Sharpness configurable

* Settings for Kalman Q, W and Sharpness

* Make it possible to enable/disable Kalman filter

* Change scaling to make initial values simpler

* Change Kalman constrains

* Compute real variance

* Drop unused function parameter

* Improve EKF processing for gyro

make/source.mk

@@ -104,6 +104,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/servo_sbus.c \

src/main/build/debug.h

@@ -72,5 +72,6 @@ typedef enum {
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
     DEBUG_IRLOCK,
     DEBUG_CD,
+    DEBUG_KALMAN,
     DEBUG_COUNT
 } debugType_e;

src/main/fc/settings.yaml

@@ -84,7 +84,7 @@ tables:
       "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",
-      "IRLOCK", "CD"]
+      "IRLOCK", "CD", "KALMAN"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -203,6 +203,25 @@ 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
+        min: 1
+        max: 16000
+      - name: gyro_kalman_w
+        field: kalman_w
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 40
+      - 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

src/main/flight/kalman.c

@@ -0,0 +1,126 @@
+/*
+ * 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];
+float setPoint[XYZ_AXIS_COUNT];
+
+static void gyroKalmanInitAxis(kalman_t *filter)
+{
+    memset(filter, 0, sizeof(kalman_t));
+    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->w = gyroConfig()->kalman_w * 8;
+    filter->inverseN = 1.0f / (float)(filter->w);
+}
+
+void gyroKalmanSetSetpoint(uint8_t axis, float rate)
+{
+    setPoint[axis] = rate;
+}
+
+void gyroKalmanInitialize(void)
+{
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X]);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y]);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z]);
+}
+
+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);
+
+    //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->lastX != 0.0f)
+    {
+        // calculate the error and add multiply sharpness boost
+        float errorMultiplier = fabsf(target - kalmanState->x) * kalmanState->s;
+
+        // 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 = fabsf(1.0f - (((targetAbs + 1.0f) * errorMultiplier) / fabsf(kalmanState->lastX)));
+    }
+
+    //prediction update
+    kalmanState->p = kalmanState->p + (kalmanState->q * kalmanState->e);
+
+    //measurement update
+    kalmanState->k = kalmanState->p / (kalmanState->p + kalmanState->r);
+    kalmanState->x += kalmanState->k * (input - kalmanState->x);
+    kalmanState->p = (1.0f - kalmanState->k) * kalmanState->p;
+    return kalmanState->x;
+}
+
+static void updateAxisVariance(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;
+}
+
+float gyroKalmanUpdate(uint8_t axis, float input)
+{
+    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

src/main/flight/kalman.h

@@ -0,0 +1,54 @@
+/*
+ * 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 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;
+
+    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);
+float gyroKalmanUpdate(uint8_t axis, float input);
+void gyroKalmanSetSetpoint(uint8_t axis, float rate);

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"
 
@@ -966,6 +967,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

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"
@@ -119,7 +120,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
-    .dynamicGyroNotchEnabled = 0
+    .dynamicGyroNotchEnabled = 0,
+    .kalman_q = 100,
+    .kalman_w = 4,
+    .kalman_sharpness = 100,
+    .kalmanEnabled = 0,
 );
 
 STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware)
@@ -307,6 +312,11 @@ bool gyroInit(void)
     }
 
     gyroInitFilters();
+#ifdef USE_GYRO_KALMAN
+    if (gyroConfig()->kalmanEnabled) {
+        gyroKalmanInitialize();
+    }
+#endif
 #ifdef USE_DYNAMIC_FILTERS
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
     gyroDataAnalyseStateInit(
@@ -450,6 +460,14 @@ void FAST_CODE NOINLINE gyroUpdate()
         gyro.gyroADCf[axis] = gyroADCf;
     }
 
+#ifdef USE_GYRO_KALMAN
+    if (gyroConfig()->kalmanEnabled) {
+        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
+
 #ifdef USE_DYNAMIC_FILTERS
     if (dynamicGyroNotchState.enabled) {
         gyroDataAnalyse(&gyroAnalyseState);

src/main/sensors/gyro.h

@@ -74,6 +74,10 @@ 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;
+    uint8_t kalmanEnabled;
 } gyroConfig_t;
 
 PG_DECLARE(gyroConfig_t, gyroConfig);

src/main/target/common.h

@@ -72,6 +72,7 @@
 #define USE_PITOT_VIRTUAL
 
 #define USE_DYNAMIC_FILTERS
+#define USE_GYRO_KALMAN
 #define USE_EXTENDED_CMS_MENUS
 #define USE_UAV_INTERCONNECT
 #define USE_RX_UIB