IMUs contain sensors that measure acceleration, magnetic fields and rotation. This post is about the maths used to get orientation (pitch, roll, yaw) from these sensors.
Getting The IMU Maths Library
I made a maths library for Arduino and it has been used in quite a few cool projects ( such as this and this ). I merged that library into ETK a while ago, and this series of posts will be using ETK as the reference implementation of the maths functions I’m going to write about.
If you want to try writing some code yourself based on these blog posts, you need to download and use ETK. It also comes with heaps of stuff that you may find useful such as PID controllers and a navigation library.
or if you’re using Git, add it as a submodule
git submodule add https://github.com/supercamel/EmbeddedToolKit
Here’s a script that updates submodules to the latest version
git pull origin master --recurse-submodules
git submodule update --rebase --init --remote --recursive
If you’re unfamiliar with vectors, matrices or quaternions, take some time to go through these pages before diving head first into this topic.
Nearly all IMUs use a 3 axis MARG array. This stands for Magnetometer, Accelerometer and Rate Gyroscope. In the past these sensors have been big and expensive, but MEMs technology made it possible to shrink the sensors down so they fit inside a single chip! Some sensors such as the BNO055 have a processor that works out orientation for you (this is great for basic applications but has some serious limitations that will be discussed later).
Magnetometers and accelerometers produce 3D vectors. A vector is great for indicating a direction. Think about gravity. You can feel gravity, so you know which way is up, but gravity alone can’t help you find your orientation. It doesn’t indicated which way you are facing (where’s north??). That’s where the magnetometers come in. By defining a second vector, we’re able to find 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 a 3×3 matrix that contains 3 perpendicular 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
using namespace etk;
Vector<3> down = accelerometers.read();
Vector<3> east = down.cross(magnetometers.read());
Vector<3> north = east.cross(down);
Let’s go through this. The down vector is acceleration. That’s easy enough.
The cross product of down and the magnetometer vector creates the east vector.
The cross product of down and east creates the north vector. This ensure that north is also perpendicular. It corrects for magnetic dip, too.
Each vector is normalized then packed into the rows of a matrix. The Quaterion::fromMatrix function converts the matrix to a quaternion.
The matrix method is interesting because it ‘automatically’ corrects for magnetic dip. An alternative method, however, is to use euler angles to construct q_accel.
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());
Notice the z axis of the magnetometers isn’t used? This method doesn’t account for magnetic dip.
This Ain’t Practical
In practice, accelerometers and magnetometers alone don’t work well for measuring orientation because they pick up lots of signal noise. We only want to measure gravity with accelerometers and every vibration or motion will disrupt that. Likewise, magnetometers pick up stray magnetic fields from motors or electronics. This is where gryoscopes come in handy . . .