[MATLAB MEX] Complementary filter for AHRS


Small, lightweight and cheap Inertial Measurement Units (IMU) made their way into many portable electronic devices. Especially those involving physical movements and rotations. There are plenty of applications starting from toy airplanes to serious Inertial Navigation Systems (INS).

What exactly IMU devices are measuring? What useful information they provide? The IMUs typically comprise 3 devices – accelerometer, gyroscope and magnetometer. All three ┬átypically provide 3 values at time (axis X, Y and Z). These measures are not enough to tell how our plane is oriented. Good starting point about necessary maths can be found [HERE].

Provide exhaustive explanation of orientations and accompanying linear algebra is out of scope of this article. A reader should understand only few things: orientation of our plane is always with respect to some fixed reference (very commonly Earth), orientation of our plane can be understood as transformation from the fixed reference to the current orientation of the local sensor frame. This transformation is usually encoded using a 3×3 rotation matrix or more conveniently using a quaternion.

The Madgwick’s complementary filter algorithm [PDF] for quaternion computation from IMU calibrated data is widely used. Author provides MATLAB and lightweight, 3rd party library-free C code. The latter can be run on an embedded device with limited computing power and memory storage.

For prototyping needs and IMU data analysis it is better to use native MATLAB code. The downside is that for large corpuses of IMU data, the code is very slow (compared to C version). I developed a MEX wrapper function around the existing code with very few modifications to the code (e.g. avoiding approximation of inverse square root).

This is the AHRS version of the Madgwick’s algorithm [WWW] which uses the magnetometer sensor. Therefore it is advised to keep sensor away from large metallic objects and let sensor still for at least few seconds upon start of recording.

The algorithm has a single parameter beta which controls the amount of tradeoff between information accelerometer+gyroscope and magnetometer. This value can change from sensor to sensor. You can set large beta (say 3-8 for first few seconds) and then set it small for runtime (say 0.01-0.1). This is makes the filter converge much faster to its true state (sensor frame orientation with respect to fixed frame of reference). Failing to do so will make filter spit unreliable quaternions.

The source code and a compiled MEX file for 64bit Mac OS X (Mavericks), Matlab R2013a can be downloaded [HERE].

There is *lot* to say about the algorithm and practical side. I encourage reader to investigate this exciting domain of signal processing.

This entry was posted in Uncategorized and tagged , , , , . Bookmark the permalink.

Leave a Reply

Your email address will not be published. Required fields are marked *