How to calculate static lean and pitch angles from accelerometer measurements
Description of calculations used to determine lean and pitch angles.
Danique and I have been working on the data acquisition system together and recently have gotten the ST Microelectronics digital 3-axis accelerometer to talk to our Arduino and the Arduino to the computer. When stationary, the accelerometer is measuring the gravitational field, which it senses as an acceleration in the opposite direction of the field. This can be most easily understood by thinking of what a 9.8 m/s^2 acceleration would feel like in a gravity free environment.
The sensor detects x, y, and z body-fixed acceleration measure numbers. The sensor dimensions are 7x7x1.8mm, and has the x, and y axes in the plane, and the z-axis normal to the plane. The measurements are represented by the device as 12 bit, signed (two's complement) integers. The measurement range is selectable to +/-2g or +/-6g. Using the +/-2g range, this results in a measurement resolution of 9.77e-4 g, or about 9.58e-3 m/s^2.
By choosing a set of angle conventions to parameterize the orientation of the sensor (Euler Z-X-Y, for example), we can resolve the gravitational field into the components of the body fixed axes, and thereby determine the lean and pitch angles. This only works when the device is stationary, although for small accelerations, it may still provide a reasonable estimate of the pitch and lean angles.
ax, ay, az: Accelerometer measurements in x, y, z directions of the device, units m/s^2
Nx, Ny, Nz: Inertial unit vectors, Nz = cross(Nx, Ny), Nz aligned with the gravitational field
Cx, Cy, Cz: unit vectors fixed to digital accelerometer, Cz perpendicular to top surface of the device
Assuming the sensor is held stationary, the magnitude of the gravitational field can be calculated as:
|g| = sqrt(ax^2 + ay^2 + az^2)
When we did this, we found the gravitational field in our office to be about 9.77 m/s^2. Holding the device in different orientations (but still stationary) resulted in fluctuations of about +/- 0.15 m/s^2, which is about 15 times greater than the sampling resolution. Ideally, if the device is held stationary in any configuration, one would expect |g| to be constant to within some well specified tolerance that would be a function of how stationary the device is being held, the sampling resolution, and any electrical noise that could be influencing the analog signals before they are sampled. Furthermore, the device characteristics should be such that the orientation shouldn't affect the magnitude of this measurement much -- we will have to explore this to find out for sure.
To compute the lean and pitch angles (the second and third angles in the Euler Z-X-Y angles), we resolve the gravity vector, |g| * Nz, into the three body fixed directions and equate them with the measurements:
dot(Cx, |g|*Nz) = ax
dot(Cy, |g|*Nz) = ay
dot(Cz, |g|*Nz) = az
I won't go into the kinematics needed to compute the dot products, but the result is:
-cos(lean)*sin(pitch)*|g| = ax
sin(lean)*|g| = ay
cos(lean)*cos(pitch)*|g| = az
Only two of these equations are need to solve for the lean and pitch angles, with the lean being the most straight forward:
lean = asin(ay/|g|)
To first order, this approximates to:
lean ~= ay/|g|
The pitch angle can be solved as:
pitch = acos(az / (|g| * cos(asin(ay/|g|))))
pitch = asin(-ax / (|g| * cos(asin(ay/|g|))))
You need to ensure the correct quadrant of the inverse trig functions here, which can be determined by the sign of the argument.
These calculations of the lean and pitch angles can be used for the initial conditions to the algorithm that determines the lean and pitch angles by numerical integration of the rate gyroscope signals.