IMUs typically contain 9DOF sensors and are used to calculate orientation. Let’s talk about the maths required to get orientation (pitch, roll, yaw) from these sensors.

### Getting The IMU Maths Library

There once was an IMU maths library that I made and it’s been incorporated into quite a few cool projects ( such as this and this ), but I actually merged it into ETK a while ago, aaand . . . it has some other ETK dependancies. Your best option is to just download and use ETK – it comes with heaps of stuff that you’ll probably find useful such as PID controllers and a navigation library. If you don’t need or want to use these ‘etk extras’ they won’t increase the size of your binary or anything like that, so feel free to just ignore them.

or if you’re using Git, add it as a submodule

*git submodule add https://github.com/supercamel/EmbeddedToolKit*

Here’s a bonus script that updates submodules to the latest version

1 2 |
git pull origin master --recurse-submodules git submodule update --rebase --init --remote --recursive |

### Mandatory Reading

If you’re totally unfamiliar with IMU maths, vector maths, matrices or quaternions I very strongly recommend you at least peruse some of these links before diving head first into this topic.

https://www.mathsisfun.com/algebra/vectors.html

http://www.camelsoftware.com/2016/02/20/quaternions-and-how-to-really-use-them/

### Orientation Sensing Basics

Virtually all IMUs will use what’s known as a 3 axis MARG array. This acronym stands for Magnetometer, Accelerometer and Rate Gyroscope. These sensors have historically been big and expensive, but MEMs technology has made it possible to miniaturise all of these sensors down so that they fit inside a single chip! Infact, some sensors such as the BNO055 include a processor that calculates orientation for you (this is great for basic applications but has some serious limitations that will be discussed later).

Magnetometers and accelerometers both produce 3 dimensional vectors. A single vector is great for indicating a direction or for defining a plane (search for *normal vectors*). Think about acceleration due to gravity. It always points up. The acceleration vector defines the ground surface, because gravity is essentially the *normal vector* of the ground plane. Orientation is still unknown though, because although the ground plane is defined there is nothing to indicate which way we’re facing (where’s north??). Obviously that’s where the magnetometers come in. By defining a second vector, we’re able to determine orientation. One vector is good for direction, but two are required for orientation.

We can combine acceleration and magnetic vectors to produce a rotation matrix. A rotation matrix is simply a 3×3 matrix that contains 3 perpendicular 3D vectors. These are north, east and down vectors. Notice that the matrix contains 3 vectors, when it’s just been mentioned that only two are required for orientation? Matrices contain redundant information and occasionally the components of the matrix lose their orthogonal properties, which is one of the down sides to using a matrix. Anyway, here’s how they’re made

1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 |
#include <etk/etk.h> using namespace etk; Vector<3> down = accelerometers.read(); Vector<3> east = down.cross(magnetometers.read()); Vector<3> north = east.cross(down); down.normalize(); east.normalize(); north.normalize(); Matrix<3,3> rotationMatrix; rotationMatrix.vector_to_col(north, 0); rotationMatrix.vector_to_col(east, 1); rotationMatrix.vector_to_col(down, 2); |

Let’s go through this. Creating the down vector is easy; it’s just acceleration. The east vector is created by calculating the cross product of down and the magnetometer reading. Remember the cross product of two vectors is always perpendicular (at right angles) to both. East happens to be perpendicular to down and north, so we’re using the cross product to generate the east vector for the matrix. Next the north vector is created from the cross product of east and down. This cancels out ‘magnetic dip’ and is done to ensure that north is also perpendicular.

Each vector is normalized then packed into the rows of a matrix. The matrix can then be converted to a quaternion using the quaternion fromMatrix function.

1 2 |
etk::Quaternion q_accel; q_accel.fromMatrix(rotationMatrix); |

The matrix method is interesting in that it ‘automatically’ corrects for magnetic dip. An alternative method, however, is to construct q_accel from euler angles.

1 2 3 4 5 6 |
Vector<3> euler; euler.x() = atan2f(mag.y(),mag.x()); euler.y() = atan2(-accel.x(), sqrt(accel.y()*accel.y() + accel.z()*accel.z())); euler.z() = atan2(accel.y(), accel.z()); q_accel.fromEuler(euler); |

Notice the z axis of the magnetometers isn’t used? This particular method doesn’t account for magnetic dip.

In practice, these two sensors (accelerometers in particular) are unusable for steady orientation data because they contain so much signal noise. Every single vibration, motion or acceleration will cause the accelerometers to point in some direction that isn’t perfectly downwards and this is where gyroscopes come in handy.

Hi,

I’m tryin to apply your code in matlab, however, when I calculate cross product for the north vector it is not perpendicular to east vector. I’ve checked all calculations, but I have no ideia, why is this happen.

Do you now why?

Hi Marco,

I don’t/can’t know what the cause might be. I’d suggest providing some very basic inputs (like 0,0,1 as the down vector and 1,0,0 as the north vector). The result should be the identity matrix I think.

If you can get your most basic tests to parse, go from there & good luck.

Cheers, Sam