this algorith is used for averaging compass values

Files at this revision

API Documentation at this revision

Comitter:
fin4478
Date:
Tue Aug 12 11:52:35 2014 +0000
Commit message:
Initial

Changed in this revision

Yamartino.cpp Show annotated file Show diff for this revision Revisions of this file
Yamartino.h Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 726c69fad7d8 Yamartino.cpp
--- /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
diff -r 000000000000 -r 726c69fad7d8 Yamartino.h
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Yamartino.h	Tue Aug 12 11:52:35 2014 +0000
@@ -0,0 +1,24 @@
+#ifndef YAMARTINO_H
+#define YAMARTINO_H
+#include "mbed.h"
+
+//Yamartino algorithm is used for averaging compass values
+class Yamartino
+{
+public:
+
+int historyLength;
+//float* history; // keep our history values
+float* historyX;
+float* historyY;
+
+Yamartino();
+float calculateYamartinoAverage();
+float myAtan2(float y, float x);
+void addItemsToHistoryBuffers(float input);
+void addToHistory(float* buffer, float input);
+
+};
+
+
+#endif
\ No newline at end of file