src/main/common/lulu.c

   1 #include "lulu.h"
   2
   3 #include <stdbool.h>
   4 #include <stdint.h>
   5 #include <string.h>
   6 #include <math.h>
   7
   8 #include "platform.h"
   9
  10 #include "common/filter.h"
  11 #include "common/maths.h"
  12 #include "common/utils.h"
  13
  14 #ifdef __ARM_ACLE
  15 #include <arm_acle.h>
  16 #endif /* __ARM_ACLE */
  17 #include <fenv.h>
  18
  19 void luluFilterInit(luluFilter_t *filter, int N)
  20 {
  21         filter->N = constrain(N, 1, 15);
  22         filter->windowSize = filter->N * 2 + 1;
  23         filter->windowBufIndex = 0;
  24
  25         memset(filter->luluInterim, 0, sizeof(float) * (filter->windowSize));
  26         memset(filter->luluInterimB, 0, sizeof(float) * (filter->windowSize));
  27 }
  28
  29 FAST_CODE float fixRoad(float *series, float *seriesB, int index, int filterN, int windowSize)
  30 {
  31         float curVal = 0;
  32         float curValB = 0;
  33         for (int N = 1; N <= filterN; N++)
  34         {
  35                 int indexNeg = (index + windowSize - 2 * N) % windowSize;
  36             int curIndex = (indexNeg + 1) % windowSize;
  37             float prevVal = series[indexNeg];
  38                 float prevValB = seriesB[indexNeg];
  39                 int indexPos = (curIndex + N) % windowSize;
  40
  41         for (int i = windowSize - 2 * N; i < windowSize - N; i++)
  42                 {
  43                         if (indexPos >= windowSize)
  44                         {
  45                                 indexPos = 0;
  46                         }
  47                         if (curIndex >= windowSize)
  48                         {
  49                                 curIndex = 0;
  50                         }
  51                         // curIndex = (2 - 1) % 3 = 1
  52                         curVal = series[curIndex];
  53                         curValB = seriesB[curIndex];
  54                         float nextVal = series[indexPos];
  55                         float nextValB = seriesB[indexPos];
  56                         // onbump (s, 1, 1, 3)
  57                         // if(onBump(series, curIndex, N, windowSize))
  58                         if (prevVal < curVal && curVal > nextVal)
  59                         {
  60                                 float maxValue = MAX(prevVal, nextVal);
  61
  62                                 series[curIndex] = maxValue;
  63                             int k = curIndex;
  64                                 for (int j = 1; j < N; j++)
  65                                 {
  66                                         if (++k >= windowSize)
  67                                         {
  68                                                 k = 0;
  69                                         }
  70                                         series[k] = maxValue;
  71                                 }
  72                         }
  73
  74                         if (prevValB < curValB && curValB > nextValB)
  75                         {
  76                                 float maxValue = MAX(prevValB, nextValB);
  77
  78                                 curVal = maxValue;
  79                                 seriesB[curIndex] = maxValue;
  80                                 int k = curIndex;
  81                                 for (int j = 1; j < N; j++)
  82                                 {
  83                                         if (++k >= windowSize)
  84                                         {
  85                                                 k = 0;
  86                                         }
  87                                         seriesB[k] = maxValue;
  88                                 }
  89                         }
  90                         prevVal = curVal;
  91                         prevValB = curValB;
  92                         curIndex++;
  93                         indexPos++;
  94                 }
  95
  96                 curIndex = (indexNeg + 1) % windowSize;
  97                 prevVal = series[indexNeg];
  98                 prevValB = seriesB[indexNeg];
  99                 indexPos = (curIndex + N) % windowSize;
 100                 for (int i = windowSize - 2 * N; i < windowSize - N; i++)
 101                 {
 102                         if (indexPos >= windowSize)
 103                         {
 104                                 indexPos = 0;
 105                         }
 106                         if (curIndex >= windowSize)
 107                         {
 108                                 curIndex = 0;
 109                         }
 110                         // curIndex = (2 - 1) % 3 = 1
 111                         curVal = series[curIndex];
 112                         curValB = seriesB[curIndex];
 113                         float nextVal = series[indexPos];
 114                         float nextValB = seriesB[indexPos];
 115
 116                         if (prevVal > curVal && curVal < nextVal)
 117                         {
 118                                 float minValue = MIN(prevVal, nextVal);
 119
 120                                 curVal = minValue;
 121                                 series[curIndex] = minValue;
 122                                 int k = curIndex;
 123                                 for (int j = 1; j < N; j++)
 124                                 {
 125                                         if (++k >= windowSize)
 126                                         {
 127                                                 k = 0;
 128                                         }
 129                                         series[k] = minValue;
 130                                 }
 131                         }
 132
 133                         if (prevValB > curValB && curValB < nextValB)
 134                         {
 135                                 float minValue = MIN(prevValB, nextValB);
 136                                 curValB = minValue;
 137                                 seriesB[curIndex] = minValue;
 138                                 int k = curIndex;
 139                                 for (int j = 1; j < N; j++)
 140                                 {
 141                                         if (++k >= windowSize)
 142                                         {
 143                                                 k = 0;
 144                                         }
 145                                         seriesB[k] = minValue;
 146                                 }
 147                         }
 148                         prevVal = curVal;
 149                         prevValB = curValB;
 150                         curIndex++;
 151                         indexPos++;
 152                 }
 153         }
 154         return (curVal - curValB) / 2;
 155 }
 156
 157 FAST_CODE float luluFilterPartialApply(luluFilter_t *filter, float input)
 158 {
 159         // This is the value N of the LULU filter.
 160         int filterN = filter->N;
 161         // This is the total window size for the rolling buffer
 162         int filterWindow = filter->windowSize;
 163
 164         int windowIndex = filter->windowBufIndex;
 165         float inputVal = input;
 166         int newIndex = (windowIndex + 1) % filterWindow;
 167         filter->windowBufIndex = newIndex;
 168         filter->luluInterim[windowIndex] = inputVal;
 169         filter->luluInterimB[windowIndex] = -inputVal;
 170         return fixRoad(filter->luluInterim, filter->luluInterimB, windowIndex, filterN, filterWindow);
 171 }
 172
 173 FAST_CODE float luluFilterApply(luluFilter_t *filter, float input)
 174 {
 175         // This is the UL filter
 176         float resultA = luluFilterPartialApply(filter, input);
 177         // We use the median interpretation of this filter to remove bias in the output
 178         return resultA;
 179 }