Gyro Kalman filter from EmuFlight

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 \

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;

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

src/main/flight/kalman.c

@@ -0,0 +1,168 @@
+/*
+ * 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];
+
+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->e = 1.0f;
+    //FIXME hardcoded config
+    filter->s = 2500 / 250.0f; //adding the new sharpness :) time to overfilter :O
+}
+
+void gyroKalmanSetSetpoint(uint8_t axis, float rate)
+{
+    setPoint[axis] = rate;
+}
+
+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);
+
+    //FIXME values hardcoded for now
+    varStruct.w = 32;
+    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;
+}
+
+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 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, 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

src/main/flight/kalman.h

@@ -0,0 +1,77 @@
+/*
+ * 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;
+
+void gyroKalmanInitialize(void);
+void gyroKalmanUpdate(float* input, float* output);
+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"
 
@@ -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

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);

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