Calculating magnetic heading using raw accelerometer and magnetometer data

I have an accelerometer and magnetometer each producing raw X, Y and Z readouts. From this I need to determine the magnetic heading of an object.

I'm not that great at trig, but I've put together a formula that does respond pretty well to the rotation of the d开发者_运维问答evice, but also responds to movement that one would not think is relevant, such as angling the device in such a way that has no impact on the direction it is pointed. Such as laying it flat and "rolling" the device.

I think the formula I have for calculating the magnetic heading is fine, but I think my pitch and roll radians for input are wrong.

So I guess the core of my question (unless someone actually has a formula around that does this), is how do you calculate angles, in radians, using an accelerometer for pitch and roll.

Then secondly, any info on the heading formula itself would be great.

Depending on the accuracy your application requires, you may need to solve several problems:

1. Are the accelerometer axes calibrated? I've seen MEMs accelerometers that had axes that were not mutually perpendicular, and had significantly different response characteristics for each axis (typically X and Y would match, and Z would differ). You will need to synthesize ideal XYZ axes from whatever physical reading your device provides. (Google 'accelerometer calibration'.)

2. Are the magnetometer axes calibrated? Similar problem as above, except much harder to check: It is very difficult to generate uniform calibrated magnetic fields. If you use the ambient geomagnetic field, you will need to carefully control the local magnetism of your work environment and your tools. (Google 'magnetometer calibration'.)

3. After the accelerometer and magnetometer have been individually calibrated, their axes need to be calibrated relative to each other. Since both of these devices are typically soldered to a PCB, there is almost guaranteed to be significant misalignment. In many cases, the board layout and device parameters may not even permit the XYZ axes to correspond with each other! This may be the hardest part to do from a lab perspective, so I'd recommend you do a direct comparison using other hardware that has both kinds of sensors and is already calibrated (such as an iPhone or Android phone - but verify the device before use). Normally, this is accomplished by adjusting the prior two calibration matrices until they generate vectors that are correctly aligned relative to each other.

4. Only after you are generating mutually calibrated magnetic and accelerometer vectors can you apply the solutions suggested by the other respondents.

I've only described the static solution, where both the magnetometer and accelerometer are motionless relative to the local gravitational and magnetic fields. If you need to generate responses in real-time while the system is rapidly moving, you will need to account for the time behavior of each sensor. There are two basic ways to do this: 1) Apply time-domain filters to each sensor so that their outputs share a common time domain (generally adding some delay). 2) Use predictive modeling to modify the sensor outputs in real-time (less delay, but more noise).

I've seen Kalman filters used for such applications, but applying them in a vector domain may require using quaternions instead of Euler matrices. Quaternions are easier to use computationally (fewer operations needed compared to matrices), but I found them to be much more difficult to comprehend and get right.

Or, you may choose a completely different path, and use statistics and data fitting to do all the above work in one giant step. Consider the problem as follows: Given 6 input values (XYZ each from uncalibrated magnetometer and accelerometer) and a reference to the device (assuming it is hand-held, and there is an arrow painted on the case), output a single angle representing the magnetic bearing toward which the arrow on the case is pointing, and the elevation of the arrow relative to the gravity vector (tilt of the case).

Using a calibrated reference device (as mentioned above), pair it with the device to be calibrated, and take a several hundred data points, with the device at different orientations. Then use a powerful math package such a Matlab, MathCAD, R or SciPy to setup and solve the nonlinear equations to create the transformation matrices.

I would point to Euler Angles and Roll Pich Yaw.

You're not thinking in enough dimensions. This would be the answer in only 2 dimensions, and it works great if you can find a way to ensure "Z" always aligns with gravity.

int heading=180-atan2(mag_datX, mag_datY)/0.0174532925; // 0/359=N, 90=E, 180=S, 270=W 

(if you're reading directly form the device - beware that it probably returns X, Z, Y - not X, Y, Z !)

However - this is not a 2D compass problem - imagine you take the needle out of the compass, balance it so that gravity plays no part in keeping it "level", and you'll find that "north" will point a bit up or down - depending where on earth you are (or, if at the poles, directly up or down!).

So you need to try and compute the THREE DIMENSIONAL vector from all 3 values - which is a matrix operation.