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.