Complimentary Filter Example: Quaternion Based IMU for Accel+Gyro sensor

  In this post I am going to post the code for a simple 6 degree of freedom version of my complimentary filter.  This should give anyone who wants to better understand what is going on an opportunity to play with the actual code.  In future posts I will explain how to add a magnetometer and then give the code for a 9dof complimentary filter.

Functional Overview

  I am going to start by giving a visual over view of what this complimentary filter is doing.

Fig 1

  As can be seen above, the basic operation only has a few simple operations.  Here's a list detailing what is happening above:

1. Read gyro and accel data from the sensor (I'm using the MPU-6050).  Note: using my library the readings are automatically converted to rad/s for the gyro and g for the accel.  These units are critical for the function of this filter.
2. Rotate the accel vector from the body frame to the world frame using our current attitude estimate.
3. Take the cross product of the rotated accel vector and perfect vertical.  The result is an estimate of the attitude error.  Note, this will also include effects from non-gravitational acceleration on the body (velocity changes, vibration, etc...).  In the future we will discuss what might be done to reduce the effect these have on our correction vector.  For now we will assume that over long periods of time the accel will generally point in the direction of gravity.
4. Rotate the correction vector back to the body frame using the attitude estimate.
5. Add the rotated correction vector to the gyro reading.  In my post on the accelerometer, I talk about why we can add the correction vector with units that approximate radians to the gyro vector (units rad/s).  Short version: the correction vector gets divided by our sample rate and so we only correct by a small portion of the indicated error (1/400 in this example).  Short term vibration or disturbances have a very small effect on the attitude estimate while long term forces like gravity will consistently pull the estimate in a direction that counters drift and errors.
6. Create a quaternion that approximates the change in attitude indicated by this sensor reading.  You can read in my post on Fast Quaternion Integration how I do this with a minimum of processor time.
7. Combine this new change in attitude quaternion with our previous attitude estimate.  This will give us our new attitude estimate.  When working with quaternions, two rotations can be combine through multiplication.  As with all 3D rotations, order of operations matters as they are not commutative. 

  At this point we can start the loop over again.  In the future I will cover how to use the attitude estimate to help control an aircraft.  My example program outputs Tait-Bryan angles (yaw, pitch and roll) since they are easy for most people to relate to, but there are much, much faster ways to use to our attitude estimate for actual control purposes.

Test Hardware

  To run the following code you will need an Arduino of almost any flavor and an InvenSense MPU-6050 attached using I2C.  I use an Arduino Pro Micro (5v/16Mhz) Chinese clone and a GY-521, both purchased through eBay.  If you're considering ordering a sensor breakout board, getting the GY-86 is a good way to get accel+gyro+mag+baro all on one board.  Here's a picture of my test hardware:

Fig 2 - Hardware

  Ignore the second sensor above (on the left).  It is a magnetometer (HMC-5883L) which we will not be using in this example.  I won't be covering how to connect these as that is covered very well by a simple Google search.  I will provide two tips:

1. You do not need to worry about signal level conversion/translation on the I2C lines.
2. The breakout must either have a voltage regulator to supply the chip with 3.3v from your 5v supply (GY-521 does) or you must supply 3.3v to it (Sparkfun breakout).  If you supply 5v to a non-regulated breakout board, bad things will happen.

Software Libraries

  First, make sure you have Arduino version 1.6.5 or newer.  This example will not work with older versions of the Arduino software.

  All of the unique software libraries needed for this test were written by myself and are located on my GitHub account.  You must download and move them to you Arduino libraries directory. If they are not installed correctly the example will not run (in some cases you may need to restart the Arduino software after installation for the libraries to show up.)  Here is a list of libraries and the direct links:

• MPU6050.h - Library to configure, read and convert sensor data from an InvenSense MPU-6050.
• Math3D.h - Library to perform vector, matrix and quaternion operations.
• PollTimers.h - Simple functions for performing tasks at regular intervals.

Example Code

  The example is located on my GitHub account.  Download it and open the INO file using the Arduino software.  A quick glance at the code in this example should confirm that the list I made in the the first section of this post is almost exactly identical to the code snip below:

// get data from the sensor (STEP 1)

MPU.retrieve();

// move gyro data to vector structure
GyroVec  = Vector(MPU.gX, MPU.gY, MPU.gZ);

// move accel data to vector structure
Accel_Body = Vector(MPU.aX, MPU.aY, MPU.aZ);

// rotate accel from body frame to world frame (STEP 2)
Accel_World = Rotate(RotationQuat, Accel_Body); 

// cross product to determine error (STEP 3)
correction_World = CrossProd(Accel_World, VERTICAL);

// rotate correction vector to body frame (STEP 4)
Vec3 correction_Body = Rotate(correction_World, RotationQuat);

// add correction vector to gyro data (STEP 5)
GyroVec = Sum(GyroVec, correction_Body);

// create incremental rotation quat (STEP 6)
Quat incrementalRotation = Quaternion(GyroVec, MPU.samplePeriod);

// quaternion integration (rotation composting through multiplication) (STEP 7)
RotationQuat = Mul(incrementalRotation, RotationQuat);

  Once you upload the complete example to your Arduino, open the Serial Monitor.  It should look something like the picture below:

Fig 3 - Yaw, Pitch, Roll Output 

  Smoothly moving your sensor should cause the numbers to change in a way that reflects your movements.  Note, the X-axis of the sensor will point towards forward.  Placing the sensor back on a flat surface should cause it to return to something very close to zero in both pitch and roll.  Shaking the sensor rapidly will cause deviations in the attitude estimate due to the extreme vibration, but once you stop and only move the sensor smoothly or let it sit still, the error will quickly be eliminated.  The error in yaw will continue to slowly accumulate without a magnetometer to provide corrections.

Additional Notes

• This solution requires no floating point division or trig functions so it runs fast even with a moderate number of floating point multiplies.  On a 16Mhz Atmel 328P, the entire code above executes in 1700us.  Of that time, 700us is getting the data from the sensor and converting it.  With a sensor that supports SPI or some other fast interface, this portion could be reduced to ~300us or less. 
• Converting from a quaternion to Tait-Bryan angles takes a rather long 610us.  This is why we will cover how to use this quaternion attitude estimate more efficiently in a future post.
• There are no orientations where this method will ever encounter gimbal lock.  
• Re-normalizing of the main quaternion should only need to happen at very infrequent intervals.  For recreational flying devices with flight times under 30min it should never be required.
• Unlike a 3x3 rotation matrix, the quaternion attitude estimate will never have orthogonality issues no matter how long the IMU runs.
• Simple tests can be performed to reject accelerometer readings that are improbable.  This reduces the effect these "bad" readings have on the attitude estimate.  This will be the focus of a future post.
• In a situation where the sensors are giving reasonable data, this method will give results that are comparable to a Kalman filter.  In situations where the sensors are providing marginal data, the Kalman filter will give a better estimate.  The advantage this method has is that it can run on a low power processor like the Atmel 328P where a full 3D Kalman filter implementation cannot do so at any reasonable speed.
• We could save some time by not rotating the accel vector into the world frame, but instead rotating the vertical vector from the world to body frame, and then perform the cross product there.  This example is presented like this as in the future it will be important to have the accel vector in the world frame as we add more functionality.  If one was committed to only ever use a gyro and accel sensors, this arrangement would reduce the time required above to ~1300us opening up the possibility of using a higher sample rate.
• A higher power 32bit micro-controller (like a Teensy 3.1, or anything with a FPU) could be used for a high sample rate IMU (1kHz+).  This would allow for better vibration accommodation and reduced error from non-linearity during dynamic movements.

  In my next post we will discuss how to use a magnetometer to counter yaw drift. Following that I will post on adapting the code presented here to include the magnetometer for a 9 degree of freedom IMU.  Beyond that we will be adding a barometer and GPS while covering velocity and position estimation.

EDIT: Here is the link to my post detailing how a magnetometer can be added to this system.

  Please let me know if you have any questions.

Regards,

Phillip