Commented the hell out of everything. Organized the code nicely. Fixed where code runs so that state updates only happen when the IMU interrupt occurs. Calibration routine much more robust. Added Existing Kalman Filter Code.

Signed-off-by: Ben Schwartz <benschwart@gmail.com>