this algorith is used for averaging compass values
Revision 0:726c69fad7d8, committed 2014-08-12
- 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