0

I am having a hard time trying to understand the mathematical derivation of the equations I have included below. This piece of code is part of an example from a SparkFun IMU library that can be found here. Could someone please help me understand the theory behind the use of arctan functions to estimate roll, pitch, and yaw? And how is the magnetometer data used to get the right yaw estimate? All the resources I could find online didn't answer these questions in a way that I could understand.

 float roll = atan2(ay, az);
 float pitch = atan2(-ax, sqrt(ay * ay + az * az));

 float heading;
 if (my == 0)
   heading = (mx < 0) ? PI : 0;
 else
 heading = atan2(mx, my);

 heading -= EARTH_DECLINATION * PI / 180;

 if (heading > PI) heading -= (2 * PI);
 else if (heading < -PI) heading += (2 * PI);

 // Convert everything from radians to degrees:
 heading *= 180.0 / PI;
 pitch *= 180.0 / PI;
 roll  *= 180.0 / PI;
πάντα ῥεῖ
  • 1
  • 13
  • 116
  • 190
user11271728
  • 13
  • 4
  • 1
    @πάνταῥεῖ Be careful and compare e.g. https://www.urbandictionary.com/define.php?term=Attitude or https://www.collinsdictionary.com/dictionary/english/attitude both list OPs interpretation among the possible meanings... – Yunnosch Apr 26 '19 at 11:54
  • See e.g. https://calcworkshop.com/vector-applications/force-vector/ – JimmyB Apr 26 '19 at 11:56
  • Well I don't understand this either, since I don't see where they compensate for 1G earth gravitation. An accelerometer will give a_total = 1G + ax + ay + az, where ax, ay and az is the acceleration of the device. And you don't know how much of the 1G that goes into each axis read without a reference. – Lundin Apr 26 '19 at 13:29
  • @Lundin You do that by assuming that there is no significant `ax`/`ay`/`az` outside of the 1G. How that 1G is distributed into the channels (i.e. among `ax`/`ay`/`az`) is exactly how you obtain roll/pitch/yaw. This means that an accellerometer in free fall will not be able to tell you which way "down" is. That's somewhat obvious because the same would be true in a gravity-free space (which is equivalent to free fall from a mechanical perspective). – Max Langhof Apr 26 '19 at 14:06
  • @user11271728 You can derive these formulas yourself by decomposing a gravity vector into the `ax`/`ay`/`az` components based on roll/yaw/pitch. You then have to solve for the latter based on the former, which is all these formulas do. Requires only minor amounts of trigonometry (and a pythagoras). – Max Langhof Apr 26 '19 at 14:13
  • @MaxLanghof I assume the device is something that moves, like an airplane. If the sum of all axis is 1G, the device isn't moving. Thats how MEMS accelerometers work anyway. – Lundin Apr 26 '19 at 14:16
  • @Lundin _" If the sum of all axis is 1G, the device isn't moving."_ No, it's not _accelerating_ in that case. – Max Langhof Apr 26 '19 at 14:18

2 Answers2

1

Assuming ay and az are the offset from origin given by the magnetometer then atan(ay, az) will give you the angle that produced that offset.

The sqrt(ay * ay + az * az) follows by the pythagorean theorem to give you the length of the third side of the "offset triangle" to be able to calculate pitch. The -ax comes from how pitch is defined.

Carl Onsjö
  • 151
  • 1
  • 5
  • I assume `ax`, `ay` and `az` are the accelerations, `mx` and `my` are the magnetic field strengths. – JimmyB Apr 26 '19 at 11:55
  • Thanks for your reply! Aren't az ad ay the values for linear acceleration with respect to the z and y axes of the accelerometer? mx and my are the magnetic field strengths like @JimmyB said – user11271728 Apr 26 '19 at 12:00
  • The angle between two vectors will be the same regardless if they describe the displacement, velocity or acceleration of an object, so the same reasoning will hold true in all cases. When it comes to the third leg of the triangle which the pythagorean theorem is used for it is applicable since you are only interested in the relationship between the acceleration vectors (the angle). – Carl Onsjö Apr 29 '19 at 06:28
0

atan2(y,x) is the angle theta as in the following diagram:

enter image description here

In three dimensions you have three planes and the atan2() is applied to a pair from x, y, and z depending on which plane you are calculating theta for (roll, pitch, yaw).

When at a steady velocity, (i.e. not changing speed or direction) the values ax, ay, az each measure the gravitational component of acceleration only. The value will be inaccurate under acceleration. You have to be a bit cleverer at that point by combining information from other sensors such as the gyro or magnetometer and the other accelerators - that is when stationary ax, ay, az sum to 1G - anything else and there is an additional acceleration in effect. An accelerator measures acceleration, but that includes acceleration due to gravity. That is when at a steady velocity, an accelerator is a tilt sensor relative to gravity.

The magnetometer calculation is determining an angle relative to north from its x and y components and compensating for the difference between true north and magnetic north (the magnetic declination). The x an y components of the magnetometer are at a maximum when they are aligned with a magnetic field, because they are mounted orthogonality, their relative values resolve to a single direction using atan2(mx,my). x and y are swapped from the conventional order because compass directions increase clockwise, while mathematically angles increase counter-clockwise.

In practice you need to use sensor function to combine information from the gyro (angular velocity), accelerator, and magnetometer and probably apply some heuristics too to track motion accurately. Each of these sensors have different confounding factors and some element of measurement overlap in the sense that a single external force can have an effect on more than one sensor. This can be used to discriminate different kinds of motion and attitude. It is complicated, which is probably why this does does not attempt to deal with it.

Clifford
  • 88,407
  • 13
  • 85
  • 165
  • Thank you for the detailed answer! All that makes sense, the only thing I still haven't understood is, why is pitch calculated using `float pitch = atan2(-ax, sqrt(ay * ay + az * az)) ` ? – user11271728 Apr 28 '19 at 15:01
  • Yes, I omitted that. May rectify that, but in the meantime it is probably already covered by https://stackoverflow.com/questions/23009549/roll-pitch-yaw-calculation – Clifford Apr 28 '19 at 16:13
  • Thank you so much @Clifford, that clarified it for me! One last thing: I am confused about what the lines `if (heading > PI) heading -= (2 * PI);`and `else if (heading < -PI) heading += (2 * PI);` are there for – user11271728 Apr 30 '19 at 11:49
  • @user11271728 Probably deserves another question rather than a comment. However it normalises the heading to +/- PI. – Clifford Apr 30 '19 at 12:31