Implemented a much more stable madgewick integration on the orientation