Roger Lopes / Mbed 2 deprecated MARe_VT_ble_PeakSearch_Kalman_Inativ

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@6:e9a2bc040ada, 2019-08-04 (annotated)

Committer:
Rogercl
Date:
Sun Aug 04 11:38:08 2019 +0000
Revision:
6:e9a2bc040ada
Parent:
5:d87c25f009d1 
Algoritmo funcionando com a biblioteca de inatividade utilizando dos dados do acelerometro e a biblioteca de PeakSearch se utilizando dos dados filtrados pelo filtro Kalman.

Who changed what in which revision?

UserRevisionLine numberNew 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 }