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.
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)
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
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))
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);
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
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)