Diff: Yamartino.cpp

Revision:

0:726c69fad7d8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Yamartino.cpp	Tue Aug 12 11:52:35 2014 +0000
@@ -0,0 +1,63 @@
+#include "Yamartino.h"
+#define PI                    3.14159265
+// Converts degrees to radians.
+#define degreesToRadians(angleDegrees) (angleDegrees * PI / 180.0)
+ 
+// Converts radians to degrees.
+#define radiansToDegrees(angleRadians) (angleRadians * 180.0 / PI)
+
+Yamartino::Yamartino() {
+    historyLength = 10;
+    //history = new float[historyLength]; // keep our history values
+    historyX = new float[historyLength];
+    historyY = new float[historyLength];
+}
+
+float Yamartino::myAtan2(float y, float x) {
+  float t = atan2(y, x);
+  return t > 0 ? t : 2 * PI + t;
+}
+
+float Yamartino::calculateYamartinoAverage() {
+
+  float sumX = 0;
+  float sumY = 0;
+  
+  for (int i = 0; i < historyLength; i++) {
+    sumX += historyX[i];
+    sumY += historyY[i];
+  }
+
+  //float meanX = sumX / history.length;
+  //float meanY = sumY / history.length;
+  // YAMARTINO METHOD FOR STANDARD DEVIATION!!
+  // http://en.wikipedia.org/wiki/Yamartino_method
+  //float eps = sqrt(1 - (meanX*meanX + meanY*meanY));
+  //eps = Float.isNaN(eps) ? 0 : eps; // correct for NANs
+  //historyCorrectStd = asin(eps)* (1 + (2 / sqrt(3) - 1) * (eps * eps * eps));
+  
+  return radiansToDegrees(myAtan2(sumY, sumX));
+}
+
+void Yamartino::addItemsToHistoryBuffers(float input) {
+  float valueRadians = degreesToRadians(input);
+  //addToHistory(history,input);
+  addToHistory(historyX,cos(valueRadians));
+  addToHistory(historyY,sin(valueRadians));
+}
+
+void Yamartino::addToHistory(float* buffer, float input) {
+  // delete the oldest value from the history
+  // add one value to the history (the input)
+  // take the average of the history and return it;
+
+  // shift the values to the left in the array
+  for (int i = historyLength - 1; i >= 0; i--) {
+    if (i == 0) {
+      buffer[0] = input;
+    }
+    else {
+      buffer[i] = buffer[i-1];
+    }
+  }
+}
\ No newline at end of file