Calculating pitch, yaw, and roll from mag, acc, and gyro data

Question

I have an Arduino board with a 9 degree of freedom sensor, from which I must determine the pitch, yaw, and roll of the board.

Here is an example of one set of data from the 9-DOF sensor:

Accelerometer (m/s)

• $\text{Acc}_{X}$ = -5,85
• $\text{Acc}_{Y}$ = 1,46
• $\text{Acc}_{Z}$ = 17,98

Gyroscope (RPM)

• $\text{Gyr}_{X}$ = 35,14
• $\text{Gyr}_{Y}$ = -40,22
• $\text{Gyr}_{Z}$ = -9,86

Magnetometer (Gauss)

• $\text{Mag}_{X}$ = 0,18
• $\text{Mag}_{Y}$ = -0,04
• $\text{Mag}_{Z}$ = -0,15

How can I calculate pitch, yaw, and roll from these data?

Answer 1

Pitch, roll and yaw are defined as the rotation around X, Y and Z axis. Below as a picture to illustrate the definition.

In a previous project I used a ADXL345 Accelerometer from Analog Devices to calculate roll and pitch. Below are the equations used to calculated roll and pitch. I have made some of source code available for public use.

accelerationX = (signed int)(((signed int)rawData_X) * 3.9);
accelerationY = (signed int)(((signed int)rawData_Y) * 3.9);
accelerationZ = (signed int)(((signed int)rawData_Z) * 3.9);
pitch = 180 * atan (accelerationX/sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))/M_PI;
roll = 180 * atan (accelerationY/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Complete source code can be found here.

Base on the above definitions

yaw = 180 * atan (accelerationZ/sqrt(accelerationX*accelerationX + accelerationZ*accelerationZ))/M_PI;

Note: M_PI = 3.14159265358979323846 it is constant defined in math.h

Below are some references including Arduino base source code that might help you.

---

References:

• Flight dynamics
• Beaglebone black ADXL345 source code
• Adxl345 Beaglebone black Embedded QT Demo
• Arduino IMU: Pitch & Roll from an Accelerometer

Answer 2

So my longer answer below assumes that the board will undergo acceleration and during this time you still need to be able to measure your pitch, roll and yaw within a short amount of time. If the board will be stationary for all measurements then Mahendra Gunawardena's answer will work perfectly for you. If this is going into a device like a segway or model plane or multirotor or anything that moves around, you may want to keep reading. This post is about how to use all three sensors though a method called sensor fusion. Sensor fusion allows you to get the strengths of each sensor and minimize the effects of each sensors weaknesses.

Sensor characteristics and background

First understand that an accelerometer measure all forces being applied to it, not just the force of gravity. So in a perfect world with the accelerometer in a stationary position without any vibrations you could perfectly determine which way is up using some basic trigonometry as shown by Mahendra Gunawardena's answer. However since an accelerometer will pick up all forces, any vibrations will result in noise. It should also be noted that if the board is accelerating you can not just use simple trigonometry as the force the accelerometer is reporting is not only the earths force of gravity but also the force that is causing you to accelerate.

A magnetometer is more straightforward then an accelerometer. Movement will not cause problems with it but things like iron and other magnets will end up effecting your output. If the sources causing this interference are constant its not to hard to deal with but if these sources are not constant it will create tons of noise that is problematic to remove.

Of the three sensors, the gyroscope is arguable the most reliable and they are normally very very good at measuring rotational speed. It is not affected by things like iron sources and accelerations have basically no impact on their ability to measure rotational speed. They do a very good job of reporting the speed at which the device is turning at, however since you are looking for an absolute angle you have to integrate the speed to get position. Doing this will add the error of the last measurement to the error of the new measurements since integration is basically a sum of values over a range, even if the error for one measurement is only 0.01 degrees per second off, in 100 measurements, your position can be off by 1 degree, by 1000 measurements, you can by off by 10 degrees. If you are taking hundreds of measurements a second, you can see this causes problems. This is commonly called gyro drift.

Sensor fusion

Now the beauty of having all of these sensors work together is that you can use the information from the accelerometer and magnetometer to cancel out gyro drift. This ends up allowing you to giving you the accuracy and speed of the gyro without the fatal flaw of gyro drift.

Combining the data from these three sensors can be done in more then one way, I'll talk about using a complementary filter because its far simpler then a kalman filter and kalman filters will eat up much more resources on embedded systems. Often times a complementary filter is good enough, simpler to implement(assuming your not using a pre-built library) and lets you process the data faster.

Now onto the process. The first steps you need to do is to integrate the gyroscope output to convert the angular speed into angular position. You will also most likely have to apply a low pass filter on the accelerometer and magnetometer to deal with noise in the output. A simple FIR filter like the one shown below works here. With some trigonometry you can find the pitch and roll with the accelerometer and the yaw with the magnetometer.

filteredData = (1-weight)*filteredData + weight*newData

The weight is just a constant that can be adjusted depending on how much noise you have to deal with, the higher the noise is the smaller the weight value will be. Now combining the data from the sensors can be done by the following line of code.

fusedData = (1-weight)*gyroData + weight*accelMagData

It should be noted that the data is a vector of the pitch, roll and yaw. You can just use three variables to do this as well instead of arrays if you want. For this calculation the gyro provides a position in degrees in pitch, roll and yaw, the magnetometer provides an angle for yaw while the accelerometer provides its own numbers for pitch and roll.

If you still want more information you can google "sensor fusion with complementary filter" there are plenty of articles about this.

Answer 3

From the accelerator sensor data, you can only calculate pitch and roll. The bellow document from Freescale explains with plenty of information what you need:

AN3461 - Tilt Sensing Using a Three-Axis Accelerometer

Based on the sayings of the document,

$$\tan \phi_{xyz} = \frac{G_{py}}{G_{pz}}$$

$$\tan \theta_{xyz} = \frac{-G_{px}}{G_{py}\sin \phi + G_{pz}\cos \phi} = \frac{-G_{px}}{\sqrt{G_{py}^2 + G_{pz}^2}}$$

which equates to:

roll = atan2(accelerationY, accelerationZ)

pitch = atan2(-accelerationX, sqrt(accelerationY*accelerationY + accelerationZ*accelerationZ))

Of course, the result is this only when the rotations are occurring on a specific order (Rxyz):

1. Roll around the x-axis by angle $\phi$
2. Pitch around the y-axis by angle $\theta$
3. Yaw around z-axis by angle $\psi$

Depending on the rotations order, you get different equations. For the $R_{xyz}$ rotation order, you can not find the angle $\psi$ for the Yaw around z-axis.