worasuchad haomachai / calculator

hormone v.1 for optimizing COT

Dependents:   TurtleBot_with_AHS

Fork of calculator by worasuchad haomachai

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?

UserRevisionLine numberNew 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 /*--------------------------------------------------------------------*/