Determining Orientation from Accelerometer
OUTLINE . introduction . continuous equations . discretization . references
Introduction
An accelerometer is useful for measuring orientation relative to Earth's surface ground ('static acceleration' due to gravitational force) or accelerating motion throughout space ('dynamic acceleration' due to non-gravitational force such as a car speeding up or a human shaking an accelerometer-clad device). However, note that accelerometers are implicity restricted to measuring acceleration, and cannot measure velocity. For example, constant nonzero velocity will produce same measurement as zero velocity.
It's important to first determine if an accelerometer is appropriate for your application. If you're not sure, you may consider other orientation/position sensing devices such as: gyroscope (angular velocity), magnetic compass (2D direction of maximum local magnetic field), magnetometer (magnitude and 3D direction of local magnetic field), GPS (rough 3D position).
In order to achieve higher-level measurements such as 'dead reckoning' (calculation of position and orientation in 3D space), you will need to combine sensor readings. For example, an accelerometer, gyroscope, and magnetometer are commonly packaged together as an 'intertial measurement unit' (or IMU) to be used by 'quadcopter' hobbyists. This 'sensor fusion' also allows for sensors to recalibrate each other such that measurement 'drift' of certain sensors (esp. gyroscope) is minimized. This inter-recalibration can further benefit from probabilistic noise-reducing filters such as the Bayesian or Kalman filter.
The following video may give you a better idea of what an accelerometer [1] can be used for. Extensive description provided at YouTube page (access by clicking on YouTube logo in lower right-hand side of video).
Continuous Equations
It's instructive to read through pages 3-4 of [2] in order to understand how an accelerometer works on a physical level, but not necessary to interpret the analog or digital signals generated from the sensor.
It would be redundant of me to repeat what I learned from [2], so suffice it to say that I used the following two equations (eqn 25, eqn 26) in determining roll and pitch angles. I'll leave you to [2] to understand why this works, and also why an accelerometer cannot calculate yaw angle.
$ G_{x/y/z} = gravitational \ force \ component, \ roll \ angle = \phi, \ pitch \ angle = \theta $
$ \phi = atan(\frac{G_y}{G_z}) $
$ \theta = atan(\frac{-G_x}{ \sqrt{G_y^2 + G_z^2} }) $
Discretization
Because an HCS12 microcontroller works with fixed-point integers, computing the required arctangent and sqrt equations is non-trivial. [3] very clearly explains the situation with fixed-point and floating-point representations in the computer. The sqrt implementation was derived from [4], but discretization of atan is not quite as straightforward. The atan() function you might normally invoke from the <math.h> library requires floating point numbers. (An example implementation can be seen here [5].) However, because this application dealt with an embedded microprocessor, I considered two atan implementations [6]:
- 'Closed Form' (finite number of terms) Approximation, whose range is inherently bounded to [-45 degrees,+45 degrees]:
$ arctan(x) \approx \frac{x}{1 + .28125x^{2}} , -1 \leq x \leq 1 $
- Lookup Table (LUT), in which the arctan argument is used as an index to the LUT [7]
As seen below, the error of each implementation is relatively small. However, because processing time is the primary constraint for my application, the I opted for the LUT implementation. The closed form approximation may seem simple to implement, but the fractional number '.28125' in the denominator requires non-natural placement of the fixed-point decimal in order to calculate. It's possible to do this, but will result in overall 'less clean' code since the programmer will have to bear in mind that the decimal place of some of the variables s/he is working with are different than others. On the other hand, the unsigned char type can be successfully used with a lookup table (note that this also inherently bounds the LUT index input to [0,255], a safe programming practice).
Because the neither accuracy nor range of sensitivity was critical for my application, I implemented a relatively small 256-value LUT in which each arctan argument was multiplied by 255. This means that arctan_LUT[255] = 45 degrees, or in other words, all arctan arguments are normalized and bounded to '1'. However, the LUT could easily be extended up to 90 degrees in both directions by using a larger table or a smaller scaling constant.
To deal with negative arguments, I utilized the property that arctan(-x) = -arctan(x). Explicitly, I set a 'negative flag' if the arctan to be calculated had a negative argument, and fed in the absolute value of the argument to the lookup table, finally appending a 'negative sign' to the table value if appropriate. This effectively doubles the LUT size, and also saves me the hassle of dealing with negative numbers (two's complement). Little tricks like this are useful when dealing with a fixed-point microprocessor, even though they would be trivial and unnecessary for a modern PC's memory and processing resources.
You can get a bit more insight into the LUT I implemented from the following image. Also note how the characteristic of the atan function can be seen in the frequency trend of LUT values.
To measure the worst case time delay from accelerometer signal input to orientation angle output at a macro scale (ie not by counting assembly instruction cycles), I set the conditionals in the code such that the longest logical processing path was taken. I then asserted an output pin from beginning to end of the processing path in order to measure the time on an oscilloscope. The result for a 24MHz bus clock is ~26us (seen below) which means (24M cyc/s * ~26us =) ~624 cycles--a more than acceptable delay for a function that will only be called a few times for second. However, this relatively short delay was only made possible by implementing efficient atan and sqrt functions as outlined above.
A finished implementation (with atan LUT), displaying analog axis outputs and calculated orientation angles (roll, pitch) can be seen here. As before, an explanation of the video can be found on the original Youtube page.
References
[1] Freescale Semiconductor, "±1.5g, ±6g Three Axis Low-g Micromachined Accelerometer" Freescale Datasheet. <http://www.sparkfun.com/datasheets/Components/General/MMA7361L.pdf>
[2] L. Salhuana, "Tilt Sensing Using Linear Accelerometers," Freescale Application Note. <http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf>
[3] S. Smith, "DSP Software," The Scientist and Engineer's Guide to Digital Signal Processing. <http://www.dspguide.com/ch4.htm>
[4] "Fast square root in C," IAR Application Note. <http://netstorage.iar.com/SuppDB/Public/SUPPORT/000419/AN-G-002.pdf>
[5] "atan.c," Apple Open Source. <http://opensource.apple.com/source/Libm/Libm-315/Source/Intel/atan.c>
[6] A. Ukil et al., "Fast Computation of arctangent Functions for Embedded Applications: A Comparative Analysis," IEEE ISIE 2011. <http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=5984330>
[7] N. Jones, "A tutorial on lookup tables in C", EmbeddedGurus.com. <http://embeddedgurus.com/stack-overflow/2010/01/a-tutorial-on-lookup-tables-in-c/>
