Merge pull request #5928 from iNavFlight/dzikuvx-gyro-ekf-refactor

Gyro EKF refactor

src/main/build/debug.h

@@ -72,7 +72,8 @@ typedef enum {
     DEBUG_DYNAMIC_FILTER_FREQUENCY,
     DEBUG_IRLOCK,
     DEBUG_CD,
-    DEBUG_KALMAN,
+    DEBUG_KALMAN_GAIN,
+    DEBUG_PID_MEASUREMENT,
     DEBUG_SPM_CELLS,    // Smartport master FLVSS
     DEBUG_SPM_VS600,    // Smartport master VS600 VTX
     DEBUG_SPM_VARIO,    // Smartport master variometer

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", "KALMAN", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
+      "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574"]
   - name: async_mode
     values: ["NONE", "GYRO", "ALL"]
   - name: aux_operator
@@ -229,25 +229,6 @@ 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
@@ -1802,6 +1783,33 @@ groups:
         field: controlDerivativeLpfHz
         min: 0
         max: 200
+      - name: setpoint_kalman_enabled
+        description: "Enable Kalman filter on the PID controller setpoint"
+        default_value: "OFF"
+        condition: USE_GYRO_KALMAN
+        field: kalmanEnabled
+        type: bool
+      - name: setpoint_kalman_q
+        description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds"
+        default_value: "100"
+        field: kalman_q
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 16000
+      - name: setpoint_kalman_w
+        description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response"
+        default_value: "4"
+        field: kalman_w
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 40
+      - name: setpoint_kalman_sharpness
+        description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets"
+        default_value: "100"
+        field: kalman_sharpness
+        condition: USE_GYRO_KALMAN
+        min: 1
+        max: 16000
 
   - name: PG_PID_AUTOTUNE_CONFIG
     type: pidAutotuneConfig_t

src/main/flight/kalman.c

@@ -29,30 +29,24 @@ FILE_COMPILE_FOR_SPEED
 #include "build/debug.h"
 
 kalman_t kalmanFilterStateRate[XYZ_AXIS_COUNT];
-float setPoint[XYZ_AXIS_COUNT];
 
-static void gyroKalmanInitAxis(kalman_t *filter)
+static void gyroKalmanInitAxis(kalman_t *filter, uint16_t q, uint16_t w, uint16_t sharpness)
 {
     memset(filter, 0, sizeof(kalman_t));
-    filter->q = gyroConfig()->kalman_q * 0.03f; //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;
-    filter->s = gyroConfig()->kalman_sharpness / 10.0f;
-    filter->w = gyroConfig()->kalman_w * 8;
+    filter->s = sharpness / 10.0f;
+    filter->w = w * 8;
     filter->inverseN = 1.0f / (float)(filter->w);
 }
 
-void gyroKalmanSetSetpoint(uint8_t axis, float rate)
+void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness)
 {
-    setPoint[axis] = rate;
-}
-
-void gyroKalmanInitialize(void)
-{
-    gyroKalmanInitAxis(&kalmanFilterStateRate[X]);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Y]);
-    gyroKalmanInitAxis(&kalmanFilterStateRate[Z]);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[X], q, w, sharpness);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Y], q, w, sharpness);
+    gyroKalmanInitAxis(&kalmanFilterStateRate[Z], q, w, sharpness);
 }
 
 float kalman_process(kalman_t *kalmanState, float input, float target)
@@ -114,13 +108,13 @@ static void updateAxisVariance(kalman_t *kalmanState, float rate)
     kalmanState->r = squirt * VARIANCE_SCALE;
 }
 
-float gyroKalmanUpdate(uint8_t axis, float input)
+float gyroKalmanUpdate(uint8_t axis, float input, float setpoint)
 {
     updateAxisVariance(&kalmanFilterStateRate[axis], input);
 
-    DEBUG_SET(DEBUG_KALMAN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
+    DEBUG_SET(DEBUG_KALMAN_GAIN, axis, kalmanFilterStateRate[axis].k * 1000.0f); //Kalman gain
 
-    return kalman_process(&kalmanFilterStateRate[axis], input, setPoint[axis]);
+    return kalman_process(&kalmanFilterStateRate[axis], input, setpoint);
 }
 
 #endif

src/main/flight/kalman.h

@@ -49,6 +49,5 @@ typedef struct kalman
     uint16_t w;
 } kalman_t;
 
-void gyroKalmanInitialize(void);
-float gyroKalmanUpdate(uint8_t axis, float input);
-void gyroKalmanSetSetpoint(uint8_t axis, float rate);
+void gyroKalmanInitialize(uint16_t q, uint16_t w, uint16_t sharpness);
+float gyroKalmanUpdate(uint8_t axis, float input, float setpoint);

src/main/flight/pid.c

@@ -155,7 +155,7 @@ static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
 static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn;
 static EXTENDED_FASTRAM bool levelingEnabled = false;
 
-PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 13);
+PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 14);
 
 PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .bank_mc = {
@@ -276,6 +276,10 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
         .pidControllerType = PID_TYPE_AUTO,
         .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT,
         .controlDerivativeLpfHz = 30,
+        .kalman_q = 100,
+        .kalman_w = 4,
+        .kalman_sharpness = 100,
+        .kalmanEnabled = 0,
 );
 
 bool pidInitFilters(void)
@@ -974,9 +978,13 @@ 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);
+        if (pidProfile()->kalmanEnabled) {
+            pidState[axis].gyroRate = gyroKalmanUpdate(axis, pidState[axis].gyroRate, pidState[axis].rateTarget);
+        }
 #endif
+        DEBUG_SET(DEBUG_PID_MEASUREMENT, axis, pidState[axis].gyroRate);
     }
 
     // Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
@@ -1108,6 +1116,11 @@ void pidInit(void)
     }
 
     pidResetTPAFilter();
+#ifdef USE_GYRO_KALMAN
+    if (pidProfile()->kalmanEnabled) {
+        gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness);
+    }
+#endif
 }
 
 const pidBank_t * pidBank(void) { 

src/main/flight/pid.h

@@ -151,6 +151,10 @@ typedef struct pidProfile_s {
 
     uint16_t navFwPosHdgPidsumLimit;
     uint8_t controlDerivativeLpfHz;
+    uint16_t kalman_q;
+    uint16_t kalman_w;
+    uint16_t kalman_sharpness;
+    uint8_t kalmanEnabled;
 } pidProfile_t;
 
 typedef struct pidAutotuneConfig_s {

src/main/sensors/gyro.c

@@ -72,7 +72,6 @@ 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"
@@ -102,7 +101,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState;
 
 #endif
 
-PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
+PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 10);
 
 PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .gyro_lpf = GYRO_LPF_42HZ,      // 42HZ value is defined for Invensense/TDK gyros
@@ -121,10 +120,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig,
     .dynamicGyroNotchQ = 120,
     .dynamicGyroNotchMinHz = 150,
     .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)
@@ -312,11 +307,6 @@ bool gyroInit(void)
     }
 
     gyroInitFilters();
-#ifdef USE_GYRO_KALMAN
-    if (gyroConfig()->kalmanEnabled) {
-        gyroKalmanInitialize();
-    }
-#endif
 #ifdef USE_DYNAMIC_FILTERS
     dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
     gyroDataAnalyseStateInit(
@@ -460,14 +450,6 @@ 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,10 +74,6 @@ 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);