Added printouts of raw measurements and Kalman Filtered states for debugging.  Added actual readings and filtering functionality to main loop, and added a utilities file.  Still havent done smart offset correction.  Need to add that and possibly high pass filters