3d Accelerometer calculate the orientation

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.


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.


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))

Tags:

C

Math

Opengl