LULU filter by C.H. Rohwer -- nonlinear filter which seems to work nicely on the d-term

make/source.mk

@@ -248,6 +248,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
             $(CMSIS_SRC) \
             $(DEVICE_STDPERIPH_SRC) \
             common/kalman.c \
+            common/lulu.c \
 
 SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \
             bus_bst_stm32f30x.c \

src/main/blackbox/blackbox.c

@@ -1218,6 +1218,9 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("mixer_impl", "%d",                      currentPidProfile->mixer_impl);
         BLACKBOX_PRINT_HEADER_LINE("mixer_laziness", "%d",                  currentPidProfile->mixer_laziness);
         BLACKBOX_PRINT_HEADER_LINE("mixer_yaw_throttle_comp", "%d",         currentPidProfile->mixer_yaw_throttle_comp);
+#ifdef USE_LULU
+        BLACKBOX_PRINT_HEADER_LINE("lulu_n_val", "%d",                      currentPidProfile->lulu_n_val);
+#endif
         // End of EmuFlight controller parameters
         BLACKBOX_PRINT_HEADER_LINE("deadband", "%d",                        rcControlsConfig()->deadband);
         BLACKBOX_PRINT_HEADER_LINE("yaw_deadband", "%d",                    rcControlsConfig()->yaw_deadband);

src/main/build/debug.c

@@ -77,5 +77,6 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "IMU",
     "KALMAN",
     "ANGLE",
-    "HORIZON"
+    "HORIZON",
+    "LULU"
 };

src/main/build/debug.h

@@ -96,6 +96,7 @@ typedef enum {
     DEBUG_KALMAN,
     DEBUG_ANGLE,
     DEBUG_HORIZON,
+    DEBUG_LULU,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cms/cms_menu_imu.c

@@ -87,6 +87,9 @@ static uint8_t tmpRateProfileIndex;
 static uint8_t rateProfileIndex;
 static char rateProfileIndexString[] = " p-r";
 static controlRateConfig_t rateProfile;
+#ifdef USE_LULU
+static uint8_t lulu_n_val;
+#endif
 
 static const char * const cms_offOnLabels[] = {
     "OFF", "ON"
@@ -101,7 +104,10 @@ static const char * const cms_mixerImplTypeLabels[] = {
 };
 
 static const char * const cms_FilterType[] = {
-    "PT1", "BIQUAD", "PT2", "PT3", "PT4",
+    "PT1", "BIQUAD", "PT2", "PT3", "PT4", 
+#ifdef USE_LULU
+    "LULU",
+#endif
 };
 
 #ifdef USE_GYRO_DATA_ANALYSE
@@ -159,6 +165,9 @@ static long cmsx_PidAdvancedRead(void) {
     itermWindup = pidProfile->itermWindupPointPercent;
     emuGravityGain = pidProfile->emuGravityGain;
     linear_thrust_low_output = pidProfile->linear_thrust_low_output;
+#ifdef USE_LULU
+    lulu_n_val = pidProfile->lulu_n_val;
+#endif
     linear_thrust_high_output = pidProfile->linear_thrust_high_output;
     linear_throttle = pidProfile->linear_throttle;
     mixer_impl = pidProfile->mixer_impl;
@@ -192,6 +201,9 @@ static long cmsx_PidAdvancedWriteback(const OSD_Entry *self) {
     pidProfile->itermWindupPointPercent = itermWindup;
     pidProfile->emuGravityGain = emuGravityGain;
     pidProfile->linear_thrust_low_output = linear_thrust_low_output;
+#ifdef USE_LULU
+    pidProfile->lulu_n_val = lulu_n_val;
+#endif
     pidProfile->linear_thrust_high_output = linear_thrust_high_output;
     pidProfile->linear_throttle = linear_throttle;
     pidProfile->mixer_impl = mixer_impl;
@@ -230,7 +242,9 @@ static OSD_Entry cmsx_menuPidAdvancedEntries[] = {
     { "MIXER IMPL",        OME_TAB,   NULL, &(OSD_TAB_t)   { &mixer_impl, MIXER_IMPL_COUNT - 1, cms_mixerImplTypeLabels }, 0 },
     { "MIXER LAZINESS",    OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &mixer_laziness, 1, cms_offOnLabels }, 0 },
     { "MIXER YAW THR COMP", OME_TAB,   NULL, &(OSD_TAB_t)   { (uint8_t *) &mixer_yaw_throttle_comp, 1, cms_offOnLabels }, 0 },
-
+#ifdef USE_LULU
+    { "LULU N", OME_UINT8, NULL, &(OSD_UINT8_t) { &lulu_n_val, 0, 25,  1}, 0 },
+#endif
     { "SAVE&EXIT",         OME_OSD_Exit, cmsMenuExit,   (void *)CMS_EXIT_SAVE, 0},
     { "BACK",              OME_Back, NULL, NULL, 0 },
     { NULL,                OME_END, NULL, NULL, 0 }
@@ -788,11 +802,11 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) {
 static OSD_Entry cmsx_menuFilterPerProfileEntries[] = {
     { "-- FILTER PP  --", OME_Label, NULL, NULL, 0 },
 
-    { "DTERM LPF TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)    { (uint8_t *) &cmsx_dterm_lowpass_type, 4, cms_FilterType }, 0 },
+    { "DTERM LPF TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)    { (uint8_t *) &cmsx_dterm_lowpass_type, 5, cms_FilterType }, 0 },
     { "DTERM LPF ROLL",  OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_roll,     0, 500, 1 }, 0 },
     { "DTERM LPF PITCH",  OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_pitch,     0, 500, 1 }, 0 },
     { "DTERM LPF YAW",  OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass_hz_yaw,     0, 500, 1 }, 0 },
-    { "DTERM LPF2 TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)    { (uint8_t *) &cmsx_dterm_lowpass2_type, 4, cms_FilterType }, 0 },
+    { "DTERM LPF2 TYPE",    OME_TAB,    NULL, &(OSD_TAB_t)    { (uint8_t *) &cmsx_dterm_lowpass2_type, 5, cms_FilterType }, 0 },
     { "DTERM LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_roll,    0, 500, 1 }, 0 },
     { "DTERM LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_pitch,    0, 500, 1 }, 0 },
     { "DTERM LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_yaw,    0, 500, 1 }, 0 },

src/main/common/filter.h

@@ -61,6 +61,9 @@ typedef enum {
     FILTER_PT2,
     FILTER_PT3,
     FILTER_PT4,
+#ifdef USE_LULU
+    FILTER_LULU,
+#endif
 } lowpassFilterType_e;
 
 typedef enum {

src/main/common/lulu.c

@@ -0,0 +1,149 @@
+#include "lulu.h"
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <string.h>
+#include <math.h>
+
+#include "platform.h"
+
+#include "common/filter.h"
+#include "common/maths.h"
+#include "common/utils.h"
+
+#ifdef __ARM_ACLE
+#include <arm_acle.h>
+#endif /* __ARM_ACLE */
+#include <fenv.h>
+
+void luluFilterInit(luluFilter_t *filter, int N)
+{
+    if (N > 15)
+    {
+        N = 15;
+    }
+    if (N < 1)
+    {
+        N = 1;
+    }
+    filter->N = N;
+    filter->windowSize = filter->N * 2 + 1;
+    filter->windowBufIndex = 0;
+
+    memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
+    memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
+}
+
+FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
+{
+    register float curVal = 0;
+    register float curValB = 0;
+    for (int N = 1; N <= filterN; N++)
+    {
+        int indexNeg = (index + windowSize - 2 * N) % windowSize;
+        register int curIndex = (indexNeg + 1) % windowSize;
+        register float prevVal = series[indexNeg];
+        register float prevValB = seriesB[indexNeg];
+        register int indexPos = (curIndex + N) % windowSize;
+        for (int i = windowSize - 2 * N; i < windowSize - N; i++)
+        {
+            if (indexPos >= windowSize)
+            {
+                indexPos = 0;
+            }
+            if (curIndex >= windowSize)
+            {
+                curIndex = 0;
+            }
+
+            curVal = series[curIndex];
+            curValB = seriesB[curIndex];
+            register float nextVal = series[indexPos];
+            register float nextValB = seriesB[indexPos];
+
+            if (prevVal < curVal && curVal > nextVal)
+            {
+                float maxValue = MAX(prevVal, nextVal);
+
+                series[curIndex] = maxValue;
+            }
+
+            if (prevValB < curValB && curValB > nextValB)
+            {
+                float maxValue = MAX(prevValB, nextValB);
+
+                curVal = maxValue;
+                seriesB[curIndex] = maxValue;
+            }
+            prevVal = curVal;
+            prevValB = curValB;
+            curIndex++;
+            indexPos++;
+        }
+
+        curIndex = (indexNeg + 1) % windowSize;
+        prevVal = series[indexNeg];
+        prevValB = seriesB[indexNeg];
+        indexPos = (curIndex + N) % windowSize;
+        for (int i = windowSize - 2 * N; i < windowSize - N; i++)
+        {
+            if (indexPos >= windowSize)
+            {
+                indexPos = 0;
+            }
+            if (curIndex >= windowSize)
+            {
+                curIndex = 0;
+            }
+
+            curVal = series[curIndex];
+            curValB = seriesB[curIndex];
+            register float nextVal = series[indexPos];
+            register float nextValB = seriesB[indexPos];
+
+            if (prevVal > curVal && curVal < nextVal)
+            {
+                float minValue = MIN(prevVal, nextVal);
+
+                curVal = minValue;
+                series[curIndex] = minValue;
+            }
+
+            if (prevValB > curValB && curValB < nextValB)
+            {
+                float minValue = MIN(prevValB, nextValB);
+                curValB = minValue;
+                seriesB[curIndex] = minValue;
+            }
+            prevVal = curVal;
+            prevValB = curValB; 
+            curIndex++;
+            indexPos++;
+        }
+    }
+    return (curVal - curValB) / 2;
+}
+
+FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
+{
+    // This is the value N of the LULU filter.
+    register int filterN = filter->N;
+    // This is the total window size for the rolling buffer
+    register int filterWindow = filter->windowSize;
+
+    register int windowIndex = filter->windowBufIndex;
+    register float inputVal = input;
+    register int newIndex = (windowIndex + 1) % filterWindow;
+    filter->windowBufIndex = newIndex;
+    filter->luluInterim[windowIndex] = inputVal;
+    filter->luluInterimB[windowIndex] = -inputVal;
+    return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
+}
+
+FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
+{
+    // This is the UL filter
+    float resultA = luluFilterPartialApply(filter, input);
+    // We use the median interpretation of this filter to remove bias in the output
+    return resultA;
+}

src/main/common/lulu.h

@@ -0,0 +1,13 @@
+#pragma once
+
+//Max N = 15
+typedef struct {
+    int windowSize;
+    int windowBufIndex;
+    int N;
+    float luluInterim[32] __attribute__ ((aligned (128)));
+    float luluInterimB[32];
+} luluFilter_t;
+
+void luluFilterInit(luluFilter_t *filter, int N);
+float luluFilterApply(luluFilter_t *filter, float input);

src/main/fc/fc_core.c

@@ -33,6 +33,9 @@
 #include "common/filter.h"
 #include "common/maths.h"
 #include "common/utils.h"
+#ifdef USE_LULU
+#include "common/lulu.h"
+#endif
 
 #include "config/feature.h"
 #include "pg/pg.h"

src/main/flight/pid.c

@@ -31,6 +31,9 @@
 #include "common/axis.h"
 #include "common/maths.h"
 #include "common/filter.h"
+#ifdef USE_LULU
+#include "common/lulu.h"
+#endif
 
 #include "config/config_reset.h"
 #include "pg/pg.h"
@@ -197,6 +200,9 @@ void resetPidProfile(pidProfile_t *pidProfile) {
     .angle_filter = 100,
     .dtermDynNotch = false,
     .dterm_dyn_notch_q = 400,
+#ifdef USE_LULU
+    .lulu_n_val = 2
+#endif
                 );
 }
 
@@ -222,6 +228,7 @@ typedef union dtermLowpass_u {
     pt1Filter_t pt1Filter;
     biquadFilter_t biquadFilter;
     ptnFilter_t ptnFilter;
+    luluFilter_t luluFilter;
 } dtermLowpass_t;
 
 static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT];
@@ -274,6 +281,10 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
                 dtermLowpassApplyFn = (filterApplyFnPtr)ptnFilterApply;
                 ptnFilterInit(&dtermLowpass[axis].ptnFilter, FILTER_PT2, pidProfile->dFilter[axis].dLpf, dT);
                 break;
+            case FILTER_LULU:
+                dtermLowpassApplyFn = (filterApplyFnPtr)luluFilterApply;
+                luluFilterInit(&dtermLowpass[axis].luluFilter, pidProfile->lulu_n_val);
+                break;
             default: // case FILTER_PT1:
                 dtermLowpassApplyFn = (filterApplyFnPtr)pt1FilterApply;
                 pt1FilterInit(&dtermLowpass[axis].pt1Filter, pt1FilterGain(pidProfile->dFilter[axis].dLpf, dT));
@@ -299,6 +310,10 @@ void pidInitFilters(const pidProfile_t *pidProfile) {
                 dtermLowpass2ApplyFn = (filterApplyFnPtr)ptnFilterApply;
                 ptnFilterInit(&dtermLowpass2[axis].ptnFilter, FILTER_PT2, pidProfile->dFilter[axis].dLpf2, dT);
                 break;
+            case FILTER_LULU:
+                dtermLowpass2ApplyFn = (filterApplyFnPtr)luluFilterApply;
+                luluFilterInit(&dtermLowpass2[axis].luluFilter, pidProfile->lulu_n_val);
+                break;
             default: // case FILTER_PT1:
                 dtermLowpass2ApplyFn = (filterApplyFnPtr)pt1FilterApply;
                 pt1FilterInit(&dtermLowpass2[axis].pt1Filter, pt1FilterGain(pidProfile->dFilter[axis].dLpf2, dT));
@@ -823,8 +838,16 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
                 }
             }
 #endif
+            if(axis < 2) 
+            {
+                DEBUG_SET(DEBUG_LULU, axis, lrintf(dDelta));
+            }
             dDelta = dtermLowpassApplyFn((filter_t *)&dtermLowpass[axis], dDelta);
             dDelta = dtermLowpass2ApplyFn((filter_t *)&dtermLowpass2[axis], dDelta);
+            if(axis < 2) 
+            {
+                DEBUG_SET(DEBUG_LULU, axis + 2, lrintf(dDelta));
+            }
             dDelta = dtermABGapplyFn((filter_t *)&dtermABG[axis], dDelta);
             //dterm boost, similar to emuboost
             float boostedDtermRate;

src/main/flight/pid.h

@@ -157,6 +157,9 @@ typedef struct pidProfile_s {
     uint8_t dterm_ABG_half_life;
     uint8_t dtermDynNotch;
     uint16_t dterm_dyn_notch_q;
+#ifdef USE_LULU
+    uint8_t lulu_n_val;
+#endif
 } pidProfile_t;
 
 #ifndef USE_OSD_SLAVE

src/main/interface/settings.c

@@ -296,6 +296,9 @@ static const char * const lookupTableRcInterpolationChannels[] = {
 
 static const char * const lookupTableFilterType[] = {
     "PT1", "BIQUAD", "PT2", "PT3", "PT4",
+#ifdef USE_LULU
+    "LULU",
+#endif
 };
 
 #if defined(USE_GYRO_IMUF9001)
@@ -974,6 +977,9 @@ const clivalue_t valueTable[] = {
     { "mixer_impl",                VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_MIXER_IMPL_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_impl) },
     { "mixer_laziness",            VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_laziness) },
     { "mixer_yaw_throttle_comp",   VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, mixer_yaw_throttle_comp) },
+#ifdef USE_LULU
+    { "lulu_n_val", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 25 }, PG_PID_PROFILE, offsetof(pidProfile_t, lulu_n_val) },
+#endif
 
 
 // PG_TELEMETRY_CONFIG