Kalman Filter

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 \

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",
 };

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;
 

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, 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) },
 #endif

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"
@@ -776,6 +777,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 +799,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 +825,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 +850,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,   MAX_KALMAN_WINDOW_SIZE, 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}

src/main/common/kalman.c

@@ -0,0 +1,110 @@
+/*
+ * 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 "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 / 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);
+}
+
+// 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;
+    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;
+
+  // 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;
+  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;
+}

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 64
+
+#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;
+    uint8_t windex;
+    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;
+
+    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);

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)

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

src/main/sensors/gyro_filter_impl.c

@@ -46,6 +46,14 @@ 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));
 
@@ -82,6 +90,9 @@ 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));
 

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)
@@ -580,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:

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