Determining Roll and Pitch of a Body from the Gravity Vector

Problem: Determine Pitch and Roll from the gravity vector measured via 3-orthogonal accelerometers measured by an Inertial Measurement Unit.

First constructing the coordinate transformation matrix for Euler transformations about the y-axis (pitch) and then about the x-axis (roll). The conventional first rotation about the inertial z-axis (yaw) is ignored since changes in yaw does not effect the accelerations measured by the body axis accelerations.

Refer to Figure 1 below where theta, single prime and red represent the first rotation (pitch) and phi, double prime and blue represent the second rotation (roll).

Figure 1
Euler Rotations in Pitch and Yaw

The coordinate transformation from the resulting rotated body frame to the inertial reference frame, which is defined as north-east-down (NED) frame with the z-axis collinear with the gravity vector:

T = [ M1 * M2 ] T’’

Where:

| cos(theta) 0 sin(theta) |

M1 = | 0 1 0 |

|-sin(theta) 0 cos(theta) |

| 1 0 0 |

M2 = | 0 cos(phi) -sin(phi) |

| 0 sin(phi) cos(phi) |

Multiplying the M1 and M2 and using the following notation for brevity:

c1 = cos(theta)

s1 = sin(theta)

c2 = cos(phi)

s2 = sin(phi)

gives:

| c1 s2s1 c2s1 |

M1 * M2 = | 0 c2 s2 |

| -s1 s2c1 c2c1 |

For our given situation plugging in the known values we have:

| 0 | | c1 s2s1 c2s1 | | ax |

| 0 | = | 0 c2 s2 | * | ay |

| G | | -s1 s2c1 c2c1 | | az |

Where G is the gravity vector and ax,ay, and az are the measured accelerations in the body axis.

From this the roll is readily found as:

c2*ay + s2*az = 0

roll = arctan(ay/az) Eqn 1

To find pitch is a little more difficult. Although it is possible to solve the above equations explicitly for theta (pitch), the last remaining unknown, as a function of (phi,ax,ay,az) a cleaner solution is found by examining the geometry. Pitch is defined as the angle between the vertical gravity vector and the y-z plane. This can be found by dividing the measured magnitude of gravity vector in the y-z plane by the gravity vector.

Tan(q) = sqrt(ay^2 + az^2)/ g

But since g = sqrt(ax^2 + ay^2 +az^2) we have pitch as:

Pitch = arctan{sqrt(ay^2+az^2)/(sqrt(ax^2+ay^2+az^2) } Eqn 2