Improved performance with real data

Overall most of what I did here was bang my head against a wall trying to make it work. I made a lot of small changes but as you can see with the video still a lot to go. The biggest change I made was originally I used a NED or north east down corresponding to x y z and I switched over for ENU or east north up. I though originally I could just choose at a whim which one to use but different sensors use different reference frames and converting it over is more trouble than its worth. Now the output is a good bit better and I think I can improve it a lot by calibrating the sensor, currently the gyro outputs 3 deg/s when stationary. Next I will also be comparing against a much better kalman filter implementation in python to see how much of it is processing the raw data or the core algorithm.