# Simple IMU - My quaternion based complimentary filter

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.

Regards,

Phillip

Views: 7314

### Replies to This Discussion

Your path 2 is faster (116us vs. 144us for the complete normalization operation) and more accurate for anything that is very near 1.0

I used to have an InvSqrtFast function that was identical to your path2 in my 3D math header.  I will add it back in.  Not really sure why I removed it...

The InvSqrt function in my header is fast compared to a "real" inverse square root if you can't assume the vector is near 1.0, clearly this should never be the case for an orientation quaternion.

For those interested in seeing how I use a magnetometer to counter yaw drift in my complimentary filter, I have posted another blog post talking about it:

Quaternion IMU Drift Compensation: Magnetometer

This method of yaw drift compensation allows for corrections regardless of body orientation.  Some other light weight complimentary filters only provide correction when near level.

I will be releasing a library in the near future that should make it simple for anyone to add this to any project.

1

2

3

4

5

6

7

## Groups

3156 members

1116 members

272 members

294 members

• ### RTKLIB Users

49 members

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