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.



You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –


                  • Seen this? Ternions. 


                  • This mockup is missing acceleration control, but it does limit incremental target advancement to a specified rate and NLerps towards to final target along the shortest 'achievable' path. True acceleration limiting -> trapezoidal velocity control would be a cool addition. Maybe as simple as limiting the body rate vector and letting the NLerp readjust...

                •   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 find Python really useful for quickly mocking something up. Adding the visualization took about 5 minutes.  

                    Here's a video for those how don't want to install all the python packages. 

                  • I've toyed with python in the past, but never had a project that drove me to use it.  That may have just changed.  Although I am stuck with C++ for my Arduino based projects, python looks very good for other projects.

                    You have to love eliminating gimbal lock.

                  • 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)


This reply was deleted.


David Hori liked Isabella Domi's profile
gotham liked gotham's profile
Dec 3, 2020