3d Accelerometer calculate the orientation

Question

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;

score 52 · Accepted Answer · edited Mar 07 '22 at 09:03

52

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.

edited Mar 07 '22 at 09:03

Humam Helfawi

19,566
15
85
160

answered Apr 25 '12 at 17:12

matteo

2,934
7
44
59

3

I know this question is very old, but I hate seing wrong ansswers. I myself was looking for this, and I found hundreds of hits in google, most of them with the same wrong answer – matteo Apr 25 '12 at 17:14
2

By the way I forgot to mention the source, which contains a very exhaustive explanation: http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf – matteo Apr 25 '12 at 17:14
1

Assuming you are using equations 25 & 26 from the linked source (a great resource btw) shouldn't the Pitch be `atan2(-X, sqrt(Y*Y + Z+Z))`? – uesp Apr 25 '12 at 18:22
Yes sorry, that was a typo, actually it's atan2(X, sqrt(Y*Y+Z*Z)) without the minus sign on X if we assume that the accelerometer gives positive values when the axis is aligned with gravity and points UPWARDS, which is at least what my phone does. In the article they assume the opposite convention. I haven't digged into why I don't need another minus sign for Roll but i've tried it and it works – matteo Apr 25 '12 at 22:28
What are X, Y and Z? Do you mean Rx, Ry, and Rz? Or are you referring to something else completely? – Anubian Noob Aug 03 '14 at 15:45
X, Y and Z are the projections on the x, y and z axes of the acceleration measured by the accelerometer. These are usually the "raw" values that an accelerometer physically measures and that low-level interfaces allow you to read. I don't know what Rx, Ry and Rz are but they sound like "rotation around the x axis", etc., which is what we're trying to calculate here. If that's the case and you have access to such values, you're probably using a more high-level API or library, and perhaps don't need the answers to this question at all. – matteo Aug 03 '14 at 21:14
The link to the document which was referred by @matteo has been changed to a new location https://www.nxp.com/docs/en/application-note/AN3461.pdf . I can't edit the answer as the edit queue is full. – Sergey V. Apr 12 '21 at 09:47

score 20 · Answer 2 · answered May 12 '15 at 15:39

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.

Is it `accY` the same than `Y`? Because you mention in this answer both `Y` and `accY` and I don't know if you refer to different values or not. — Dekike, Feb 11 '20 at 15:25
I also made this question in stackoverflow. I don't know if you might help me to solve my doubts... https://stackoverflow.com/questions/60174039/correct-way-of-calculating-the-roll-of-an-animal-using-3d-acceleration — Dekike, Feb 12 '20 at 12:50

FaithNoMan · Answer 3 · 2017-04-14T05:16:59.737

5

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

edited Apr 14 '17 at 05:16

answered Apr 14 '17 at 04:51

FaithNoMan

71
1
5

I get the same problem, as soon as the device is near the halfway mark to "upside down", roll just flips very fast, i tried coutneracting by adding sgn(Z) to the other calculation or just flipping when near, but its not as simple. If i swap the axes of the coordinate system and then swap my pitch/roll outputs, i can get the same thing, but with roll causing the problem, not pitch. – rajkosto May 11 '17 at 13:59
I'm having the same problem. Things look relative good when facing about forward, but when looking close to straight down or looking straight up the whole camera turns rapidly. Did you ever find a solution? – Andres Hernandez Jan 19 '20 at 04:26

score 1 · Answer 4 · edited Mar 22 '18 at 06:49

1

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

edited Mar 22 '18 at 06:49

freestyle

3,692
11
21

answered Mar 22 '18 at 06:12

Ian Bloomfield

26
2

Is it possible to get the final forumla that would calculate yaw from the magnetometer. Presumably similar to the pitch equation as you want 180/-180 to get the full rotation. I assume atan2(z,sqrt(xx+yy) then the radians conversion using magnet north as the reference. Or do I need to modify the forumla if I wish to use all 3 axis? – Ian Bloomfield Oct 26 '18 at 22:14

score 0 · Answer 5 · answered Sep 20 '10 at 20:37

0

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

answered Sep 20 '10 at 20:37

uesp

6,194
20
15

Thanks for answering, but I tried it and I get 0°-180° for pitch and roll. I would like to get 0°-360° – Roland Soós Sep 20 '10 at 21:01
the equation for roll is wrong. See my answare (I don't take any credit, I found it out there) – matteo Apr 25 '12 at 17:16
1

I must confess to indeed basing my code and this answer on the variety of other sources out there but (unfortunately) never came across the source you posted. My equations indeed don't match anything in your source although they are close to equations 38 & 39 except for the reversed atan2() parameters. I would of also guessed that the pitch would be wrong and not the roll as from previous testing the roll appears to work fine from -90 to +90 degrees and the pitch has never been tested. I'll have to look at in more detail and figure it out... – uesp Apr 25 '12 at 18:25
The wrong one is the roll, but the roll in your equation does work correctly from -90 to 90 IF the pitch stays at 0. It becomes wrong when the pitch is non-zero, and noticeable when the pitch goes above about 45 degrees. If "the pitch has never been tested" means that in your tests you only varied the roll keeping the pitch close to zero, that would explain that you didn't detect the error in the roll formula – matteo Apr 25 '12 at 22:37

3d Accelerometer calculate the orientation

5 Answers5

Linked