Hi all,

  I have recently started writing a series of blog posts that detail how I have chosen to program my own IMU complimentary filter.  I have done this for my own entertainment and as a learning opportunity.  Hopefully others will find this interesting.

 Here's basics of what this complimentary has to offer:

  1. Quaternion based attitude estimate
  2. Never requires re-orthogonalization unlike DCM or any other 3x3 rotation matrix solution
  3. Almost never requires re-normalization of the attitude estimate Quaternion
  4. Requires no trigonometric functions or floating point division, so it runs fast.  Can process 6dof samples at 500Hz on an Arduino if non-IMU functions are light.  Functions nicely at lower sample rates (~100Hz) if processing time is needed for other operations.
  5. No gimbal lock in any orientation
  6. Fully 3D capable after initialization.  Will properly counter attitude estimate drift and error regardless of the amount of time in any orientation.

The current posts cover the following:

Future posts will build on the previous posts while examining the following:

  • Magnetometer usage and implementation
  • Example code for my 9dof complimentary filter
  • GPS and Baro implementation with simple velocity and position estimation
  • Example code for my 10dof+GPS complimentary filter
  • Aircraft control based on quaternion data
  • Anything else that strikes my interest... or is requested ;-)

  My goals have been to write a filter that runs fast, behaves nicely in any orientation, and can be used to in my own multi-copter control software.  So far I am happy.  This is not intended to be the "best" complimentary filter in any way, just me having fun writing code.  Nothing I have or will present is ground breaking, although I haven't seen any implementations quite like mine.  I hope this is valuable to someone.



Views: 7322

Reply to This

Replies to This Discussion

Hey Phillip, I've been toying with your code for a ACC+GYRO IMU and I'm getting weird results if I inject drift onto the yaw axis... 

I'm just playing in simulation, so what I've done is made a static MPU:

(I'm working in python)

class mpu:
  def __init__(self):
    self.aX = -0.25881904510252076234889883762405 # fwd  
    self.aY = 0 # left
    self.aZ = 0.9659258262890682867497431997289 # up
    self.gX = radians(0.0) # roll
    self.gY = radians(0.0) # pitch
    self.gZ = radians(0.1) # yaw  <- Z drifiting at 0.1 deg/s
    self.samplePeriod = 0.002

  def retrieve(self):

So if I loop through, it moves the estimate towards the 15 degree tilt as expected, but as the yaw accumulates it sloshes between the pitch and roll axes instead of staying on the pitch. This could be a problem with my conversion of the RotationQuat to roll/pitch/yaw values... I notice that that function is missing from your repository:


The attached file is my python port of your library with some test code at the bottom.  


Ah yes - I think it was my rpy conversion (axis ordering). 

Here's a test running with all axes drifiting and the resulting Roll/Pitch is stable. 


And one more tweak... 

Add a GyroDrift variable to zero out the Gyro over time:

GyroDrift = Vector(0.0, 0.0, 0.0)


GyroVec  = Sum(GyroDrift, Vector(MPU.gX, MPU.gY, MPU.gZ))


GyroVec = Sum(GyroVec, correction_Body)
GyroDrift = Sum(Mul(GyroDrift, 0.999), Mul(Sum(GyroDrift, correction_Body), 0.001))


Very cool.

  I forgot to commit the latest version of my 3Dmath header.  Thanks for catching that.  I only added the YPR function for this example and don't actually use it for any of my projects.

  I do have a form of my complimentary filter that incorporates gyro drift correction but I did not include it in this example to avoid making it more complicated than needed.  I would recommend implementing it slightly differently.  You appear to be accumulating the error in the world frame, but if the actual drift is caused by a bias in the sensor itself, then the true gyro drift is actually related to the body frame.  Your solution will result in good performance if the body stays on a straight path or varies slowly.  If the body reverses direction quickly (180deg turn), then the current accumulation of error will be applied incorrectly resulting in a short term disturbance while the error accumulates in the opposite direction until it hits steady state again.



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

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

// accumulate gyro drift in body frame
GyroDrift = Sum(GyroDrift, correction_Body);

// add a small portion of the GyroDrift accumulation to the correction vector
correction_Body = sum(correction_Body, Mul(GyroDrift, 0.01);

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


Effectively this is an PI controller with a P_gain of 1/sample_rate and an I_gain of 0.01/sample_rate.  When programmed like this, initial gyro bias calibration is not really needed.
Thanks for sharing your experience.

I think we're doing roughly the same thing.. I don't see where I'm accumulating in the world frame. I just wanted to keep the GyroDrift variable noise free, so it builds with it's own time constant towards correction_Body. 

Have you seen the Circuit cellar article The freespace IMU?

Basic quat basd IMU the code is availible here:


I had not seen that.  With a 2 second glance that looks very similar.

I will have to take a closer look when I have a minute.  Thanks for posting.

  You're right, I miss-understood the code in your comment and you indeed are not accumulating the error in the world frame.

  I believe you are effectively applying a very strong LPF to the correction vector and that becomes your drift estimate.  I'm partial to my solution as it requires less processor time, but that is probably not a significant consideration for anything that runs python.

 If noise becomes an issue with my I term, I can decrease the gain.  

  Ok,  I took a minute to read through this code and indeed it does use many of the same ideas.

 The biggest functional difference I see is that I "stream" my orientation corrections in with my gryo data so that error correction happens at the same time as gyro integration.  In the freespace code, error corrections are independent of the gyro integration, so it then performs unique operations to correct for error.  This looks computationally expensive to me, but they are only performed when it detects error above a certain threshold, so it may work well for situations where the gyros/accel have little  noise or bias error.

  Some of the equivalent functions used in the freespace imu are significantly more time consuming.  Its function for rotating a vector by quaternion take 1.6x more time than the one I use.  The method used for gyro integration takes roughly 2x more time than my method.  This is probably not a big deal in most situations but I enjoy making mine run fast, so I care.

Thanks again for posting this.

Had some thoughts:

What if you instead of rotating the acc into the earth frame, you instead rotated VERTICAL into the body_frame? This lets you compensate for yaw drift when titled:

if abs(gravity_b_v.x) < cos(radians(15)):
   gyro_drift_b_v.x = (gyro_drift_b_v.x * (1-drift_factor))
                           + ((gyro_drift_b_v.x + c_rate_b_v.x)*drift_factor)

if abs(gravity_b_v.y) < cos(radians(15)):
    gyro_drift_b_v.y = (gyro_drift_b_v.y * (1-drift_factor)) + ((gyro_drift_b_v.y
                            + c_rate_b_v.y)*drift_factor) 

if abs(gravity_b_v.z) < cos(radians(15)):
   gyro_drift_b_v.z = (gyro_drift_b_v.z * (1-drift_factor))
                           + ((gyro_drift_b_v.z + c_rate_b_v.z)*drift_factor)

Also, shouldn't you update the rotation estimate -before- you use it to check the vertical vector? i.e move steps 5&6 between steps 1&2 to keep the estimate in sync with the current MPU samples?

  I have versions of my complimentary filter that do exactly what you describe. They run significantly faster, but you loose the ability to integrate the accel vector for velocity and position estimation (in combination with GPS and baro).  I elected to use this version so that as I add more sensors,  I don't have to change as much in my posts.

  I don't have any problems compensating for yaw drift when tilted.  I think this will be more clear when I finish my post on the 9dof version of this filter.

  At high integration rates I haven't noticed an issue with generating corrections before integrating, but I haven't looked at it closely lately.  Might play with it again when I have time.

I'm not sure if you dabble in python much or not, but I kept going with that port to experiment with a quaternion based position controller... then borrowed some pygame/pyopengl example code to visualize it. 

The controller I put together will let you run angle mode (stabilize) with an arbitrary amount of throw on your roll/pitch sticks - and maintains an earth frame heading. My attached example is doing +/-180 roll and pitch. Bye-Bye gimbal lock!

(edit - fixed up visualization)


Reply to Discussion



Season Two of the Trust Time Trial (T3) Contest 
A list of all T3 contests is here. The current round, the Vertical Horizontal one, is here

© 2019   Created by Chris Anderson.   Powered by

Badges  |  Report an Issue  |  Terms of Service