Search code examples
c++embeddedeuler-anglesimu

Attitude estimation from accelerometer and gyroscope of an IMU?

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

  • 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.