Determining Roll and Pitch of a Body from the Gravity Vector
Problem: Determine Pitch and Roll from the gravity
vector measured via 3orthogonal accelerometers measured by an Inertial
Measurement Unit.
First constructing the
coordinate transformation matrix for Euler transformations about the yaxis
(pitch) and then about the xaxis (roll).
The conventional first rotation about the inertial zaxis (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 northeastdown (NED) frame with the zaxis
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 yz plane. This can be
found by dividing the measured magnitude of gravity vector in the yz 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