I saw several questions about this in lots of forums, but none of them resulted me clear enough or precise enough for my needs. Or I just didn't understand them, that's another possibility.
Well, consider several accelerometer measures (3 dimensions) (m/s^2) taken by a device (that's not an android device, and no real time processing is needed here). I'm reading these values from a text file and need to convert them to degrees.
The first code I tried was :
private static final double G = 9.81;
private static final double RAD_TO_DEG = 180.0 / Math.PI;
private double[] calculateAngles(double acX, double acY, double acZ) {
double accelX = acX * G;
double accelY = acY * G;
double accelZ = acZ * G;
double angleX = Math.atan2(accelX, Math.sqrt(accelY * accelY + accelZ * accelZ)) * RAD_TO_DEG;
double angleY = Math.atan2(accelY, Math.sqrt(accelX * accelX + accelZ * accelZ)) * RAD_TO_DEG;
double angleZ = Math.atan2(Math.sqrt(accelX * accelX + accelY * accelY), accelZ) * RAD_TO_DEG;
return new double[] { angleX, angleY, angleZ };
}
This code "seems" to give me a good result. At least, credible. I put my accelerometer in a fixed position, noticed the 3 angles (UPDATE: I'm talking about yaw, pitch and roll) given by a random measure, turned the accelerometer by 90º (approx) and one of the angles added this more or less 90º to its value.
Nevertheless, the inverse operation I tried doesn't work at all :
private static final double RAD_TO_DEG = 180.0 / Math.PI;
private static double[] calculateAccelerations(double angleX, double angleY, double angleZ) {
// Convertir les angles en radians
double radianAngleX = angleX * DEG_TO_RAD;
double radianAngleY = angleY * DEG_TO_RAD;
double radianAngleZ = angleZ * DEG_TO_RAD;
double accelX = Math.tan(radianAngleX) / Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));
double accelY = Math.tan(radianAngleY) / Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));
double accelZ = Math.tan(radianAngleZ) * Math.sqrt(1.0 + Math.tan(radianAngleX) * Math.tan(radianAngleX) + Math.tan(radianAngleY) * Math.tan(radianAngleY));
return new double[]{accelX/G, accelY/G, accelZ/G};
}
If I take my initial accelerometer values, estimate yaw, pitch and roll, convert them to accelerations... how to say... Inventing the values would be more accurate. The first question is... why? I can't see my mistake here.
Googling for alternatives, Kalman filter has been the most relevant result I could find to do this. I found an implementation in common-math3 (Apache), but I can't understand how this is working nor if it really applies here.
What I need would be read those accelerometer values and estimate what would have been those values with the accelerometer positioned in a different way. To do that, I need to be able to "estimate" what are the accelerometer position, add the angles I need to add to virtually change its orientation, and go back to accelerometer values.
Ideally a Java suggestion, but any help, including agnostic explanation, is welcome!
First off, what do you want your angles to measure? From the calculation that you have in calculateAngles, what you seem to be calculating is this:
Given your followup comment, I'm going to assume that these are actually the angles you wish to measure and continue from there.
Now, because of the function you're using in that method (atan2), multiplying all three acceleration measures by the same constant has no effect; therefore, the * G; step can be omitted in calculateAngles.
So first, I'd change that function to just:
private double[] calculateAngles(double acX, double acY, double acZ) {
double angleX = Math.atan2(acX, Math.sqrt(acY * acY + acZ * acZ)) * RAD_TO_DEG;
double angleY = Math.atan2(acY, Math.sqrt(acX * acX + acZ * acZ)) * RAD_TO_DEG;
double angleZ = Math.atan2(Math.sqrt(acX * acX + acY * acY), acZ) * RAD_TO_DEG;
return new double[] { angleX, angleY, angleZ };
}
Now, to reverse the calculation, we're going to need to use the fact that when we're done we should have that the sum of the three accelerations squared should equal G squared. Also, we'll want to use the fact that to reverse
angle = Math.atan2(y, x);
You generally shouldn't use the tan function, but should instead use:
x = Math.cos(angle);
y = Math.sin(angle);
// then multiple x and y by some scaling factor
So:
private static double[] calculateAccelerations(double angleX, double angleY, double angleZ) {
// Convertir les angles en radians
double radianAngleX = angleX * DEG_TO_RAD;
double radianAngleY = angleY * DEG_TO_RAD;
double radianAngleZ = angleZ * DEG_TO_RAD;
double acX = Math.sin(radianAngleX);
double acY = Math.sin(radianAngleY);
double acZ = Math.cos(radianAngleZ);
// use this if you need the sum of the acceleration squared to be G squared
//double scaling = G / Math.sqrt(acX*acX + acY*acY + acZ*acZ);
// EDIT Cheloute: with scaling = 1, that works perfectly for my use case
double scaling = 1;
return new double[]{acX*scaling, acY*scaling, acZ*scaling};
}
Note though that although the calculateAccelerations function will spit back values for any three input angles, that doesn't mean that any three angle combinations are actually valid; for example, the way you've got your measurements set up, angleY is restricted to being between -abs(abs(angleX)-90) and abs(abs(angleX)-90), and given angleX and angleY, I'm pretty sure it's possible to determine angleZ or at least narrow it down to only two possibilities.
However, if you feed calculateAccelerations angles that came out of calculateAngles you should get back the original accelerations, or close enough.