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:
The current posts cover the following:
Future posts will build on the previous posts while examining the following:
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.
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:
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.