Yamartino.cpp

00001 #include "Yamartino.h"
00002 #define PI                    3.14159265
00003 // Converts degrees to radians.
00004 #define degreesToRadians(angleDegrees) (angleDegrees * PI / 180.0)
00005  
00006 // Converts radians to degrees.
00007 #define radiansToDegrees(angleRadians) (angleRadians * 180.0 / PI)
00008 
00009 Yamartino::Yamartino() {
00010     historyLength = 10;
00011     //history = new float[historyLength]; // keep our history values
00012     historyX = new float[historyLength];
00013     historyY = new float[historyLength];
00014 }
00015 
00016 float Yamartino::myAtan2(float y, float x) {
00017   float t = atan2(y, x);
00018   return t > 0 ? t : 2 * PI + t;
00019 }
00020 
00021 float Yamartino::calculateYamartinoAverage() {
00022 
00023   float sumX = 0;
00024   float sumY = 0;
00025   
00026   for (int i = 0; i < historyLength; i++) {
00027     sumX += historyX[i];
00028     sumY += historyY[i];
00029   }
00030 
00031   //float meanX = sumX / history.length;
00032   //float meanY = sumY / history.length;
00033   // YAMARTINO METHOD FOR STANDARD DEVIATION!!
00034   // http://en.wikipedia.org/wiki/Yamartino_method
00035   //float eps = sqrt(1 - (meanX*meanX + meanY*meanY));
00036   //eps = Float.isNaN(eps) ? 0 : eps; // correct for NANs
00037   //historyCorrectStd = asin(eps)* (1 + (2 / sqrt(3) - 1) * (eps * eps * eps));
00038   
00039   return radiansToDegrees(myAtan2(sumY, sumX));
00040 }
00041 
00042 void Yamartino::addItemsToHistoryBuffers(float input) {
00043   float valueRadians = degreesToRadians(input);
00044   //addToHistory(history,input);
00045   addToHistory(historyX,cos(valueRadians));
00046   addToHistory(historyY,sin(valueRadians));
00047 }
00048 
00049 void Yamartino::addToHistory(float* buffer, float input) {
00050   // delete the oldest value from the history
00051   // add one value to the history (the input)
00052   // take the average of the history and return it;
00053 
00054   // shift the values to the left in the array
00055   for (int i = historyLength - 1; i >= 0; i--) {
00056     if (i == 0) {
00057       buffer[0] = input;
00058     }
00059     else {
00060       buffer[i] = buffer[i-1];
00061     }
00062   }
00063 }