3d Accelerometer calculate the orientation
I have accelerometer values for开发者_如何转开发 the 3 axes (usually when there is only gravity contains data between -1.0 and 1.0 ):
float Rx;
float Ry;
float Rz;
I make some calculations, then I get the angles for each axis.
float R = sqrt(pow(Rx,2)+pow(Ry,2)+pow(Rz,2));
float Arx = acos(Rx/R)*180/M_PI;
float Ary = acos(Ry/R)*180/M_PI;
float Arz = acos(Rz/R)*180/M_PI;
Then I set the values for the box angles in opengl
rquad = Arx;
yquad = Ary;
Which rotates my box:
glRotatef(yquad,1.0f,0.0f,0.0f);
glRotatef(rquad,0.0f,1.0f,0.0f);
It works on a hemisphere. I would like to use the full sphere and I know that I have to use the Arz value to make it work, but I don't know how can I use that for this rotation. Could you help me?
Update: The final answer is in my case:
rquad = -atan2(Rx/R, Rz/R)*180/M_PI;
yquad = -atan2(Ry/R, Rz/R)*180/M_PI;
The correct answer is:
Roll = atan2(Y, Z) * 180/M_PI;
Pitch = atan2(-X, sqrt(Y*Y + Z*Z)) * 180/M_PI;
Source: https://www.nxp.com/docs/en/application-note/AN3461.pdf (page 10, Eqn. 25 & 26)
uesp's answer is wrong. It looks like an acceptable approximation until pitch and roll both go above 45 degrees.
I may be assuming a different orientation convention, but even if you swap axes and invert values in any consistent way, uesp's computations will never be equivalent.
While matteo's answer is correct, it does not provide the full, complete solution: The formulas are correct:
Roll = atan2(Y, Z) * 180/M_PI;
Pitch = atan2(-X, sqrt(Y*Y + Z*Z)) * 180/M_PI;
However, when pitch is +90/-90 degrees and the X axis is vertical pointing up/down, the ideal accelerometer normalized output should be:
accX = -1 / accX = 1
accY = 0
accZ = 0
Which means a roll angle of 0 degrees
; correct.
But in practice, the accelerometer output is noisy and you would get something closer to:
accX = -1 / accX = 1
accY = 0.003
accZ = 0.004
This might seem small but it will cause the roll angle to be ~30 dregrees which is not correct.
The obvious instinct would be to filter out the last digits, but this would affect precision, which is not always acceptable.
The compromise, which is very well explained in the reference app note, is to include a very small percentage of the accelerometer X axis reading in the formula for roll:
Roll = atan2( Y, sign* sqrt(Z*Z+ miu*X*X));
sign = 1 if accZ>0, -1 otherwise
miu = 0.001
The error introduced this way is dramatically smaller than the previous case: 2-3 degrees when measuring roll under the same conditions explained above.
I've tried the recommended solution (matteo's), and while it seemed to work great at first, I noticed that when the pitch approaches 90 degrees (starting at around 70 degrees but not necessarily consistent across different phones), the roll suddenly surges. When the pitch is at 90 the roll that should be around 0 is now at over 100 and keeps increasing to 180. I'm trying to think of a way to mathematically prevent this, if I restrict the roll to +90/-90 it behaves normally but I don't get the range I want (+180/-180): Math.atan2(y, Math.sqrt((xx) + (zz))) * (180/Math.PI))
For roll
, I have found you can use arctan(y/sqrt(X*X)+(z*z))
this will give roll -90/90
which is aviation standard without giving the pitch issue
I use the following calculations to convert our accelerometer readings into roll and pitch values:
Roll = atan2( sqrt(Y*Y + X*X), Z) * 180/M_PI;
Pitch = atan2( sqrt(X*X + Z*Z), Y) * 180/M_PI;
You may need to swap the X/Y/Z values or translate the Roll/Pitch depending on how your accelerometers are defined. To use them in the display them it is a simple matter of:
glRotatef (Pitch, 0.0f, 0.0f, 1.0f);
glRotatef (Roll, 1.0f, 0.0f, 0.0f);
精彩评论