Compass Developer / Yamartino

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?

UserRevisionLine numberNew 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 }