Kalman filter for IMU implemented but not quite working.
Kalman method entirely copied from TKJ Electronics' mbed code. In the process of understanding what it does. Angle that the filter spits out drifts a lot. Possible issues: accelerometer currently only returns integers; kalman function wants microseconds, not milliseconds. Increase covariance matrix constants for more sensitivity in noise estimation.