this algorith is used for averaging compass values
Yamartino.cpp@0:726c69fad7d8, 2014-08-12 (annotated)
- Committer:
- fin4478
- Date:
- Tue Aug 12 11:52:35 2014 +0000
- Revision:
- 0:726c69fad7d8
Initial
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
fin4478 | 0:726c69fad7d8 | 1 | #include "Yamartino.h" |
fin4478 | 0:726c69fad7d8 | 2 | #define PI 3.14159265 |
fin4478 | 0:726c69fad7d8 | 3 | // Converts degrees to radians. |
fin4478 | 0:726c69fad7d8 | 4 | #define degreesToRadians(angleDegrees) (angleDegrees * PI / 180.0) |
fin4478 | 0:726c69fad7d8 | 5 | |
fin4478 | 0:726c69fad7d8 | 6 | // Converts radians to degrees. |
fin4478 | 0:726c69fad7d8 | 7 | #define radiansToDegrees(angleRadians) (angleRadians * 180.0 / PI) |
fin4478 | 0:726c69fad7d8 | 8 | |
fin4478 | 0:726c69fad7d8 | 9 | Yamartino::Yamartino() { |
fin4478 | 0:726c69fad7d8 | 10 | historyLength = 10; |
fin4478 | 0:726c69fad7d8 | 11 | //history = new float[historyLength]; // keep our history values |
fin4478 | 0:726c69fad7d8 | 12 | historyX = new float[historyLength]; |
fin4478 | 0:726c69fad7d8 | 13 | historyY = new float[historyLength]; |
fin4478 | 0:726c69fad7d8 | 14 | } |
fin4478 | 0:726c69fad7d8 | 15 | |
fin4478 | 0:726c69fad7d8 | 16 | float Yamartino::myAtan2(float y, float x) { |
fin4478 | 0:726c69fad7d8 | 17 | float t = atan2(y, x); |
fin4478 | 0:726c69fad7d8 | 18 | return t > 0 ? t : 2 * PI + t; |
fin4478 | 0:726c69fad7d8 | 19 | } |
fin4478 | 0:726c69fad7d8 | 20 | |
fin4478 | 0:726c69fad7d8 | 21 | float Yamartino::calculateYamartinoAverage() { |
fin4478 | 0:726c69fad7d8 | 22 | |
fin4478 | 0:726c69fad7d8 | 23 | float sumX = 0; |
fin4478 | 0:726c69fad7d8 | 24 | float sumY = 0; |
fin4478 | 0:726c69fad7d8 | 25 | |
fin4478 | 0:726c69fad7d8 | 26 | for (int i = 0; i < historyLength; i++) { |
fin4478 | 0:726c69fad7d8 | 27 | sumX += historyX[i]; |
fin4478 | 0:726c69fad7d8 | 28 | sumY += historyY[i]; |
fin4478 | 0:726c69fad7d8 | 29 | } |
fin4478 | 0:726c69fad7d8 | 30 | |
fin4478 | 0:726c69fad7d8 | 31 | //float meanX = sumX / history.length; |
fin4478 | 0:726c69fad7d8 | 32 | //float meanY = sumY / history.length; |
fin4478 | 0:726c69fad7d8 | 33 | // YAMARTINO METHOD FOR STANDARD DEVIATION!! |
fin4478 | 0:726c69fad7d8 | 34 | // http://en.wikipedia.org/wiki/Yamartino_method |
fin4478 | 0:726c69fad7d8 | 35 | //float eps = sqrt(1 - (meanX*meanX + meanY*meanY)); |
fin4478 | 0:726c69fad7d8 | 36 | //eps = Float.isNaN(eps) ? 0 : eps; // correct for NANs |
fin4478 | 0:726c69fad7d8 | 37 | //historyCorrectStd = asin(eps)* (1 + (2 / sqrt(3) - 1) * (eps * eps * eps)); |
fin4478 | 0:726c69fad7d8 | 38 | |
fin4478 | 0:726c69fad7d8 | 39 | return radiansToDegrees(myAtan2(sumY, sumX)); |
fin4478 | 0:726c69fad7d8 | 40 | } |
fin4478 | 0:726c69fad7d8 | 41 | |
fin4478 | 0:726c69fad7d8 | 42 | void Yamartino::addItemsToHistoryBuffers(float input) { |
fin4478 | 0:726c69fad7d8 | 43 | float valueRadians = degreesToRadians(input); |
fin4478 | 0:726c69fad7d8 | 44 | //addToHistory(history,input); |
fin4478 | 0:726c69fad7d8 | 45 | addToHistory(historyX,cos(valueRadians)); |
fin4478 | 0:726c69fad7d8 | 46 | addToHistory(historyY,sin(valueRadians)); |
fin4478 | 0:726c69fad7d8 | 47 | } |
fin4478 | 0:726c69fad7d8 | 48 | |
fin4478 | 0:726c69fad7d8 | 49 | void Yamartino::addToHistory(float* buffer, float input) { |
fin4478 | 0:726c69fad7d8 | 50 | // delete the oldest value from the history |
fin4478 | 0:726c69fad7d8 | 51 | // add one value to the history (the input) |
fin4478 | 0:726c69fad7d8 | 52 | // take the average of the history and return it; |
fin4478 | 0:726c69fad7d8 | 53 | |
fin4478 | 0:726c69fad7d8 | 54 | // shift the values to the left in the array |
fin4478 | 0:726c69fad7d8 | 55 | for (int i = historyLength - 1; i >= 0; i--) { |
fin4478 | 0:726c69fad7d8 | 56 | if (i == 0) { |
fin4478 | 0:726c69fad7d8 | 57 | buffer[0] = input; |
fin4478 | 0:726c69fad7d8 | 58 | } |
fin4478 | 0:726c69fad7d8 | 59 | else { |
fin4478 | 0:726c69fad7d8 | 60 | buffer[i] = buffer[i-1]; |
fin4478 | 0:726c69fad7d8 | 61 | } |
fin4478 | 0:726c69fad7d8 | 62 | } |
fin4478 | 0:726c69fad7d8 | 63 | } |