Improving IMU attitude estimates with velocity data

This was last week’s project: Building a Kalman filter-based IMU.

IMUs (inertial measurement units) are clever little devices which try to estimate an object’s absolute orientation (and sometimes its position) by examining the forces on the object.

Hobby drone and computer input IMUs generally look at acceleration data (which informs where “down” is), compass data (which points toward north in 3D space), and rate gyro data (which tells the axis and speed of spin). “Down” and “north” in combination can give a pretty accurate constraint on orientation, but unfortunately if there are any lateral forces (wind; turning), they will get mixed in with “down” and distort the estimate. Kalman filters use matrix math to make good use of the gyro data to correct for this. However, a constantly-accelerating drone could still be fooled about where down is.

I’ve tried here to find out whether we can try to model the drone’s translation and take this into account when estimating the orientation. It turns out that even relatively poor and infrequent data about velocity can constrain acceleration— and thus “down”— quite well. The difference in the quality of the estimate is plainly visible.

This is all done by mathematically simulating a 3D moving object using ordinary dynamics and battering it with Gaussian random forces, and then predicting what data a noisy sensor might return. A predictor algorithm using a Kalman filter (which has no knowledge about the original state) attempts to recover the true state to the best of its ability. The truth is rendered white here, and the estimate in red.

At the end you can see the same algorithm running on actual sensors. The real thing doesn’t use GPS yet, but the prediction is still pretty decent! (There is not that much sustained acceleration to throw it off in this video).

I’ve done some other cool things with the code which perhaps I’ll write up in the future: Among them are a process noise model based on Gaussian random walks, and a nice extension of it to quaternions (which has dramatic impact on the quality of the estimate!) I also make use of automatic differentiation, which makes a linearized extended Kalman filter (EKF) particularly easy and robust to implement.

Share this article:Share on FacebookShare on Google+Tweet about this on TwitterShare on RedditShare on StumbleUponEmail this to someone

6 thoughts on “Improving IMU attitude estimates with velocity data

  1. Oki Matra

    I really really appreciate your work sir! It’s brilliant, neat and easy to follow. It really helps me understand Kalman Filter, the topic in which currently I’m focusing on. Thanks a lot, please keep continuing on your great work!

  2. idr ndr

    Thank you for these grate articles on Kalman filters. I am working on a school project and I am very interested to see the actual code. Did you post it anywhere?

    Thank you.

  3. Troy Bonin


    I just saw you video! Awesome!!!! Could you show me how to build the design i saw in your video?

    i think this is what I need:
    1) accelerometer
    2) Rate gyro
    3) 3D compoass

    or even better, Could I pay you to build me one, and have it shipped to me??? I live in Texas. Thank you for taking time to consider my request.

    Dr Bonin


Leave a Reply

Your email address will not be published. Required fields are marked *