Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerômetro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.
Dependencies: mbed MatrixMath Matrix nrf51_rtc BMP180 MPU9250
findPeaks.h@5:d87c25f009d1, 2019-07-27 (annotated)
- Committer:
- Rogercl
- Date:
- Sat Jul 27 12:17:25 2019 +0000
- Revision:
- 5:d87c25f009d1
PeakSearch working with Kalman. The best Treshold is 30
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Rogercl | 5:d87c25f009d1 | 1 | // INPUTS: |
Rogercl | 5:d87c25f009d1 | 2 | // Signal - A real vector from the maxima will be found (required) |
Rogercl | 5:d87c25f009d1 | 3 | // sel - The amount above surrounding data for a peak to be, |
Rogercl | 5:d87c25f009d1 | 4 | // identified (default = (max(x0)-min(x0))/4). Larger values mean |
Rogercl | 5:d87c25f009d1 | 5 | // the algorithm is more selective in finding peaks. |
Rogercl | 5:d87c25f009d1 | 6 | // thresh - A threshold value which peaks must be larger than to be |
Rogercl | 5:d87c25f009d1 | 7 | // maxima or smaller than to be minima. |
Rogercl | 5:d87c25f009d1 | 8 | int iiSearch=0; |
Rogercl | 5:d87c25f009d1 | 9 | #define SearchSize 60 |
Rogercl | 5:d87c25f009d1 | 10 | float Signal[SearchSize]; |
Rogercl | 5:d87c25f009d1 | 11 | float Threshold; |
Rogercl | 5:d87c25f009d1 | 12 | int steps=0; |
Rogercl | 5:d87c25f009d1 | 13 | |
Rogercl | 5:d87c25f009d1 | 14 | void diff(float (*x0)[SearchSize], float (*dx0)[SearchSize]) |
Rogercl | 5:d87c25f009d1 | 15 | { |
Rogercl | 5:d87c25f009d1 | 16 | for(int ii=1; ii<SearchSize; ii=ii+1) |
Rogercl | 5:d87c25f009d1 | 17 | { |
Rogercl | 5:d87c25f009d1 | 18 | (*dx0)[ii-1] = (*x0)[ii] - (*x0)[ii-1]; |
Rogercl | 5:d87c25f009d1 | 19 | } |
Rogercl | 5:d87c25f009d1 | 20 | |
Rogercl | 5:d87c25f009d1 | 21 | } |
Rogercl | 5:d87c25f009d1 | 22 | int findSig(float (*dx0)[SearchSize], int (*ind)[SearchSize]) |
Rogercl | 5:d87c25f009d1 | 23 | { |
Rogercl | 5:d87c25f009d1 | 24 | int jj=0; |
Rogercl | 5:d87c25f009d1 | 25 | for(int ii=1; ii<SearchSize; ii=ii+1) |
Rogercl | 5:d87c25f009d1 | 26 | { |
Rogercl | 5:d87c25f009d1 | 27 | if ((*dx0)[ii]>0) |
Rogercl | 5:d87c25f009d1 | 28 | { |
Rogercl | 5:d87c25f009d1 | 29 | if((*dx0)[ii-1]<0) |
Rogercl | 5:d87c25f009d1 | 30 | { |
Rogercl | 5:d87c25f009d1 | 31 | (*ind)[jj]=ii; |
Rogercl | 5:d87c25f009d1 | 32 | jj=jj+1; |
Rogercl | 5:d87c25f009d1 | 33 | } |
Rogercl | 5:d87c25f009d1 | 34 | } |
Rogercl | 5:d87c25f009d1 | 35 | else |
Rogercl | 5:d87c25f009d1 | 36 | { |
Rogercl | 5:d87c25f009d1 | 37 | if((*dx0)[ii-1]>0) |
Rogercl | 5:d87c25f009d1 | 38 | { |
Rogercl | 5:d87c25f009d1 | 39 | (*ind)[jj]=ii; |
Rogercl | 5:d87c25f009d1 | 40 | jj=jj+1; |
Rogercl | 5:d87c25f009d1 | 41 | } |
Rogercl | 5:d87c25f009d1 | 42 | } |
Rogercl | 5:d87c25f009d1 | 43 | } |
Rogercl | 5:d87c25f009d1 | 44 | return jj; |
Rogercl | 5:d87c25f009d1 | 45 | } |
Rogercl | 5:d87c25f009d1 | 46 | |
Rogercl | 5:d87c25f009d1 | 47 | int findPeaks(float Data, float sel, float thresh) //Signal /Sel/ Theshoold |
Rogercl | 5:d87c25f009d1 | 48 | { |
Rogercl | 5:d87c25f009d1 | 49 | if (iiSearch>=SearchSize) |
Rogercl | 5:d87c25f009d1 | 50 | { |
Rogercl | 5:d87c25f009d1 | 51 | for (int jjSearch=0;jjSearch<=SearchSize-2;jjSearch=jjSearch+1) |
Rogercl | 5:d87c25f009d1 | 52 | { |
Rogercl | 5:d87c25f009d1 | 53 | Signal[jjSearch]=Signal[jjSearch+1]; |
Rogercl | 5:d87c25f009d1 | 54 | } |
Rogercl | 5:d87c25f009d1 | 55 | Signal[SearchSize-1]=Data; |
Rogercl | 5:d87c25f009d1 | 56 | |
Rogercl | 5:d87c25f009d1 | 57 | |
Rogercl | 5:d87c25f009d1 | 58 | float (dx0)[SearchSize]; |
Rogercl | 5:d87c25f009d1 | 59 | diff(&Signal,&dx0); //Find derivative |
Rogercl | 5:d87c25f009d1 | 60 | |
Rogercl | 5:d87c25f009d1 | 61 | for (int ii=0; ii<SearchSize ; ii=ii+1) |
Rogercl | 5:d87c25f009d1 | 62 | { |
Rogercl | 5:d87c25f009d1 | 63 | if ((dx0)[ii]==0) |
Rogercl | 5:d87c25f009d1 | 64 | { |
Rogercl | 5:d87c25f009d1 | 65 | (dx0)[ii]=2.22044604925031e-16; |
Rogercl | 5:d87c25f009d1 | 66 | } |
Rogercl | 5:d87c25f009d1 | 67 | } |
Rogercl | 5:d87c25f009d1 | 68 | int pointsFound=0; |
Rogercl | 5:d87c25f009d1 | 69 | int ind[SearchSize]; |
Rogercl | 5:d87c25f009d1 | 70 | pointsFound=findSig(&dx0,&ind); |
Rogercl | 5:d87c25f009d1 | 71 | if (pointsFound==0) |
Rogercl | 5:d87c25f009d1 | 72 | { |
Rogercl | 5:d87c25f009d1 | 73 | return 0; |
Rogercl | 5:d87c25f009d1 | 74 | } |
Rogercl | 5:d87c25f009d1 | 75 | float x[pointsFound]; |
Rogercl | 5:d87c25f009d1 | 76 | for (int ii=0; ii<pointsFound; ii=ii+1) |
Rogercl | 5:d87c25f009d1 | 77 | { |
Rogercl | 5:d87c25f009d1 | 78 | int aux=ind[ii]; |
Rogercl | 5:d87c25f009d1 | 79 | x[ii]=(Signal)[aux]; |
Rogercl | 5:d87c25f009d1 | 80 | } |
Rogercl | 5:d87c25f009d1 | 81 | //finding the minimal value |
Rogercl | 5:d87c25f009d1 | 82 | float minMag=x[0]; |
Rogercl | 5:d87c25f009d1 | 83 | for (int ii=1; ii<pointsFound; ii=ii+1) |
Rogercl | 5:d87c25f009d1 | 84 | { |
Rogercl | 5:d87c25f009d1 | 85 | if (minMag>x[ii]) |
Rogercl | 5:d87c25f009d1 | 86 | { |
Rogercl | 5:d87c25f009d1 | 87 | minMag=x[ii]; |
Rogercl | 5:d87c25f009d1 | 88 | } |
Rogercl | 5:d87c25f009d1 | 89 | } |
Rogercl | 5:d87c25f009d1 | 90 | float leftMin; |
Rogercl | 5:d87c25f009d1 | 91 | if (x[0]>(Signal)[0]) |
Rogercl | 5:d87c25f009d1 | 92 | { |
Rogercl | 5:d87c25f009d1 | 93 | leftMin = (Signal)[0]; |
Rogercl | 5:d87c25f009d1 | 94 | } |
Rogercl | 5:d87c25f009d1 | 95 | else |
Rogercl | 5:d87c25f009d1 | 96 | { |
Rogercl | 5:d87c25f009d1 | 97 | leftMin = x[0]; |
Rogercl | 5:d87c25f009d1 | 98 | } |
Rogercl | 5:d87c25f009d1 | 99 | |
Rogercl | 5:d87c25f009d1 | 100 | //struct Peaks PeaksFound; |
Rogercl | 5:d87c25f009d1 | 101 | int PeakCount=0; |
Rogercl | 5:d87c25f009d1 | 102 | |
Rogercl | 5:d87c25f009d1 | 103 | // x only has the peaks, valleys, and possibly endpoints |
Rogercl | 5:d87c25f009d1 | 104 | if (pointsFound > 2) |
Rogercl | 5:d87c25f009d1 | 105 | { |
Rogercl | 5:d87c25f009d1 | 106 | float tempMag=minMag; |
Rogercl | 5:d87c25f009d1 | 107 | int tempLoc; |
Rogercl | 5:d87c25f009d1 | 108 | int foundPeak=0; |
Rogercl | 5:d87c25f009d1 | 109 | //Skip the first point if it is smaller so we always start on a maxima |
Rogercl | 5:d87c25f009d1 | 110 | int xx; |
Rogercl | 5:d87c25f009d1 | 111 | if (x[0] >= x[1]) |
Rogercl | 5:d87c25f009d1 | 112 | { |
Rogercl | 5:d87c25f009d1 | 113 | xx=-1; |
Rogercl | 5:d87c25f009d1 | 114 | } |
Rogercl | 5:d87c25f009d1 | 115 | else |
Rogercl | 5:d87c25f009d1 | 116 | { |
Rogercl | 5:d87c25f009d1 | 117 | xx=0; |
Rogercl | 5:d87c25f009d1 | 118 | } |
Rogercl | 5:d87c25f009d1 | 119 | |
Rogercl | 5:d87c25f009d1 | 120 | //Loop through extrema which should be peaks and then valleys |
Rogercl | 5:d87c25f009d1 | 121 | while (xx<pointsFound-1) |
Rogercl | 5:d87c25f009d1 | 122 | { |
Rogercl | 5:d87c25f009d1 | 123 | xx=xx+1; //This is a peak |
Rogercl | 5:d87c25f009d1 | 124 | //Reset peak finding if we had a peak and the next peak is bigger |
Rogercl | 5:d87c25f009d1 | 125 | //than the last or the left min was small enough to reset. |
Rogercl | 5:d87c25f009d1 | 126 | if (foundPeak==1) |
Rogercl | 5:d87c25f009d1 | 127 | { |
Rogercl | 5:d87c25f009d1 | 128 | tempMag=minMag; |
Rogercl | 5:d87c25f009d1 | 129 | foundPeak=0; |
Rogercl | 5:d87c25f009d1 | 130 | } |
Rogercl | 5:d87c25f009d1 | 131 | //Found new peak that was lager than temp mag and selectivity larger |
Rogercl | 5:d87c25f009d1 | 132 | //than the minimum to its left. |
Rogercl | 5:d87c25f009d1 | 133 | if (x[xx] > tempMag && x[xx] > leftMin + sel && x[xx]>thresh) |
Rogercl | 5:d87c25f009d1 | 134 | { |
Rogercl | 5:d87c25f009d1 | 135 | tempLoc=xx; |
Rogercl | 5:d87c25f009d1 | 136 | tempMag=x[xx]; |
Rogercl | 5:d87c25f009d1 | 137 | } |
Rogercl | 5:d87c25f009d1 | 138 | //Make sure we don't iterate past the length of our vector |
Rogercl | 5:d87c25f009d1 | 139 | if (xx==(pointsFound-1)) |
Rogercl | 5:d87c25f009d1 | 140 | { |
Rogercl | 5:d87c25f009d1 | 141 | break; |
Rogercl | 5:d87c25f009d1 | 142 | } |
Rogercl | 5:d87c25f009d1 | 143 | xx=xx+1; //Move onto the valley |
Rogercl | 5:d87c25f009d1 | 144 | //Come down at least sel from peak && x[xx]>thresh && tempMag>thresh && tempMag>thresh && (Signal)[ii]>thresh |
Rogercl | 5:d87c25f009d1 | 145 | if (foundPeak==0 && tempMag > sel+x[xx] && tempMag>thresh) |
Rogercl | 5:d87c25f009d1 | 146 | { |
Rogercl | 5:d87c25f009d1 | 147 | foundPeak=1; |
Rogercl | 5:d87c25f009d1 | 148 | leftMin=x[xx]; |
Rogercl | 5:d87c25f009d1 | 149 | PeakCount=PeakCount+1; |
Rogercl | 5:d87c25f009d1 | 150 | } |
Rogercl | 5:d87c25f009d1 | 151 | else if (x[xx]<leftMin) |
Rogercl | 5:d87c25f009d1 | 152 | { |
Rogercl | 5:d87c25f009d1 | 153 | leftMin=x[xx]; |
Rogercl | 5:d87c25f009d1 | 154 | } |
Rogercl | 5:d87c25f009d1 | 155 | } |
Rogercl | 5:d87c25f009d1 | 156 | if (foundPeak==0) |
Rogercl | 5:d87c25f009d1 | 157 | { |
Rogercl | 5:d87c25f009d1 | 158 | float auxmin; //find min between min(x0(end), x(end)) |
Rogercl | 5:d87c25f009d1 | 159 | if ((Signal)[SearchSize-1]<x[pointsFound-1]) |
Rogercl | 5:d87c25f009d1 | 160 | { |
Rogercl | 5:d87c25f009d1 | 161 | auxmin=(Signal)[SearchSize-1]; |
Rogercl | 5:d87c25f009d1 | 162 | } |
Rogercl | 5:d87c25f009d1 | 163 | else |
Rogercl | 5:d87c25f009d1 | 164 | { |
Rogercl | 5:d87c25f009d1 | 165 | auxmin=x[pointsFound-1]; |
Rogercl | 5:d87c25f009d1 | 166 | } |
Rogercl | 5:d87c25f009d1 | 167 | |
Rogercl | 5:d87c25f009d1 | 168 | if (x[pointsFound-1] > tempMag && x[pointsFound-1] > leftMin + sel ) |
Rogercl | 5:d87c25f009d1 | 169 | { |
Rogercl | 5:d87c25f009d1 | 170 | PeakCount=PeakCount+1; |
Rogercl | 5:d87c25f009d1 | 171 | } |
Rogercl | 5:d87c25f009d1 | 172 | else if (tempMag > auxmin + sel && tempMag>thresh) |
Rogercl | 5:d87c25f009d1 | 173 | { |
Rogercl | 5:d87c25f009d1 | 174 | PeakCount=PeakCount+1; |
Rogercl | 5:d87c25f009d1 | 175 | } |
Rogercl | 5:d87c25f009d1 | 176 | } |
Rogercl | 5:d87c25f009d1 | 177 | |
Rogercl | 5:d87c25f009d1 | 178 | } |
Rogercl | 5:d87c25f009d1 | 179 | else |
Rogercl | 5:d87c25f009d1 | 180 | { |
Rogercl | 5:d87c25f009d1 | 181 | float auxMaxMag; |
Rogercl | 5:d87c25f009d1 | 182 | if (pointsFound==2) |
Rogercl | 5:d87c25f009d1 | 183 | { |
Rogercl | 5:d87c25f009d1 | 184 | int auxloc1=ind[0]; |
Rogercl | 5:d87c25f009d1 | 185 | int auxloc2=ind[1]; |
Rogercl | 5:d87c25f009d1 | 186 | if ((Signal)[auxloc1] > (Signal)[auxloc2] ) |
Rogercl | 5:d87c25f009d1 | 187 | { |
Rogercl | 5:d87c25f009d1 | 188 | auxMaxMag=(Signal)[auxloc1]; |
Rogercl | 5:d87c25f009d1 | 189 | } |
Rogercl | 5:d87c25f009d1 | 190 | else |
Rogercl | 5:d87c25f009d1 | 191 | { |
Rogercl | 5:d87c25f009d1 | 192 | auxMaxMag=(Signal)[auxloc2]; |
Rogercl | 5:d87c25f009d1 | 193 | } |
Rogercl | 5:d87c25f009d1 | 194 | } |
Rogercl | 5:d87c25f009d1 | 195 | else |
Rogercl | 5:d87c25f009d1 | 196 | { |
Rogercl | 5:d87c25f009d1 | 197 | int auxloc1=ind[0]; |
Rogercl | 5:d87c25f009d1 | 198 | auxMaxMag=(Signal)[auxloc1]; |
Rogercl | 5:d87c25f009d1 | 199 | } |
Rogercl | 5:d87c25f009d1 | 200 | |
Rogercl | 5:d87c25f009d1 | 201 | if (auxMaxMag>=thresh) |
Rogercl | 5:d87c25f009d1 | 202 | { |
Rogercl | 5:d87c25f009d1 | 203 | PeakCount=1; |
Rogercl | 5:d87c25f009d1 | 204 | } |
Rogercl | 5:d87c25f009d1 | 205 | } |
Rogercl | 5:d87c25f009d1 | 206 | return PeakCount; |
Rogercl | 5:d87c25f009d1 | 207 | } |
Rogercl | 5:d87c25f009d1 | 208 | else |
Rogercl | 5:d87c25f009d1 | 209 | { |
Rogercl | 5:d87c25f009d1 | 210 | Signal[iiSearch]=Data; |
Rogercl | 5:d87c25f009d1 | 211 | iiSearch=iiSearch+1; |
Rogercl | 5:d87c25f009d1 | 212 | return 0; |
Rogercl | 5:d87c25f009d1 | 213 | } |
Rogercl | 5:d87c25f009d1 | 214 | } |