Fixed: Yaw correction not enabled
Added: Roll compensation
Removed some old unused code