Adds good IMU readings with KF and DMP

No drift and good fusion.
Merge pull request #6 from gbledt/master

Adds good IMU readings with KF and DMP. Compass Readings are possible.