Adds good IMU readings with KF and DMP

No drift and good fusion.