Results 2 issues of Caio Vinicius de Toledo

Implemented the method explained in: [Cubli_IROS2012.pdf](https://github.com/caiotoledo/Cubli_2016/files/712690/Cubli_IROS2012.pdf)

enhancement

Now the Kalman constants (QGyroBias, QAngle and RMeasure) isn't good for MPU-6050.

bug
enhancement