hormone v.1 for optimizing COT
Dependents: TurtleBot_with_AHS
Fork of calculator by
calculator.cpp@0:22ebaf70b60e, 2018-08-13 (annotated)
- Committer:
- worasuchad
- Date:
- Mon Aug 13 11:38:52 2018 +0000
- Revision:
- 0:22ebaf70b60e
- Child:
- 1:b6345fbad095
hormone v.1 for optimizing COT
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
worasuchad | 0:22ebaf70b60e | 1 | #include "calculator.h" |
worasuchad | 0:22ebaf70b60e | 2 | #include "mbed.h" |
worasuchad | 0:22ebaf70b60e | 3 | #include <stdlib.h> |
worasuchad | 0:22ebaf70b60e | 4 | |
worasuchad | 0:22ebaf70b60e | 5 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:22ebaf70b60e | 6 | /* NAME : constructor */ |
worasuchad | 0:22ebaf70b60e | 7 | /* PARAMETERS : - */ |
worasuchad | 0:22ebaf70b60e | 8 | /* RETURN VALUE : - */ |
worasuchad | 0:22ebaf70b60e | 9 | /* DESCRIPTION : - */ |
worasuchad | 0:22ebaf70b60e | 10 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 11 | calculator::calculator() |
worasuchad | 0:22ebaf70b60e | 12 | { |
worasuchad | 0:22ebaf70b60e | 13 | |
worasuchad | 0:22ebaf70b60e | 14 | } |
worasuchad | 0:22ebaf70b60e | 15 | |
worasuchad | 0:22ebaf70b60e | 16 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:22ebaf70b60e | 17 | /* NAME : calculate SD */ |
worasuchad | 0:22ebaf70b60e | 18 | /* PARAMETERS : - */ |
worasuchad | 0:22ebaf70b60e | 19 | /* RETURN VALUE : SD value */ |
worasuchad | 0:22ebaf70b60e | 20 | /* DESCRIPTION : - */ |
worasuchad | 0:22ebaf70b60e | 21 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 22 | float calculator::calculateSD(const float *sdData, int size) |
worasuchad | 0:22ebaf70b60e | 23 | { |
worasuchad | 0:22ebaf70b60e | 24 | float sum = 0.0, mean, standardDeviation = 0.0; |
worasuchad | 0:22ebaf70b60e | 25 | int i; |
worasuchad | 0:22ebaf70b60e | 26 | |
worasuchad | 0:22ebaf70b60e | 27 | if(!sdData) |
worasuchad | 0:22ebaf70b60e | 28 | { |
worasuchad | 0:22ebaf70b60e | 29 | return NULL; |
worasuchad | 0:22ebaf70b60e | 30 | } |
worasuchad | 0:22ebaf70b60e | 31 | |
worasuchad | 0:22ebaf70b60e | 32 | for(i = 0; i < size ; i++) |
worasuchad | 0:22ebaf70b60e | 33 | { |
worasuchad | 0:22ebaf70b60e | 34 | sum += sdData[i]; |
worasuchad | 0:22ebaf70b60e | 35 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 0:22ebaf70b60e | 36 | } |
worasuchad | 0:22ebaf70b60e | 37 | mean = sum/size; |
worasuchad | 0:22ebaf70b60e | 38 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 0:22ebaf70b60e | 39 | |
worasuchad | 0:22ebaf70b60e | 40 | for(i = 0; i < size; i++) |
worasuchad | 0:22ebaf70b60e | 41 | { |
worasuchad | 0:22ebaf70b60e | 42 | standardDeviation += pow(sdData[i] - mean, 2); |
worasuchad | 0:22ebaf70b60e | 43 | //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); |
worasuchad | 0:22ebaf70b60e | 44 | } |
worasuchad | 0:22ebaf70b60e | 45 | return sqrt(standardDeviation / size); |
worasuchad | 0:22ebaf70b60e | 46 | } |
worasuchad | 0:22ebaf70b60e | 47 | |
worasuchad | 0:22ebaf70b60e | 48 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:22ebaf70b60e | 49 | /* NAME : calculate mean */ |
worasuchad | 0:22ebaf70b60e | 50 | /* PARAMETERS : - */ |
worasuchad | 0:22ebaf70b60e | 51 | /* RETURN VALUE : mean value */ |
worasuchad | 0:22ebaf70b60e | 52 | /* DESCRIPTION : - */ |
worasuchad | 0:22ebaf70b60e | 53 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 54 | float calculator::calculateMean(const float *meanData, int size) |
worasuchad | 0:22ebaf70b60e | 55 | { |
worasuchad | 0:22ebaf70b60e | 56 | float sum = 0.0, mean; |
worasuchad | 0:22ebaf70b60e | 57 | int i; |
worasuchad | 0:22ebaf70b60e | 58 | |
worasuchad | 0:22ebaf70b60e | 59 | if(!meanData) |
worasuchad | 0:22ebaf70b60e | 60 | { |
worasuchad | 0:22ebaf70b60e | 61 | return NULL; |
worasuchad | 0:22ebaf70b60e | 62 | } |
worasuchad | 0:22ebaf70b60e | 63 | |
worasuchad | 0:22ebaf70b60e | 64 | for(i = 0; i < size ; ++i) |
worasuchad | 0:22ebaf70b60e | 65 | { |
worasuchad | 0:22ebaf70b60e | 66 | sum += meanData[i]; |
worasuchad | 0:22ebaf70b60e | 67 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 0:22ebaf70b60e | 68 | } |
worasuchad | 0:22ebaf70b60e | 69 | mean = sum/size; |
worasuchad | 0:22ebaf70b60e | 70 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 0:22ebaf70b60e | 71 | return mean; |
worasuchad | 0:22ebaf70b60e | 72 | } |
worasuchad | 0:22ebaf70b60e | 73 | |
worasuchad | 0:22ebaf70b60e | 74 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:22ebaf70b60e | 75 | /* NAME : calculate size of vector A(Ax, Ay, Az) in State 2,3 */ |
worasuchad | 0:22ebaf70b60e | 76 | /* PARAMETERS : - */ |
worasuchad | 0:22ebaf70b60e | 77 | /* RETURN VALUE : size value */ |
worasuchad | 0:22ebaf70b60e | 78 | /* DESCRIPTION : - */ |
worasuchad | 0:22ebaf70b60e | 79 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 80 | float calculator::calcSDVectorA(const float *arrAx, const float *arrAy, const float *arrAz, int size) |
worasuchad | 0:22ebaf70b60e | 81 | { |
worasuchad | 0:22ebaf70b60e | 82 | float sum = 0.0, mean = 0.0; |
worasuchad | 0:22ebaf70b60e | 83 | float standardDeviation = 0.0; |
worasuchad | 0:22ebaf70b60e | 84 | int i; |
worasuchad | 0:22ebaf70b60e | 85 | float *vSize; |
worasuchad | 0:22ebaf70b60e | 86 | |
worasuchad | 0:22ebaf70b60e | 87 | /* check arg is not empty */ |
worasuchad | 0:22ebaf70b60e | 88 | if(!arrAx or !arrAy or !arrAz) |
worasuchad | 0:22ebaf70b60e | 89 | { |
worasuchad | 0:22ebaf70b60e | 90 | return NULL; |
worasuchad | 0:22ebaf70b60e | 91 | } |
worasuchad | 0:22ebaf70b60e | 92 | |
worasuchad | 0:22ebaf70b60e | 93 | vSize = (float *)malloc(size*sizeof(float)); |
worasuchad | 0:22ebaf70b60e | 94 | /* check malloc is not return NULL */ |
worasuchad | 0:22ebaf70b60e | 95 | if( vSize == NULL ) |
worasuchad | 0:22ebaf70b60e | 96 | { |
worasuchad | 0:22ebaf70b60e | 97 | return NULL; |
worasuchad | 0:22ebaf70b60e | 98 | } |
worasuchad | 0:22ebaf70b60e | 99 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 100 | /* calculate size of vector */ |
worasuchad | 0:22ebaf70b60e | 101 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 102 | for(i = 0; i < size; i++) |
worasuchad | 0:22ebaf70b60e | 103 | { |
worasuchad | 0:22ebaf70b60e | 104 | *(vSize + i) = sqrt( pow(arrAx[i],2) + pow(arrAy[i],2) + pow(arrAz[i],2) ); |
worasuchad | 0:22ebaf70b60e | 105 | } |
worasuchad | 0:22ebaf70b60e | 106 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 107 | /* calculate mean of vector size */ |
worasuchad | 0:22ebaf70b60e | 108 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 109 | for(i = 0; i < size ; i++) |
worasuchad | 0:22ebaf70b60e | 110 | { |
worasuchad | 0:22ebaf70b60e | 111 | sum += *(vSize + i); |
worasuchad | 0:22ebaf70b60e | 112 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 0:22ebaf70b60e | 113 | } |
worasuchad | 0:22ebaf70b60e | 114 | mean = sum/size; |
worasuchad | 0:22ebaf70b60e | 115 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 0:22ebaf70b60e | 116 | for(i = 0; i < size; i++) |
worasuchad | 0:22ebaf70b60e | 117 | { |
worasuchad | 0:22ebaf70b60e | 118 | standardDeviation += pow(*(vSize + i) - mean, 2); |
worasuchad | 0:22ebaf70b60e | 119 | //pc.printf("standardDeviation = %.2f \n\r",standardDeviation); |
worasuchad | 0:22ebaf70b60e | 120 | } |
worasuchad | 0:22ebaf70b60e | 121 | free(vSize); |
worasuchad | 0:22ebaf70b60e | 122 | return sqrt(standardDeviation / size); |
worasuchad | 0:22ebaf70b60e | 123 | } |
worasuchad | 0:22ebaf70b60e | 124 | |
worasuchad | 0:22ebaf70b60e | 125 | |
worasuchad | 0:22ebaf70b60e | 126 | /*------------------------< Function comment >------------------------*/ |
worasuchad | 0:22ebaf70b60e | 127 | /* NAME : calculate mean size of point(roll, pitch) */ |
worasuchad | 0:22ebaf70b60e | 128 | /* PARAMETERS : - */ |
worasuchad | 0:22ebaf70b60e | 129 | /* RETURN VALUE : mean value */ |
worasuchad | 0:22ebaf70b60e | 130 | /* DESCRIPTION : - */ |
worasuchad | 0:22ebaf70b60e | 131 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 132 | float calculator::calcG4(const float *arrRoll, const float *arrPitch, int size) |
worasuchad | 0:22ebaf70b60e | 133 | { |
worasuchad | 0:22ebaf70b60e | 134 | float sum = 0.0, mean = 0.0; |
worasuchad | 0:22ebaf70b60e | 135 | int i; |
worasuchad | 0:22ebaf70b60e | 136 | float *pSize; |
worasuchad | 0:22ebaf70b60e | 137 | |
worasuchad | 0:22ebaf70b60e | 138 | /* check arg is not empty */ |
worasuchad | 0:22ebaf70b60e | 139 | if(!arrRoll or !arrPitch) |
worasuchad | 0:22ebaf70b60e | 140 | { |
worasuchad | 0:22ebaf70b60e | 141 | return NULL; |
worasuchad | 0:22ebaf70b60e | 142 | } |
worasuchad | 0:22ebaf70b60e | 143 | |
worasuchad | 0:22ebaf70b60e | 144 | pSize = (float *)malloc(size*sizeof(float)); |
worasuchad | 0:22ebaf70b60e | 145 | /* check malloc is not return NULL */ |
worasuchad | 0:22ebaf70b60e | 146 | if( pSize == NULL ) |
worasuchad | 0:22ebaf70b60e | 147 | { |
worasuchad | 0:22ebaf70b60e | 148 | return NULL; |
worasuchad | 0:22ebaf70b60e | 149 | } |
worasuchad | 0:22ebaf70b60e | 150 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 151 | /* calculate size of point */ |
worasuchad | 0:22ebaf70b60e | 152 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 153 | for(i = 0; i < size; i++) |
worasuchad | 0:22ebaf70b60e | 154 | { |
worasuchad | 0:22ebaf70b60e | 155 | *(pSize + i) = sqrt( pow(arrRoll[i],2) + pow(arrPitch[i],2) ); |
worasuchad | 0:22ebaf70b60e | 156 | } |
worasuchad | 0:22ebaf70b60e | 157 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 158 | /* calculate mean of point size */ |
worasuchad | 0:22ebaf70b60e | 159 | /*--------------------------------------------------------------------*/ |
worasuchad | 0:22ebaf70b60e | 160 | for(i = 0; i < size ; i++) |
worasuchad | 0:22ebaf70b60e | 161 | { |
worasuchad | 0:22ebaf70b60e | 162 | sum += *(pSize + i); |
worasuchad | 0:22ebaf70b60e | 163 | //pc.printf("sum = %.2f \n\r",sum); |
worasuchad | 0:22ebaf70b60e | 164 | } |
worasuchad | 0:22ebaf70b60e | 165 | mean = sum/size; |
worasuchad | 0:22ebaf70b60e | 166 | //pc.printf("mean = %.2f \n\r",mean); |
worasuchad | 0:22ebaf70b60e | 167 | |
worasuchad | 0:22ebaf70b60e | 168 | free(pSize); |
worasuchad | 0:22ebaf70b60e | 169 | return mean; |
worasuchad | 0:22ebaf70b60e | 170 | } |
worasuchad | 0:22ebaf70b60e | 171 | /*--------------------------------------------------------------------*/ |