IMU Integration Improvement

[ksiegall]

Comment from Dryw:

IMU integration
Simple integration like this is okay, but only if the robot is expected to rotate about a single axis. That's probably true for most cases, but as soon as pitch and roll come into play, the integrated angle is very likely to be completely wrong. I would really suggest implementing a proper AHRS algorithm, such as Madgwick. There's a python library for this that looks pretty good, I'd suggest looking into using it: https://ahrs.readthedocs.io/en/latest/filters/madgwick.html