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

Inatividade.h@6:e9a2bc040ada, 2019-08-04 (annotated)

Committer:
Rogercl
Date:
Sun Aug 04 11:38:08 2019 +0000
Revision:
6:e9a2bc040ada
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 6:e9a2bc040ada 1 //variaveis inatividade
Rogercl 6:e9a2bc040ada 2
Rogercl 6:e9a2bc040ada 3 float normMean = 9.5;
Rogercl 6:e9a2bc040ada 4 float stdMean = 0.03;
Rogercl 6:e9a2bc040ada 5 int i=0;
Rogercl 6:e9a2bc040ada 6 float norma=0;
Rogercl 6:e9a2bc040ada 7 float desvio=0;
Rogercl 6:e9a2bc040ada 8 int contador_inativ=0;
Rogercl 6:e9a2bc040ada 9 int contador_desvio=0;
Rogercl 6:e9a2bc040ada 10 int flagInativ=0;
Rogercl 6:e9a2bc040ada 11 float AcelNormA=0;
Rogercl 6:e9a2bc040ada 12
Rogercl 6:e9a2bc040ada 13 void Inatividade()
Rogercl 6:e9a2bc040ada 14 {
Rogercl 6:e9a2bc040ada 15 //variaveis inatividade
Rogercl 6:e9a2bc040ada 16 float ACEL_CONV=9.807;
Rogercl 6:e9a2bc040ada 17 float window_filt = 10;
Rogercl 6:e9a2bc040ada 18 float window = Freq*2;
Rogercl 6:e9a2bc040ada 19 float mult_norm_stdMean = 2;
Rogercl 6:e9a2bc040ada 20 float config_stdMean = 0.03;
Rogercl 6:e9a2bc040ada 21 float AcelNorm=sqrt(ax*ax + ay*ay+ az*az)*ACEL_CONV;
Rogercl 6:e9a2bc040ada 22
Rogercl 6:e9a2bc040ada 23 //Identifica inativo / ativo
Rogercl 6:e9a2bc040ada 24 if (i<(window_filt-1))
Rogercl 6:e9a2bc040ada 25 {
Rogercl 6:e9a2bc040ada 26 //norma = norma + vin[i]/window_filt;
Rogercl 6:e9a2bc040ada 27 norma = norma + AcelNorm/window_filt;
Rogercl 6:e9a2bc040ada 28 }
Rogercl 6:e9a2bc040ada 29 else
Rogercl 6:e9a2bc040ada 30 {
Rogercl 6:e9a2bc040ada 31 //norma = ((window_filt - 1)/window_filt) * norma + vin[i]/window_filt;
Rogercl 6:e9a2bc040ada 32 norma = ((window_filt - 1)/window_filt) * norma + AcelNorm/window_filt;
Rogercl 6:e9a2bc040ada 33 }
Rogercl 6:e9a2bc040ada 34
Rogercl 6:e9a2bc040ada 35 if (i>=window)
Rogercl 6:e9a2bc040ada 36 {
Rogercl 6:e9a2bc040ada 37 //INATIVO
Rogercl 6:e9a2bc040ada 38 if (!(abs(norma - normMean) > mult_norm_stdMean * stdMean)) //barreira de desvio / norma
Rogercl 6:e9a2bc040ada 39 {
Rogercl 6:e9a2bc040ada 40 contador_inativ = contador_inativ + 1;
Rogercl 6:e9a2bc040ada 41
Rogercl 6:e9a2bc040ada 42 //INATIVO
Rogercl 6:e9a2bc040ada 43 if (contador_inativ>=Freq)
Rogercl 6:e9a2bc040ada 44 {
Rogercl 6:e9a2bc040ada 45 flagInativ=1;
Rogercl 6:e9a2bc040ada 46 iiSearch=0;
Rogercl 6:e9a2bc040ada 47 }
Rogercl 6:e9a2bc040ada 48 }
Rogercl 6:e9a2bc040ada 49 //ATIVO
Rogercl 6:e9a2bc040ada 50 else
Rogercl 6:e9a2bc040ada 51 {
Rogercl 6:e9a2bc040ada 52 flagInativ=0;
Rogercl 6:e9a2bc040ada 53 contador_inativ=0;
Rogercl 6:e9a2bc040ada 54 }
Rogercl 6:e9a2bc040ada 55
Rogercl 6:e9a2bc040ada 56 }
Rogercl 6:e9a2bc040ada 57
Rogercl 6:e9a2bc040ada 58 //desvio avg - corrige/adapta os paramentros de threshold da norma e std
Rogercl 6:e9a2bc040ada 59 if (i>0)
Rogercl 6:e9a2bc040ada 60 {
Rogercl 6:e9a2bc040ada 61 if (i<=(window_filt-1))
Rogercl 6:e9a2bc040ada 62 {
Rogercl 6:e9a2bc040ada 63 //desvio = desvio + abs(vin(i)-vin(i-1))/window_filt;
Rogercl 6:e9a2bc040ada 64 desvio = desvio + abs(AcelNorm-AcelNormA)/window_filt;
Rogercl 6:e9a2bc040ada 65 }
Rogercl 6:e9a2bc040ada 66 else
Rogercl 6:e9a2bc040ada 67 {
Rogercl 6:e9a2bc040ada 68 //desvio = ((window_filt - 1)/window_filt)*desvio + abs(vin(i)-vin(i-1))/window_filt;
Rogercl 6:e9a2bc040ada 69 desvio = ((window_filt - 1)/window_filt)*desvio + abs(AcelNorm-AcelNormA)/window_filt;
Rogercl 6:e9a2bc040ada 70 }
Rogercl 6:e9a2bc040ada 71 }
Rogercl 6:e9a2bc040ada 72
Rogercl 6:e9a2bc040ada 73 //Desvio
Rogercl 6:e9a2bc040ada 74 if (i>=window)
Rogercl 6:e9a2bc040ada 75 {
Rogercl 6:e9a2bc040ada 76 if (!(abs(desvio - stdMean) > stdMean)) // barreira de desvio / norma
Rogercl 6:e9a2bc040ada 77 {
Rogercl 6:e9a2bc040ada 78 contador_desvio = contador_desvio + 1;
Rogercl 6:e9a2bc040ada 79 if ((contador_desvio >= window/2) && (contador_desvio < 2*window))
Rogercl 6:e9a2bc040ada 80 {
Rogercl 6:e9a2bc040ada 81 normMean = 0.95 * normMean + 0.05 * norma;
Rogercl 6:e9a2bc040ada 82 }
Rogercl 6:e9a2bc040ada 83 else if (contador_desvio >= 2*window)
Rogercl 6:e9a2bc040ada 84 {
Rogercl 6:e9a2bc040ada 85 normMean = 0.85 * normMean + 0.15 * norma;
Rogercl 6:e9a2bc040ada 86 if (flagInativ)
Rogercl 6:e9a2bc040ada 87 {
Rogercl 6:e9a2bc040ada 88 stdMean = 0.95 * stdMean + 0.05 * desvio;
Rogercl 6:e9a2bc040ada 89
Rogercl 6:e9a2bc040ada 90 // limitante superior
Rogercl 6:e9a2bc040ada 91 if (stdMean<=2*config_stdMean) stdMean=stdMean;
Rogercl 6:e9a2bc040ada 92 else stdMean=2*config_stdMean;
Rogercl 6:e9a2bc040ada 93
Rogercl 6:e9a2bc040ada 94 // limitante inferior
Rogercl 6:e9a2bc040ada 95 if (stdMean>=config_stdMean/2) stdMean=stdMean;
Rogercl 6:e9a2bc040ada 96 else stdMean=config_stdMean/2;
Rogercl 6:e9a2bc040ada 97 }
Rogercl 6:e9a2bc040ada 98 }
Rogercl 6:e9a2bc040ada 99 }
Rogercl 6:e9a2bc040ada 100 else
Rogercl 6:e9a2bc040ada 101 {
Rogercl 6:e9a2bc040ada 102 contador_desvio = 0;
Rogercl 6:e9a2bc040ada 103 }
Rogercl 6:e9a2bc040ada 104 }
Rogercl 6:e9a2bc040ada 105
Rogercl 6:e9a2bc040ada 106 AcelNormA=AcelNorm;
Rogercl 6:e9a2bc040ada 107 i=i+1;
Rogercl 6:e9a2bc040ada 108 }